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             :           _sdDistance{sourceToCenterOfRotation + centerOfRotationToDetector},
      18             :           _P{RealMatrix_t::Identity(2, 2 + 1)},
      19             :           _Pinv{RealMatrix_t::Identity(2 + 1, 2)},
      20             :           _ext{RealMatrix_t::Identity(2, 2 + 1)},
      21             :           _K{RealMatrix_t::Identity(2, 2)},
      22             :           _R{RealMatrix_t::Identity(2, 2)},
      23             :           _t{RealVector_t::Zero(2)},
      24             :           _S{RealMatrix_t::Identity(2 + 1, 2 + 1)},
      25             :           _C{RealVector_t::Zero(2)}
      26        8787 :     {
      27        8787 :         auto [volSpacing, volOrigin] = std::move(volData);
      28        8787 :         auto [sinoSpacing, sinoOrigin] = std::move(sinoData);
      29             : 
      30             :         // setup rotation matrix _R
      31        8787 :         real_t c = std::cos(angle);
      32        8787 :         real_t s = std::sin(angle);
      33        8787 :         _R << c, -s, s, c;
      34             : 
      35             :         // setup scaling matrix _S
      36        8787 :         _S << volSpacing[0], 0, 0, 0, volSpacing[1], 0, 0, 0, 1;
      37             : 
      38             :         // set the translation _t
      39             :         // auto centerOfRotationOffset = static_cast<RealVector_t>(centerOfRotOffset);
      40             :         // centerOfRotationOffset << centerOfRotationOffsetX, centerOfRotationOffsetY;
      41             : 
      42        8787 :         _t = _R * (-centerOfRotOffset.get() - volOrigin);
      43        8787 :         _t[_objectDimension - 1] += sourceToCenterOfRotation;
      44             : 
      45             :         // set the intrinsic parameters _K
      46        8787 :         real_t alpha = sinoSpacing[0];
      47        8787 :         _K << (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha,
      48        8787 :             (sinoOrigin[0] / alpha + offset), 0, 1;
      49             : 
      50        8787 :         buildMatrices();
      51        8787 :     }
      52             : 
      53             :     Geometry::Geometry(SourceToCenterOfRotation sourceToCenterOfRotation,
      54             :                        CenterOfRotationToDetector centerOfRotationToDetector,
      55             :                        VolumeData3D&& volData, SinogramData3D&& sinoData, RotationAngles3D angles,
      56             :                        PrincipalPointOffset2D offset, RotationOffset3D centerOfRotOffset)
      57             :         : _objectDimension{3},
      58             :           _sdDistance{sourceToCenterOfRotation + centerOfRotationToDetector},
      59             :           _P{RealMatrix_t::Identity(3, 3 + 1)},
      60             :           _Pinv{RealMatrix_t::Identity(3 + 1, 3)},
      61             :           _ext{RealMatrix_t::Identity(3, 3 + 1)},
      62             :           _K{RealMatrix_t::Identity(3, 3)},
      63             :           _R{RealMatrix_t::Identity(3, 3)},
      64             :           _t{RealVector_t::Zero(3)},
      65             :           _S{RealMatrix_t::Identity(3 + 1, 3 + 1)},
      66             :           _C{RealVector_t::Zero(3)}
      67        6708 :     {
      68        6708 :         auto [volSpacing, volOrigin] = std::move(volData);
      69        6708 :         auto [sinoSpacing, sinoOrigin] = std::move(sinoData);
      70             : 
      71        6708 :         real_t alpha = angles.alpha();
      72        6708 :         real_t beta = angles.beta();
      73        6708 :         real_t gamma = angles.gamma();
      74             : 
      75             :         // setup rotation matrix
      76        6708 :         real_t ca = std::cos(alpha);
      77        6708 :         real_t sa = std::sin(alpha);
      78        6708 :         real_t cb = std::cos(beta);
      79        6708 :         real_t sb = std::sin(beta);
      80        6708 :         real_t cg = std::cos(gamma);
      81        6708 :         real_t sg = std::sin(gamma);
      82             : 
      83             :         // YZY convention
      84        6708 :         _R = (RealMatrix_t(3, 3) << cg, 0, sg, 0, 1, 0, -sg, 0, cg).finished()
      85        6708 :              * (RealMatrix_t(3, 3) << cb, -sb, 0, sb, cb, 0, 0, 0, 1).finished()
      86        6708 :              * (RealMatrix_t(3, 3) << ca, 0, sa, 0, 1, 0, -sa, 0, ca).finished();
      87             : 
      88             :         // setup scaling matrix _S
      89        6708 :         _S << volSpacing[0], 0, 0, 0, 0, volSpacing[1], 0, 0, 0, 0, volSpacing[2], 0, 0, 0, 0, 1;
      90             : 
      91             :         // setup the translation _t
      92             :         // RealVector_t centerOfRotationOffset(_objectDimension);
      93             :         // centerOfRotationOffset << centerOfRotationOffsetX, centerOfRotationOffsetY,
      94             :         //     centerOfRotationOffsetZ;
      95             : 
      96        6708 :         _t = _R * (-centerOfRotOffset.get() - volOrigin);
      97        6708 :         _t(_objectDimension - 1) += sourceToCenterOfRotation;
      98             : 
      99             :         // setup the intrinsic parameters _K
     100        6708 :         real_t alpha1 = sinoSpacing[0];
     101        6708 :         real_t alpha2 = sinoSpacing[1];
     102             : 
     103        6708 :         _K << (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha1, 0,
     104        6708 :             sinoOrigin[0] / alpha1 + offset[0], 0,
     105        6708 :             (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha2,
     106        6708 :             sinoOrigin[1] / alpha2 + offset[1], 0, 0, 1;
     107             : 
     108        6708 :         buildMatrices();
     109        6708 :     }
     110             : 
     111             :     Geometry::Geometry(real_t sourceToCenterOfRotation, real_t centerOfRotationToDetector,
     112             :                        IndexVector_t vol_shape, IndexVector_t det_shape, const RealMatrix_t& R,
     113             :                        RealVector_t vol_spacing, RealVector_t det_spacing, real_t px, real_t py,
     114             :                        real_t centerOfRotationOffsetX, real_t centerOfRotationOffsetY,
     115             :                        real_t centerOfRotationOffsetZ)
     116             :         : _objectDimension{3},
     117             :           _sdDistance{sourceToCenterOfRotation + centerOfRotationToDetector},
     118             :           _P{RealMatrix_t::Identity(3, 3 + 1)},
     119             :           _Pinv{RealMatrix_t::Identity(3 + 1, 3)},
     120             :           _ext{RealMatrix_t::Identity(3, 3 + 1)},
     121             :           _K{RealMatrix_t::Identity(3, 3)},
     122             :           _R{R},
     123             :           _t{RealVector_t::Zero(3)},
     124             :           _S{RealMatrix_t::Identity(3 + 1, 3 + 1)},
     125             :           _C{RealVector_t::Zero(3)}
     126           6 :     {
     127             :         // sanity check
     128           6 :         if (R.rows() != _objectDimension || R.cols() != _objectDimension)
     129           0 :             throw InvalidArgumentError(
     130           0 :                 "Geometry: 3D geometry requested with non-3D rotation matrix");
     131             : 
     132             :         // setup scaling matrix _S
     133           6 :         _S << vol_spacing[0], 0, 0, 0, 0, vol_spacing[1], 0, 0, 0, 0, vol_spacing[2], 0, 0, 0, 0, 1;
     134             : 
     135             :         // setup the translation _t
     136           6 :         RealVector_t centerOfRotationOffset(_objectDimension);
     137           6 :         centerOfRotationOffset << centerOfRotationOffsetX, centerOfRotationOffsetY,
     138           6 :             centerOfRotationOffsetZ;
     139             : 
     140           6 :         auto vol_origin = ::elsa::detail::computeOrigin(vol_shape, vol_spacing);
     141           6 :         _t = _R * (-centerOfRotationOffset - vol_origin);
     142           6 :         _t(_objectDimension - 1) += sourceToCenterOfRotation;
     143             : 
     144             :         // setup the intrinsic parameters _K
     145           6 :         real_t alpha1 = det_spacing[0];
     146           6 :         real_t alpha2 = det_spacing[1];
     147             : 
     148           6 :         auto det_origin = ::elsa::detail::computeOrigin(det_shape, det_spacing);
     149             : 
     150           6 :         _K << (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha1, 0,
     151           6 :             det_origin[0] / alpha1 + px, 0,
     152           6 :             (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha2,
     153           6 :             det_origin[1] / alpha2 + py, 0, 0, 1;
     154             : 
     155           6 :         buildMatrices();
     156           6 :     }
     157             : 
     158             :     Geometry::Geometry(geometry::VolumeData3D&& volData, geometry::SinogramData3D&& sinoData,
     159             :                        const RealMatrix_t& R, const RealMatrix_t& t, const RealMatrix_t& K)
     160             :         : _objectDimension{3},
     161             :           _P{RealMatrix_t::Identity(3, 3 + 1)},
     162             :           _Pinv{RealMatrix_t::Identity(3 + 1, 3)},
     163             :           _ext{RealMatrix_t::Identity(3, 3 + 1)},
     164             :           _K{K},
     165             :           _R{R},
     166             :           _t{t},
     167             :           _S{RealMatrix_t::Identity(3 + 1, 3 + 1)},
     168             :           _C{RealVector_t::Zero(3)}
     169           5 :     {
     170           5 :         auto [volSpacing, volOrigin] = std::move(volData);
     171             : 
     172             :         // reconstruct source detector Distance from intrinsics
     173           5 :         _sdDistance = sinoData.getSpacing()[0] * K(0, 0);
     174             : 
     175           5 :         _S << volSpacing[0], 0, 0, 0, 0, volSpacing[1], 0, 0, 0, 0, volSpacing[2], 0, 0, 0, 0, 1;
     176             : 
     177           5 :         buildMatrices();
     178           5 :     }
     179             : 
     180             :     const RealMatrix_t& Geometry::getProjectionMatrix() const
     181    13472287 :     {
     182    13472287 :         return _P;
     183    13472287 :     }
     184             : 
     185             :     const RealMatrix_t& Geometry::getInverseProjectionMatrix() const
     186      239117 :     {
     187      239117 :         return _Pinv;
     188      239117 :     }
     189             : 
     190             :     const RealVector_t& Geometry::getCameraCenter() const
     191      249017 :     {
     192      249017 :         return _C;
     193      249017 :     }
     194             : 
     195             :     const RealMatrix_t& Geometry::getRotationMatrix() const
     196       39876 :     {
     197       39876 :         return _R;
     198       39876 :     }
     199             : 
     200             :     real_t Geometry::getSourceDetectorDistance() const
     201    13468142 :     {
     202    13468142 :         return _sdDistance;
     203    13468142 :     }
     204             : 
     205             :     const RealMatrix_t& Geometry::getExtrinsicMatrix() const
     206    13467703 :     {
     207    13467703 :         return _ext;
     208    13467703 :     }
     209             : 
     210             :     const RealMatrix_t& Geometry::getIntrinsicMatrix() const
     211           5 :     {
     212           5 :         return _K;
     213           5 :     }
     214             : 
     215             :     const RealVector_t& Geometry::getTranslationVector() const
     216           5 :     {
     217           5 :         return _t;
     218           5 :     }
     219             : 
     220             :     bool Geometry::operator==(const Geometry& other) const
     221        1922 :     {
     222        1922 :         return (_objectDimension == other._objectDimension && _P == other._P && _Pinv == other._Pinv
     223        1922 :                 && _K == other._K && _R == other._R && _t == other._t && _S == other._S
     224        1922 :                 && _C == other._C && _sdDistance == other._sdDistance && _ext == other._ext);
     225        1922 :     }
     226             : 
     227             :     void Geometry::buildMatrices()
     228       15506 :     {
     229       15506 :         RealMatrix_t tmpRt(_objectDimension + 1, _objectDimension + 1);
     230       15506 :         tmpRt.block(0, 0, _objectDimension, _objectDimension) = _R;
     231       15506 :         tmpRt.block(0, _objectDimension, _objectDimension, 1) = _t;
     232       15506 :         tmpRt.block(_objectDimension, 0, 1, _objectDimension).setZero();
     233       15506 :         tmpRt(_objectDimension, _objectDimension) = 1;
     234             : 
     235       15506 :         RealMatrix_t tmpId(_objectDimension, _objectDimension + 1);
     236       15506 :         tmpId.setIdentity();
     237             : 
     238             :         // setup extrinsic matrix
     239       15506 :         auto _extFull = tmpRt * _S;
     240       15506 :         _ext = _extFull.block(0, 0, _objectDimension, _objectDimension + 1);
     241             : 
     242             :         // setup projection matrix _P
     243       15506 :         _P = _K * tmpId * _extFull;
     244             : 
     245             :         // compute the camera center
     246       15506 :         _C = -(_P.block(0, 0, _objectDimension, _objectDimension)
     247       15506 :                    .colPivHouseholderQr()
     248       15506 :                    .solve(_P.block(0, _objectDimension, _objectDimension, 1)));
     249             : 
     250             :         // compute inverse _Pinv of _P via its components
     251       15506 :         RealMatrix_t Sinv =
     252       15506 :             (static_cast<real_t>(1.0) / _S.diagonal().array()).matrix().asDiagonal();
     253             : 
     254       15506 :         RealMatrix_t Kinv = RealMatrix_t::Identity(_objectDimension, _objectDimension);
     255       15506 :         Kinv(0, 0) = static_cast<real_t>(1.0) / _K(0, 0);
     256       15506 :         Kinv(0, _objectDimension - 1) = -_K(0, _objectDimension - 1) / _K(0, 0);
     257       15506 :         if (_objectDimension == 3) {
     258        6719 :             Kinv(1, 1) = static_cast<real_t>(1.0) / _K(1, 1);
     259        6719 :             Kinv(1, _objectDimension - 1) = -_K(1, _objectDimension - 1) / _K(1, 1);
     260        6719 :         }
     261             : 
     262       15506 :         RealMatrix_t Rtinv(_objectDimension + 1, _objectDimension + 1);
     263       15506 :         Rtinv.block(0, 0, _objectDimension, _objectDimension) = _R.transpose();
     264       15506 :         Rtinv.block(0, _objectDimension, _objectDimension, 1) = -_R.transpose() * _t;
     265       15506 :         Rtinv.block(_objectDimension, 0, 1, _objectDimension).setZero();
     266       15506 :         Rtinv(_objectDimension, _objectDimension) = 1;
     267             : 
     268       15506 :         RealMatrix_t tmpIdinv(_objectDimension + 1, _objectDimension);
     269       15506 :         tmpIdinv.setIdentity();
     270             : 
     271       15506 :         _Pinv = Sinv * Rtinv * tmpIdinv * Kinv;
     272       15506 :     }
     273             :     index_t Geometry::getDimension() const
     274           0 :     {
     275           0 :         return _objectDimension;
     276           0 :     }
     277             : 
     278             : } // namespace elsa