D:/Programmierung/C/TRTK/include/TRTK/EstimateRigidTransformation3D.hpp

Go to the documentation of this file.

/*
    Copyright (C) 2010 - 2014 Christoph Haenisch

    Chair of Medical Engineering (mediTEC)
    RWTH Aachen University
    Pauwelsstr. 20
    52074 Aachen
    Germany

    Version 0.4.0 (2019-07-19)
*/

#ifndef ESTIMATE_RIGID_TRANSFORMATION_3D_HPP_1230814474
#define ESTIMATE_RIGID_TRANSFORMATION_3D_HPP_1230814474


#include <cmath>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <Eigen/SVD>

#include "Coordinate.hpp"
#include "ErrorObj.hpp"
#include "EstimateTransformation3D.hpp"


namespace TRTK
{


template <class T>
class EstimateRigidTransformation3D : public EstimateTransformation3D<T>
{
private:

    typedef EstimateTransformation3D<T> super;

public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef T value_type;

    typedef typename super::VectorXT VectorXT;
    typedef typename super::MatrixXT MatrixXT;
    typedef typename super::Vector2T Vector2T;
    typedef typename super::Vector3T Vector3T;
    typedef typename super::Vector4T Vector4T;
    typedef typename super::RowVector2T RowVector2T;
    typedef typename super::RowVector3T RowVector3T;
    typedef typename super::Matrix2T Matrix2T;
    typedef typename super::Matrix3T Matrix3T;
    typedef typename super::Matrix4T Matrix4T;

    using super::NOT_ENOUGH_POINTS;
    using super::UNEQUAL_NUMBER_OF_POINTS;
    using super::UNKNOWN_ERROR;
    using super::WRONG_POINT_SIZE;

    EstimateRigidTransformation3D();

    EstimateRigidTransformation3D(const std::vector<Coordinate<T> > & source_points,
                                  const std::vector<Coordinate<T> > & target_points);

    EstimateRigidTransformation3D(const std::vector<Vector3T> & source_points,
                                  const std::vector<Vector3T> & target_points);

    EstimateRigidTransformation3D(const std::vector<Vector4T, Eigen::aligned_allocator<Vector4T> > & source_points,
                                  const std::vector<Vector4T, Eigen::aligned_allocator<Vector4T> > & target_points);

    virtual ~EstimateRigidTransformation3D();

    virtual void compute() override;
    void allowReflection(bool value = true);

    Matrix3T getRotationMatrix() const;
    Vector3T getTranslationVector() const;

private:
    bool allow_reflection = false;
};


template <class T>
EstimateRigidTransformation3D<T>::EstimateRigidTransformation3D()
{
}


template <class T>
EstimateRigidTransformation3D<T>::EstimateRigidTransformation3D(const std::vector<Coordinate<T> > & source_points,
                                                                const std::vector<Coordinate<T> > & target_points)
{
    super::setSourcePoints(source_points);
    super::setTargetPoints(target_points);
}


template <class T>
EstimateRigidTransformation3D<T>::EstimateRigidTransformation3D(const std::vector<Vector3T> & source_points,
                                                                const std::vector<Vector3T> & target_points)
{
    super::setSourcePoints(source_points);
    super::setTargetPoints(target_points);
}


template <class T>
EstimateRigidTransformation3D<T>::EstimateRigidTransformation3D(const std::vector<Vector4T, Eigen::aligned_allocator<Vector4T> > & source_points,
                                                                const std::vector<Vector4T, Eigen::aligned_allocator<Vector4T> > & target_points)
{
    super::setSourcePoints(source_points);
    super::setTargetPoints(target_points);
}


template <class T>
EstimateRigidTransformation3D<T>::~EstimateRigidTransformation3D()
{
}


template <class T>
void EstimateRigidTransformation3D<T>::allowReflection(bool value)
{
    allow_reflection = value;
}


template <class T>
void EstimateRigidTransformation3D<T>::compute()
{
    if (super::m_source_points.size() != super::m_target_points.size())
    {
        ErrorObj error;
        error.setClassName("EstimateRigidTransformation3D<T>");
        error.setFunctionName("compute");
        error.setErrorMessage("Point sets must have the same cardinality.");
        error.setErrorCode(UNEQUAL_NUMBER_OF_POINTS);
        throw error;
    }

    if (super::m_source_points.cols() < 3 || super::m_target_points.cols() < 3)
    {
        ErrorObj error;
        error.setClassName("EstimateRigidTransformation3D<T>");
        error.setFunctionName("compute");
        error.setErrorMessage("Not enough points to compute transformation matrix.");
        error.setErrorCode(NOT_ENOUGH_POINTS);
        throw error;
    }

    /*

    Explanation of the algorithm (see also "Image Guided Therapy" lecture):

    We try to estimate the rotation around the centroids of both data sets. Thus we first
    subtract the mean of both point sets such that the sets are centered around the point
    of origin. Then we compute the correlation matrix between both point sets:

    R_XY = (X - mean(X)) * (Y - mean(Y))^T.

    By the way, R_XY is nothing else but the cross-covariance matrix of X and Y, cov(X,Y).
    Now, R_XY can be (equivalently) decomposed in two ways, first by means of a singular
    value decomposition (SVD)

    R_XY = U * Sigma * V^T,

    where U and V are orthogonal matrices and Sigma is a diagonal matrix, and second
    by using an Eigen decomposition

    R_XY = (X - mean(X)) * (Y - mean(Y))^T
         = x * y^T
         = x * [R * x]^T                (use y = R * x)
         = x * x^T * R^T
         = S * D * S^T * R^T            (Eigen decomposition of x * x^T)

    where x and y are the zero mean vectors of X and Y, respectively, D is a diagonal
    matrix, S is an orthogonal matrix and R is the sought rotation matrix. Comparison of

    R_XY = U * Sigma * V^T,
         = S * D     * S^T * R^T

    yields

    V^T = S^T * R^T = U^T * R^T.

    It follows that

    R = V * U^T.

    Now we can construct the homogeneous transformation matrix as follows:

    | R | T |     | I | mean(Y) |     | R | 0 |     | I | -mean(X) |
    | --+-- |  =  | --+-------- |  *  | --+-- |  *  | --+--------- |
    | 0 | 1 |     | 0 |   1     |     | 0 | 1 |     | 0 |    1     |

                  | R | mean(Y) - R * mean(X) |
               =  | --+---------------------- |
                  | 0 |          1            |

    Note: The above equation basically says: move the centroid of the source data to
          the point of origin, rotate the data and move it to the position of the
          centroid of the target data.

    Note: I is the identity matrix and T is an arbitrary translation vector.

    Note: Actually the rotation matrix on the left hand side should have had a name
          different from R, but since the entries are the same, we kept the name.
    */

    MatrixXT X = super::m_source_points;
    MatrixXT Y = super::m_target_points;

    // subtract mean

    Vector3T mean_X = X.rowwise().mean();
    Vector3T mean_Y = Y.rowwise().mean();

    X.colwise() -= mean_X;
    Y.colwise() -= mean_Y;

    // compute SVD of cross-covariance matrix

    MatrixXT R_XY = X * Y.adjoint();

    Eigen::JacobiSVD<MatrixXT> svd(R_XY, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // compute estimate of the rotation matrix

    Matrix3T R = svd.matrixV() * svd.matrixU().adjoint();

    // assure a right-handed coordinate system

    if (R.determinant() < 0 && !allow_reflection)
    {
        R = svd.matrixV() * Vector3T(1, 1, -1).asDiagonal() * svd.matrixU().transpose();
    }

    // construct homogeneous transformation matrix

    super::m_transformation_matrix.block(0, 0, 3, 3) = R;
    super::m_transformation_matrix.block(0, 3, 3, 1) = mean_Y - R * mean_X;
    super::m_transformation_matrix.block(3, 0, 1, 3) = RowVector3T::Zero();
    // super::m_transformation_matrix(3, 3) = 1.0; // already one from initialisation
}


template <class T>
typename EstimateRigidTransformation3D<T>::Matrix3T EstimateRigidTransformation3D<T>::getRotationMatrix() const
{
    return super::m_transformation_matrix.block(0, 0, 3, 3);
}


template <class T>
typename EstimateRigidTransformation3D<T>::Vector3T EstimateRigidTransformation3D<T>::getTranslationVector() const
{
    return super::m_transformation_matrix.block(0, 3, 3, 1);
}


} // namespace TRTK


#endif // ESTIMATE_RIGID_TRANSFORMATION_3D_HPP_1230814474