In January 2021 we will introduce a 10 GB quota for project repositories. Higher limits for individual projects will be available on request. Please see https://doku.lrz.de/display/PUBLIC/GitLab for more information.

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

#include <cmath>
#include <stdexcept>

#include <Eigen/Dense>

namespace elsa
{
Jens Petit's avatar
Jens Petit committed
10 11 12 13 14 15 16 17 18 19 20 21
    Geometry::Geometry(real_t sourceToCenterOfRotation, real_t centerOfRotationToDetector,
                       real_t angle, const DataDescriptor& volumeDescriptor,
                       const DataDescriptor& sinoDescriptor, real_t offset,
                       real_t centerOfRotationOffsetX, real_t centerOfRotationOffsetY)
        : _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)}
22 23 24 25 26 27 28
    {
        // 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
Jens Petit's avatar
Jens Petit committed
29 30
        _S << volumeDescriptor.getSpacingPerDimension()[0], 0, 0, 0,
            volumeDescriptor.getSpacingPerDimension()[1], 0, 0, 0, 1;
31 32 33 34 35 36 37 38 39 40 41

        // set the translation _t
        RealVector_t centerOfRotationOffset(_objectDimension);
        centerOfRotationOffset << centerOfRotationOffsetX, centerOfRotationOffsetY;

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

        // set the intrinsic parameters _K
        real_t alpha = sinoDescriptor.getSpacingPerDimension()[0];
        _K << (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha,
Jens Petit's avatar
Jens Petit committed
42
            (sinoDescriptor.getLocationOfOrigin()[0] / alpha + offset), 0, 1;
43 44 45 46 47 48 49

        buildMatrices();
    }

    Geometry::Geometry(real_t sourceToCenterOfRotation, real_t centerOfRotationToDetector,
                       const DataDescriptor& volumeDescriptor, const DataDescriptor& sinoDescriptor,
                       real_t gamma, real_t beta, real_t alpha, real_t px, real_t py,
Jens Petit's avatar
Jens Petit committed
50 51 52 53 54 55 56 57 58 59
                       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{RealMatrix_t::Identity(3, 3)},
          _t{RealVector_t::Zero(3)},
          _S{RealMatrix_t::Identity(3 + 1, 3 + 1)},
          _C{RealVector_t::Zero(3)}
60 61 62 63 64 65 66 67 68 69
    {
        // 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
70 71 72
        _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();
73 74

        // setup scaling matrix _S
Jens Petit's avatar
Jens Petit committed
75 76 77
        _S << volumeDescriptor.getSpacingPerDimension()[0], 0, 0, 0, 0,
            volumeDescriptor.getSpacingPerDimension()[1], 0, 0, 0, 0,
            volumeDescriptor.getSpacingPerDimension()[2], 0, 0, 0, 0, 1;
78 79 80

        // setup the translation _t
        RealVector_t centerOfRotationOffset(_objectDimension);
Jens Petit's avatar
Jens Petit committed
81 82
        centerOfRotationOffset << centerOfRotationOffsetX, centerOfRotationOffsetY,
            centerOfRotationOffsetZ;
83 84 85 86 87 88 89 90

        _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
91 92 93 94
        _K << (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha1, 0,
            sinoDescriptor.getLocationOfOrigin()[0] / alpha1 + px, 0,
            (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha2,
            sinoDescriptor.getLocationOfOrigin()[1] / alpha2 + py, 0, 0, 1;
95 96 97 98 99 100

        buildMatrices();
    }

    Geometry::Geometry(real_t sourceToCenterOfRotation, real_t centerOfRotationToDetector,
                       const DataDescriptor& volumeDescriptor, const DataDescriptor& sinoDescriptor,
Jens Petit's avatar
Jens Petit committed
101 102 103 104 105 106 107 108 109 110
                       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)}
111 112 113
    {
        // sanity check
        if (R.rows() != _objectDimension || R.cols() != _objectDimension)
Jens Petit's avatar
Jens Petit committed
114 115
            throw std::invalid_argument(
                "Geometry: 3D geometry requested with non-3D rotation matrix");
116 117

        // setup scaling matrix _S
Jens Petit's avatar
Jens Petit committed
118 119 120
        _S << volumeDescriptor.getSpacingPerDimension()[0], 0, 0, 0, 0,
            volumeDescriptor.getSpacingPerDimension()[1], 0, 0, 0, 0,
            volumeDescriptor.getSpacingPerDimension()[2], 0, 0, 0, 0, 1;
121 122 123

        // setup the translation _t
        RealVector_t centerOfRotationOffset(_objectDimension);
Jens Petit's avatar
Jens Petit committed
124 125
        centerOfRotationOffset << centerOfRotationOffsetX, centerOfRotationOffsetY,
            centerOfRotationOffsetZ;
126 127 128 129 130 131 132 133

        _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
134 135 136 137
        _K << (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha1, 0,
            sinoDescriptor.getLocationOfOrigin()[0] / alpha1 + px, 0,
            (sourceToCenterOfRotation + centerOfRotationToDetector) / alpha2,
            sinoDescriptor.getLocationOfOrigin()[1] / alpha2 + py, 0, 0, 1;
138 139 140 141 142 143 144 145 146 147 148 149 150

        buildMatrices();
    }

    std::pair<RealVector_t, RealVector_t> Geometry::computeRayTo(const RealVector_t& p) const
    {
        // the ray origin is always the camera center
        RealVector_t ro = _C;

        // homogeneous coordinates [p;1] - p is in detector space
        RealVector_t homP(_objectDimension);
        homP << p, 1;

151 152 153
        // multiplication of inverse projection matrix and homogeneous detector coordinate
        auto rd = (_Pinv * homP).normalized();
        return std::make_pair(ro, rd.head(_objectDimension));
154 155
    }

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

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

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

Jens Petit's avatar
Jens Petit committed
162
    const RealMatrix_t& Geometry::getRotationMatrix() const { return _R; }
163 164 165

    bool Geometry::operator==(const Geometry& other) const
    {
Jens Petit's avatar
Jens Petit committed
166 167 168
        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);
169 170 171 172
    }

    void Geometry::buildMatrices()
    {
Jens Petit's avatar
Jens Petit committed
173
        RealMatrix_t tmpRt(_objectDimension + 1, _objectDimension + 1);
174 175 176 177 178
        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
179
        RealMatrix_t tmpId(_objectDimension, _objectDimension + 1);
180 181 182 183 184 185
        tmpId.setIdentity();

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

        // compute the camera center
Jens Petit's avatar
Jens Petit committed
186 187 188
        _C = -(_P.block(0, 0, _objectDimension, _objectDimension)
                   .colPivHouseholderQr()
                   .solve(_P.block(0, _objectDimension, _objectDimension, 1)));
189 190

        // compute inverse _Pinv of _P via its components
Jens Petit's avatar
Jens Petit committed
191 192
        RealMatrix_t Sinv =
            (static_cast<real_t>(1.0) / _S.diagonal().array()).matrix().asDiagonal();
193 194 195

        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
196
        Kinv(0, _objectDimension - 1) = -_K(0, _objectDimension - 1) / _K(0, 0);
197 198
        if (_objectDimension == 3) {
            Kinv(1, 1) = static_cast<real_t>(1.0) / _K(1, 1);
Jens Petit's avatar
Jens Petit committed
199
            Kinv(1, _objectDimension - 1) = -_K(1, _objectDimension - 1) / _K(1, 1);
200 201
        }

Jens Petit's avatar
Jens Petit committed
202
        RealMatrix_t Rtinv(_objectDimension + 1, _objectDimension + 1);
203 204 205 206 207
        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
208
        RealMatrix_t tmpIdinv(_objectDimension + 1, _objectDimension);
209 210 211 212 213 214
        tmpIdinv.setIdentity();

        _Pinv = Sinv * Rtinv * tmpIdinv * Kinv;
    }

} // namespace elsa