D:/Programmierung/C/TRTK/include/TRTK/EstimateSimilarityTransformation3D.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.1.3 (2012-10-12)
*/

#ifndef ESTIMATE_SIMILARITY_TRANSFORMATION_3D_HPP_7829730810
#define ESTIMATE_SIMILARITY_TRANSFORMATION_3D_HPP_7829730810


#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 EstimateSimilarityTransformation3D : 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;

    EstimateSimilarityTransformation3D();

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

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

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

    virtual ~EstimateSimilarityTransformation3D();

    virtual void compute();

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

private:

    value_type scaling_factor;
};


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


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


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


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


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


template <class T>
void EstimateSimilarityTransformation3D<T>::compute()
{
    if (super::m_source_points.size() != super::m_target_points.size())
    {
        ErrorObj error;
        error.setClassName("EstimateSimilarityTransformation3D<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() < 2 || super::m_target_points.cols() < 2)
    {
        ErrorObj error;
        error.setClassName("EstimateSimilarityTransformation3D<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

    up to a possible scaling factor alpha contained in Sigma.

    To find the lost scaling factor, we use x and y again which are zero mean vectors of X and Y.
    The distance from origin (norm) of each target vector in x (column in x) is divided through the 
    distance from the origin (norm) of its corresponding source vector in y (column in y).
    The scaling factor should be the same for all target source pairs. We use the mean
    value as scaling factor and apply it to the rotation matrix:

    R = R * scaling_factor;

    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.

    Note: The rotation matrix is computed via the SVD to guarantee an orthogonality.
          The scaling factor can still be computed via the almost orthogonal matrix
          [x * x^T]^(-1) * R_XY.

    */

    using std::pow;

    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.transpose();

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

    // compute estimate of the rotation matrix

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

    // assure a right-handed coordinate system

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

    // compute the scaling factor

    scaling_factor = ( Y.colwise().norm().array() / X.colwise().norm().array() ).mean();

    R *= scaling_factor;

    // 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 EstimateSimilarityTransformation3D<T>::Matrix3T EstimateSimilarityTransformation3D<T>::getRotationMatrix() const
{
    return super::m_transformation_matrix.block(0, 0, 3, 3);
}


template <class T>
typename EstimateSimilarityTransformation3D<T>::value_type EstimateSimilarityTransformation3D<T>::getScalingFactor() const
{
    return scaling_factor;
}


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


} // namespace TRTK


#endif // ESTIMATE_SIMILARITY_TRANSFORMATION_3D_HPP_7829730810