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