Line data    Source code

       1             : #include "Geometry.h"
       2             : 
       3             : #include <cmath>
       4             : #include <stdexcept>
       5             : 
       6             : #include <Eigen/Dense>
       7             : 
       8             : namespace elsa
       9             : {
      10             :     using namespace geometry;
      11             : 
      12             :     Geometry::Geometry(SourceToCenterOfRotation sourceToCenterOfRotation,
      13             :                        CenterOfRotationToDetector centerOfRotationToDetector, Radian angle,
      14             :                        VolumeData2D&& volData, SinogramData2D&& sinoData,
      15             :                        PrincipalPointOffset offset, RotationOffset2D centerOfRotOffset)
      16             :         : _objectDimension{2},
      17             :           _P{RealMatrix_t::Identity(2, 2 + 1)},
      18             :           _Pinv{RealMatrix_t::Identity(2 + 1, 2)},
      19             :           _K{RealMatrix_t::Identity(2, 2)},
      20             :           _R{RealMatrix_t::Identity(2, 2)},
      21             :           _t{RealVector_t::Zero(2)},
      22             :           _S{RealMatrix_t::Identity(2 + 1, 2 + 1)},
      23             :           _C{RealVector_t::Zero(2)}
      24        4960 :     {
      25        4960 :         auto [volSpacing, volOrigin] = std::move(volData);
      26        4960 :         auto [sinoSpacing, sinoOrigin] = std::move(sinoData);
      27             : 
      28             :         // setup rotation matrix _R
      29        4960 :         real_t c = std::cos(angle);
      30        4960 :         real_t s = std::sin(angle);
      31        4960 :         _R << c, -s, s, c;
      32             : 
      33             :         // setup scaling matrix _S
      34        4960 :         _S << volSpacing[0], 0, 0, 0, volSpacing[1], 0, 0, 0, 1;
      35             : 
      36             :         // set the translation _t
      37             :         // auto centerOfRotationOffset = static_cast<RealVector_t>(centerOfRotOffset);
      38             :         // centerOfRotationOffset << centerOfRotationOffsetX, centerOfRotationOffsetY;
      39             : 
      40        4960 :         _t = _R * (-centerOfRotOffset.get() - volOrigin);
      41        4960 :         _t[_objectDimension - 1] += sourceToCenterOfRotation;
      42             : 
      43             :         // set the intrinsic parameters _K
      44        4960 :         real_t alpha = sinoSpacing[0];
      45        4960 :         _K << (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha,
      46        4960 :             (sinoOrigin[0] / alpha + offset), 0, 1;
      47             : 
      48        4960 :         buildMatrices();
      49        4960 :     }
      50             : 
      51             :     Geometry::Geometry(SourceToCenterOfRotation sourceToCenterOfRotation,
      52             :                        CenterOfRotationToDetector centerOfRotationToDetector,
      53             :                        VolumeData3D&& volData, SinogramData3D&& sinoData, RotationAngles3D angles,
      54             :                        PrincipalPointOffset2D offset, RotationOffset3D centerOfRotOffset)
      55             :         : _objectDimension{3},
      56             :           _P{RealMatrix_t::Identity(3, 3 + 1)},
      57             :           _Pinv{RealMatrix_t::Identity(3 + 1, 3)},
      58             :           _K{RealMatrix_t::Identity(3, 3)},
      59             :           _R{RealMatrix_t::Identity(3, 3)},
      60             :           _t{RealVector_t::Zero(3)},
      61             :           _S{RealMatrix_t::Identity(3 + 1, 3 + 1)},
      62             :           _C{RealVector_t::Zero(3)}
      63        1720 :     {
      64        1720 :         auto [volSpacing, volOrigin] = std::move(volData);
      65        1720 :         auto [sinoSpacing, sinoOrigin] = std::move(sinoData);
      66             : 
      67        1720 :         real_t alpha = angles.alpha();
      68        1720 :         real_t beta = angles.beta();
      69        1720 :         real_t gamma = angles.gamma();
      70             : 
      71             :         // setup rotation matrix
      72        1720 :         real_t ca = std::cos(alpha);
      73        1720 :         real_t sa = std::sin(alpha);
      74        1720 :         real_t cb = std::cos(beta);
      75        1720 :         real_t sb = std::sin(beta);
      76        1720 :         real_t cg = std::cos(gamma);
      77        1720 :         real_t sg = std::sin(gamma);
      78             : 
      79             :         // YZY convention
      80        1720 :         _R = (RealMatrix_t(3, 3) << cg, 0, sg, 0, 1, 0, -sg, 0, cg).finished()
      81        1720 :              * (RealMatrix_t(3, 3) << cb, -sb, 0, sb, cb, 0, 0, 0, 1).finished()
      82        1720 :              * (RealMatrix_t(3, 3) << ca, 0, sa, 0, 1, 0, -sa, 0, ca).finished();
      83             : 
      84             :         // setup scaling matrix _S
      85        1720 :         _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             :         // RealVector_t centerOfRotationOffset(_objectDimension);
      89             :         // centerOfRotationOffset << centerOfRotationOffsetX, centerOfRotationOffsetY,
      90             :         //     centerOfRotationOffsetZ;
      91             : 
      92        1720 :         _t = _R * (-centerOfRotOffset.get() - volOrigin);
      93        1720 :         _t(_objectDimension - 1) += sourceToCenterOfRotation;
      94             : 
      95             :         // setup the intrinsic parameters _K
      96        1720 :         real_t alpha1 = sinoSpacing[0];
      97        1720 :         real_t alpha2 = sinoSpacing[1];
      98             : 
      99        1720 :         _K << (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha1, 0,
     100        1720 :             sinoOrigin[0] / alpha1 + offset[0], 0,
     101        1720 :             (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha2,
     102        1720 :             sinoOrigin[1] / alpha2 + offset[1], 0, 0, 1;
     103             : 
     104        1720 :         buildMatrices();
     105        1720 :     }
     106             : 
     107             :     Geometry::Geometry(real_t sourceToCenterOfRotation, real_t centerOfRotationToDetector,
     108             :                        const DataDescriptor& volumeDescriptor, const DataDescriptor& sinoDescriptor,
     109             :                        const RealMatrix_t& R, real_t px, real_t py, real_t centerOfRotationOffsetX,
     110             :                        real_t centerOfRotationOffsetY, real_t centerOfRotationOffsetZ)
     111             :         : _objectDimension{3},
     112             :           _P{RealMatrix_t::Identity(3, 3 + 1)},
     113             :           _Pinv{RealMatrix_t::Identity(3 + 1, 3)},
     114             :           _K{RealMatrix_t::Identity(3, 3)},
     115             :           _R{R},
     116             :           _t{RealVector_t::Zero(3)},
     117             :           _S{RealMatrix_t::Identity(3 + 1, 3 + 1)},
     118             :           _C{RealVector_t::Zero(3)}
     119           4 :     {
     120             :         // sanity check
     121           4 :         if (R.rows() != _objectDimension || R.cols() != _objectDimension)
     122           0 :             throw InvalidArgumentError(
     123           0 :                 "Geometry: 3D geometry requested with non-3D rotation matrix");
     124             : 
     125             :         // setup scaling matrix _S
     126           4 :         _S << volumeDescriptor.getSpacingPerDimension()[0], 0, 0, 0, 0,
     127           4 :             volumeDescriptor.getSpacingPerDimension()[1], 0, 0, 0, 0,
     128           4 :             volumeDescriptor.getSpacingPerDimension()[2], 0, 0, 0, 0, 1;
     129             : 
     130             :         // setup the translation _t
     131           4 :         RealVector_t centerOfRotationOffset(_objectDimension);
     132           4 :         centerOfRotationOffset << centerOfRotationOffsetX, centerOfRotationOffsetY,
     133           4 :             centerOfRotationOffsetZ;
     134             : 
     135           4 :         _t = _R * (-centerOfRotationOffset - volumeDescriptor.getLocationOfOrigin());
     136           4 :         _t(_objectDimension - 1) += sourceToCenterOfRotation;
     137             : 
     138             :         // setup the intrinsic parameters _K
     139           4 :         real_t alpha1 = sinoDescriptor.getSpacingPerDimension()[0];
     140           4 :         real_t alpha2 = sinoDescriptor.getSpacingPerDimension()[1];
     141             : 
     142           4 :         _K << (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha1, 0,
     143           4 :             sinoDescriptor.getLocationOfOrigin()[0] / alpha1 + px, 0,
     144           4 :             (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha2,
     145           4 :             sinoDescriptor.getLocationOfOrigin()[1] / alpha2 + py, 0, 0, 1;
     146             : 
     147           4 :         buildMatrices();
     148           4 :     }
     149             : 
     150        4626 :     const RealMatrix_t& Geometry::getProjectionMatrix() const { return _P; }
     151             : 
     152      309291 :     const RealMatrix_t& Geometry::getInverseProjectionMatrix() const { return _Pinv; }
     153             : 
     154      304936 :     const RealVector_t& Geometry::getCameraCenter() const { return _C; }
     155             : 
     156        4072 :     const RealMatrix_t& Geometry::getRotationMatrix() const { return _R; }
     157             : 
     158             :     bool Geometry::operator==(const Geometry& other) const
     159          93 :     {
     160          93 :         return (_objectDimension == other._objectDimension && _P == other._P && _Pinv == other._Pinv
     161          93 :                 && _K == other._K && _R == other._R && _t == other._t && _S == other._S
     162          93 :                 && _C == other._C);
     163          93 :     }
     164             : 
     165             :     void Geometry::buildMatrices()
     166        6684 :     {
     167        6684 :         RealMatrix_t tmpRt(_objectDimension + 1, _objectDimension + 1);
     168        6684 :         tmpRt.block(0, 0, _objectDimension, _objectDimension) = _R;
     169        6684 :         tmpRt.block(0, _objectDimension, _objectDimension, 1) = _t;
     170        6684 :         tmpRt.block(_objectDimension, 0, 1, _objectDimension).setZero();
     171        6684 :         tmpRt(_objectDimension, _objectDimension) = 1;
     172             : 
     173        6684 :         RealMatrix_t tmpId(_objectDimension, _objectDimension + 1);
     174        6684 :         tmpId.setIdentity();
     175             : 
     176             :         // setup projection matrix _P
     177        6684 :         _P = _K * tmpId * tmpRt * _S;
     178             : 
     179             :         // compute the camera center
     180        6684 :         _C = -(_P.block(0, 0, _objectDimension, _objectDimension)
     181        6684 :                    .colPivHouseholderQr()
     182        6684 :                    .solve(_P.block(0, _objectDimension, _objectDimension, 1)));
     183             : 
     184             :         // compute inverse _Pinv of _P via its components
     185        6684 :         RealMatrix_t Sinv =
     186        6684 :             (static_cast<real_t>(1.0) / _S.diagonal().array()).matrix().asDiagonal();
     187             : 
     188        6684 :         RealMatrix_t Kinv = RealMatrix_t::Identity(_objectDimension, _objectDimension);
     189        6684 :         Kinv(0, 0) = static_cast<real_t>(1.0) / _K(0, 0);
     190        6684 :         Kinv(0, _objectDimension - 1) = -_K(0, _objectDimension - 1) / _K(0, 0);
     191        6684 :         if (_objectDimension == 3) {
     192        1724 :             Kinv(1, 1) = static_cast<real_t>(1.0) / _K(1, 1);
     193        1724 :             Kinv(1, _objectDimension - 1) = -_K(1, _objectDimension - 1) / _K(1, 1);
     194        1724 :         }
     195             : 
     196        6684 :         RealMatrix_t Rtinv(_objectDimension + 1, _objectDimension + 1);
     197        6684 :         Rtinv.block(0, 0, _objectDimension, _objectDimension) = _R.transpose();
     198        6684 :         Rtinv.block(0, _objectDimension, _objectDimension, 1) = -_R.transpose() * _t;
     199        6684 :         Rtinv.block(_objectDimension, 0, 1, _objectDimension).setZero();
     200        6684 :         Rtinv(_objectDimension, _objectDimension) = 1;
     201             : 
     202        6684 :         RealMatrix_t tmpIdinv(_objectDimension + 1, _objectDimension);
     203        6684 :         tmpIdinv.setIdentity();
     204             : 
     205        6684 :         _Pinv = Sinv * Rtinv * tmpIdinv * Kinv;
     206        6684 :     }
     207             : 
     208             : } // namespace elsa