Geometry.cpp 8.5 KB
Newer Older
1 2 3 4 5 6 7 8 9
#include "Geometry.h"

#include <cmath>
#include <stdexcept>

#include <Eigen/Dense>

namespace elsa
{
10 11 12 13 14 15
    using namespace geometry;

    Geometry::Geometry(SourceToCenterOfRotation sourceToCenterOfRotation,
                       CenterOfRotationToDetector centerOfRotationToDetector, Radian angle,
                       VolumeData2D&& volData, SinogramData2D&& sinoData,
                       PrincipalPointOffset offset, RotationOffset2D centerOfRotOffset)
Jens Petit's avatar
Jens Petit committed
16 17 18 19 20 21 22 23
        : _objectDimension{2},
          _P{RealMatrix_t::Identity(2, 2 + 1)},
          _Pinv{RealMatrix_t::Identity(2 + 1, 2)},
          _K{RealMatrix_t::Identity(2, 2)},
          _R{RealMatrix_t::Identity(2, 2)},
          _t{RealVector_t::Zero(2)},
          _S{RealMatrix_t::Identity(2 + 1, 2 + 1)},
          _C{RealVector_t::Zero(2)}
24
    {
25 26 27
        auto [volSpacing, volOrigin] = std::move(volData);
        auto [sinoSpacing, sinoOrigin] = std::move(sinoData);

28 29 30 31 32 33
        // setup rotation matrix _R
        real_t c = std::cos(angle);
        real_t s = std::sin(angle);
        _R << c, -s, s, c;

        // setup scaling matrix _S
34
        _S << volSpacing[0], 0, 0, 0, volSpacing[1], 0, 0, 0, 1;
35 36

        // set the translation _t
37 38
        // auto centerOfRotationOffset = static_cast<RealVector_t>(centerOfRotOffset);
        // centerOfRotationOffset << centerOfRotationOffsetX, centerOfRotationOffsetY;
39

40
        _t = _R * (-centerOfRotOffset.get() - volOrigin);
41 42 43
        _t[_objectDimension - 1] += sourceToCenterOfRotation;

        // set the intrinsic parameters _K
44
        real_t alpha = sinoSpacing[0];
45
        _K << (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha,
46
            (sinoOrigin[0] / alpha + offset), 0, 1;
47 48 49 50

        buildMatrices();
    }

51 52 53 54
    Geometry::Geometry(SourceToCenterOfRotation sourceToCenterOfRotation,
                       CenterOfRotationToDetector centerOfRotationToDetector,
                       VolumeData3D&& volData, SinogramData3D&& sinoData, RotationAngles3D angles,
                       PrincipalPointOffset2D offset, RotationOffset3D centerOfRotOffset)
Jens Petit's avatar
Jens Petit committed
55 56 57 58 59 60 61 62
        : _objectDimension{3},
          _P{RealMatrix_t::Identity(3, 3 + 1)},
          _Pinv{RealMatrix_t::Identity(3 + 1, 3)},
          _K{RealMatrix_t::Identity(3, 3)},
          _R{RealMatrix_t::Identity(3, 3)},
          _t{RealVector_t::Zero(3)},
          _S{RealMatrix_t::Identity(3 + 1, 3 + 1)},
          _C{RealVector_t::Zero(3)}
63
    {
64 65 66 67 68 69 70
        auto [volSpacing, volOrigin] = std::move(volData);
        auto [sinoSpacing, sinoOrigin] = std::move(sinoData);

        real_t alpha = angles.alpha();
        real_t beta = angles.beta();
        real_t gamma = angles.gamma();

71 72 73 74 75 76 77 78 79
        // setup rotation matrix
        real_t ca = std::cos(alpha);
        real_t sa = std::sin(alpha);
        real_t cb = std::cos(beta);
        real_t sb = std::sin(beta);
        real_t cg = std::cos(gamma);
        real_t sg = std::sin(gamma);

        // YZY convention
Jens Petit's avatar
Jens Petit committed
80 81 82
        _R = (RealMatrix_t(3, 3) << cg, 0, sg, 0, 1, 0, -sg, 0, cg).finished()
             * (RealMatrix_t(3, 3) << cb, -sb, 0, sb, cb, 0, 0, 0, 1).finished()
             * (RealMatrix_t(3, 3) << ca, 0, sa, 0, 1, 0, -sa, 0, ca).finished();
83 84

        // setup scaling matrix _S
85
        _S << volSpacing[0], 0, 0, 0, 0, volSpacing[1], 0, 0, 0, 0, volSpacing[2], 0, 0, 0, 0, 1;
86 87

        // setup the translation _t
88 89 90
        // RealVector_t centerOfRotationOffset(_objectDimension);
        // centerOfRotationOffset << centerOfRotationOffsetX, centerOfRotationOffsetY,
        //     centerOfRotationOffsetZ;
91

92
        _t = _R * (-centerOfRotOffset.get() - volOrigin);
93 94 95
        _t(_objectDimension - 1) += sourceToCenterOfRotation;

        // setup the intrinsic parameters _K
96 97
        real_t alpha1 = sinoSpacing[0];
        real_t alpha2 = sinoSpacing[1];
98

Jens Petit's avatar
Jens Petit committed
99
        _K << (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha1, 0,
100
            sinoOrigin[0] / alpha1 + offset[0], 0,
Jens Petit's avatar
Jens Petit committed
101
            (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha2,
102
            sinoOrigin[1] / alpha2 + offset[1], 0, 0, 1;
103 104 105 106 107 108

        buildMatrices();
    }

    Geometry::Geometry(real_t sourceToCenterOfRotation, real_t centerOfRotationToDetector,
                       const DataDescriptor& volumeDescriptor, const DataDescriptor& sinoDescriptor,
Jens Petit's avatar
Jens Petit committed
109 110 111 112 113 114 115 116 117 118
                       const RealMatrix_t& R, real_t px, real_t py, real_t centerOfRotationOffsetX,
                       real_t centerOfRotationOffsetY, real_t centerOfRotationOffsetZ)
        : _objectDimension{3},
          _P{RealMatrix_t::Identity(3, 3 + 1)},
          _Pinv{RealMatrix_t::Identity(3 + 1, 3)},
          _K{RealMatrix_t::Identity(3, 3)},
          _R{R},
          _t{RealVector_t::Zero(3)},
          _S{RealMatrix_t::Identity(3 + 1, 3 + 1)},
          _C{RealVector_t::Zero(3)}
119 120 121
    {
        // sanity check
        if (R.rows() != _objectDimension || R.cols() != _objectDimension)
Jens Petit's avatar
Jens Petit committed
122 123
            throw std::invalid_argument(
                "Geometry: 3D geometry requested with non-3D rotation matrix");
124 125

        // setup scaling matrix _S
Jens Petit's avatar
Jens Petit committed
126 127 128
        _S << volumeDescriptor.getSpacingPerDimension()[0], 0, 0, 0, 0,
            volumeDescriptor.getSpacingPerDimension()[1], 0, 0, 0, 0,
            volumeDescriptor.getSpacingPerDimension()[2], 0, 0, 0, 0, 1;
129 130 131

        // setup the translation _t
        RealVector_t centerOfRotationOffset(_objectDimension);
Jens Petit's avatar
Jens Petit committed
132 133
        centerOfRotationOffset << centerOfRotationOffsetX, centerOfRotationOffsetY,
            centerOfRotationOffsetZ;
134 135 136 137 138 139 140 141

        _t = _R * (-centerOfRotationOffset - volumeDescriptor.getLocationOfOrigin());
        _t(_objectDimension - 1) += sourceToCenterOfRotation;

        // setup the intrinsic parameters _K
        real_t alpha1 = sinoDescriptor.getSpacingPerDimension()[0];
        real_t alpha2 = sinoDescriptor.getSpacingPerDimension()[1];

Jens Petit's avatar
Jens Petit committed
142 143 144 145
        _K << (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha1, 0,
            sinoDescriptor.getLocationOfOrigin()[0] / alpha1 + px, 0,
            (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha2,
            sinoDescriptor.getLocationOfOrigin()[1] / alpha2 + py, 0, 0, 1;
146 147 148 149

        buildMatrices();
    }

Jens Petit's avatar
Jens Petit committed
150
    const RealMatrix_t& Geometry::getProjectionMatrix() const { return _P; }
151

Jens Petit's avatar
Jens Petit committed
152
    const RealMatrix_t& Geometry::getInverseProjectionMatrix() const { return _Pinv; }
153

Jens Petit's avatar
Jens Petit committed
154
    const RealVector_t& Geometry::getCameraCenter() const { return _C; }
155

Jens Petit's avatar
Jens Petit committed
156
    const RealMatrix_t& Geometry::getRotationMatrix() const { return _R; }
157 158 159

    bool Geometry::operator==(const Geometry& other) const
    {
Jens Petit's avatar
Jens Petit committed
160 161 162
        return (_objectDimension == other._objectDimension && _P == other._P && _Pinv == other._Pinv
                && _K == other._K && _R == other._R && _t == other._t && _S == other._S
                && _C == other._C);
163 164 165 166
    }

    void Geometry::buildMatrices()
    {
Jens Petit's avatar
Jens Petit committed
167
        RealMatrix_t tmpRt(_objectDimension + 1, _objectDimension + 1);
168 169 170 171 172
        tmpRt.block(0, 0, _objectDimension, _objectDimension) = _R;
        tmpRt.block(0, _objectDimension, _objectDimension, 1) = _t;
        tmpRt.block(_objectDimension, 0, 1, _objectDimension).setZero();
        tmpRt(_objectDimension, _objectDimension) = 1;

Jens Petit's avatar
Jens Petit committed
173
        RealMatrix_t tmpId(_objectDimension, _objectDimension + 1);
174 175 176 177 178 179
        tmpId.setIdentity();

        // setup projection matrix _P
        _P = _K * tmpId * tmpRt * _S;

        // compute the camera center
Jens Petit's avatar
Jens Petit committed
180 181 182
        _C = -(_P.block(0, 0, _objectDimension, _objectDimension)
                   .colPivHouseholderQr()
                   .solve(_P.block(0, _objectDimension, _objectDimension, 1)));
183 184

        // compute inverse _Pinv of _P via its components
Jens Petit's avatar
Jens Petit committed
185 186
        RealMatrix_t Sinv =
            (static_cast<real_t>(1.0) / _S.diagonal().array()).matrix().asDiagonal();
187 188 189

        RealMatrix_t Kinv = RealMatrix_t::Identity(_objectDimension, _objectDimension);
        Kinv(0, 0) = static_cast<real_t>(1.0) / _K(0, 0);
Jens Petit's avatar
Jens Petit committed
190
        Kinv(0, _objectDimension - 1) = -_K(0, _objectDimension - 1) / _K(0, 0);
191 192
        if (_objectDimension == 3) {
            Kinv(1, 1) = static_cast<real_t>(1.0) / _K(1, 1);
Jens Petit's avatar
Jens Petit committed
193
            Kinv(1, _objectDimension - 1) = -_K(1, _objectDimension - 1) / _K(1, 1);
194 195
        }

Jens Petit's avatar
Jens Petit committed
196
        RealMatrix_t Rtinv(_objectDimension + 1, _objectDimension + 1);
197 198 199 200 201
        Rtinv.block(0, 0, _objectDimension, _objectDimension) = _R.transpose();
        Rtinv.block(0, _objectDimension, _objectDimension, 1) = -_R.transpose() * _t;
        Rtinv.block(_objectDimension, 0, 1, _objectDimension).setZero();
        Rtinv(_objectDimension, _objectDimension) = 1;

Jens Petit's avatar
Jens Petit committed
202
        RealMatrix_t tmpIdinv(_objectDimension + 1, _objectDimension);
203 204 205 206 207 208
        tmpIdinv.setIdentity();

        _Pinv = Sinv * Rtinv * tmpIdinv * Kinv;
    }

} // namespace elsa