D:/Programmierung/C/TRTK/include/TRTK/EstimateSimilarityTransformation2D.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.1 (2011-11-10)
*/

#ifndef ESTIMATE_SIMILARITY_TRANSFORMATION_2D_HPP_7322190897
#define ESTIMATE_SIMILARITY_TRANSFORMATION_2D_HPP_7322190897


#include <cmath>
#include <vector>

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

#include "Coordinate.hpp"
#include "ErrorObj.hpp"
#include "EstimateTransformation2D.hpp"


namespace TRTK
{


template <class T>
class EstimateSimilarityTransformation2D : public EstimateTransformation2D<T>
{
private:

    typedef EstimateTransformation2D<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;

    EstimateSimilarityTransformation2D();

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

    EstimateSimilarityTransformation2D(const std::vector<Vector2T, Eigen::aligned_allocator<Vector2T> > & source_points,
                                       const std::vector<Vector2T, Eigen::aligned_allocator<Vector2T> > & target_points);

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

    virtual ~EstimateSimilarityTransformation2D();

    virtual void compute();

    Matrix2T getRotationMatrix() const;
    Vector2T getTranslationVector() const;

    value_type getRotationAngle() const;
    value_type getScalingFactor() const;

private:

    value_type rotation_angle;
    value_type scaling_factor;
};


template <class T>
EstimateSimilarityTransformation2D<T>::EstimateSimilarityTransformation2D() :
    rotation_angle(0),
    scaling_factor(0)
{
}


template <class T>
EstimateSimilarityTransformation2D<T>::EstimateSimilarityTransformation2D(const std::vector<Coordinate<T> > & source_points,
                                                                          const std::vector<Coordinate<T> > & target_points) :
    rotation_angle(0),
    scaling_factor(0)

{
    super::setSourcePoints(source_points);
    super::setTargetPoints(target_points);
}


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


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


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


template <class T>
void EstimateSimilarityTransformation2D<T>::compute()
{
    if (super::m_source_points.size() != super::m_target_points.size())
    {
        ErrorObj error;
        error.setClassName("EstimateSimilarityTransformation2D<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("EstimateSimilarityTransformation2D<T>");
        error.setFunctionName("compute");
        error.setErrorMessage("Not enough points to compute transformation matrix.");
        error.setErrorCode(NOT_ENOUGH_POINTS);
        throw error;
    }

    /*

    Explanation of the algorithm:

    By using homogeneous coordinates a rigid transformation can be described as follows:

    | alpha*cos(phi)  -alpha*sin(phi)  t_x |     | source_x |    | target_x |
    | alpha*sin(phi)  alpha*cos(phi)   t_y |  *  | source_y | =  | target_y |
    | 0               0                1   |     | 1        |    | 1        |

    The equation can be rewritten as:

                                      | alpha * cos(phi) |
    | source_x  -source_y  1  0 |  *  | alpha * sin(phi) |  =  | target_x |
    | source_y  source_x   0  1 |     | t_x              |     | target_y |
                                      | t_Y              |

    Or in short:  A * x = b

    By using more and more points this leads to an over-determined system, which can
    be solved via the pseudo inverse (i.e. A becomes A = [A1 A2 A3 ...]^T):

    <==>    A * x = b
    <==>    A^T * A * x = A^T * b
    <==>    inverse(A^T * A) * A^T * A * x = x = inverse(A^T * A) * A^T * b

    Thus:

    x = pseudo_inverse * b = [inverse(A^T * A) * A^T] * b

    ==>     phi = atan2(x(1), x(0))                         (angle of rotation)
    ==>     alpha = sqrt(x(0) * x(0) + x(1) * x(1))         (scaling factor)

    */

    using std::atan2;
    using std::cos;
    using std::sin;

    const int number_points =  super::m_source_points.cols();

    // fill matrix A with the above described entries

    MatrixXT A(2*number_points, 4);

    A.setZero();
    A.block(0*number_points, 0, number_points, 1) =  super::m_source_points.block(0, 0, 1, number_points).transpose();
    A.block(0*number_points, 1, number_points, 1) = -super::m_source_points.block(1, 0, 1, number_points).transpose();
    A.block(0*number_points, 2, number_points, 1) =  VectorXT::Ones(number_points);
    A.block(1*number_points, 0, number_points, 1) =  super::m_source_points.block(1, 0, 1, number_points).transpose();
    A.block(1*number_points, 1, number_points, 1) =  super::m_source_points.block(0, 0, 1, number_points).transpose();
    A.block(1*number_points, 3, number_points, 1) =  VectorXT::Ones(number_points);

    // fill vector b

    VectorXT b(2*number_points, 1);
    b.block(0*number_points, 0, number_points, 1) = super::m_target_points.block(0, 0, 1, number_points).transpose();
    b.block(1*number_points, 0, number_points, 1) = super::m_target_points.block(1, 0, 1, number_points).transpose();

    // compute pseudo inverse

    MatrixXT pseudo_inverse = (A.transpose() * A).inverse() * A.transpose();

    // compute vector x whose components are/contain the sought transformation entries

    VectorXT x = pseudo_inverse * b;

    // compute the angle of rotation as well as the scaling factor

    rotation_angle = atan2(x(1), x(0)); // cos(phi) and sin(phi) by oneself might diverge from each other

    using std::sqrt;
    scaling_factor = sqrt(x(0) * x(0) + x(1) * x(1));

    super::m_transformation_matrix(0, 0) = scaling_factor * cos(rotation_angle);
    super::m_transformation_matrix(0, 1) = scaling_factor * -sin(rotation_angle);
    super::m_transformation_matrix(0, 2) = x(2);
    super::m_transformation_matrix(1, 0) = scaling_factor * sin(rotation_angle);
    super::m_transformation_matrix(1, 1) = scaling_factor * cos(rotation_angle);
    super::m_transformation_matrix(1, 2) = x(3);
    super::m_transformation_matrix(2, 0) = 0;
    super::m_transformation_matrix(2, 1) = 0;
    super::m_transformation_matrix(2, 2) = 1;
}


template <class T>
typename EstimateSimilarityTransformation2D<T>::Matrix2T EstimateSimilarityTransformation2D<T>::getRotationMatrix() const
{
    return super::m_transformation_matrix.block(0, 0, 2, 2);
}


template <class T>
typename EstimateSimilarityTransformation2D<T>::Vector2T EstimateSimilarityTransformation2D<T>::getTranslationVector() const
{
    return super::m_transformation_matrix.block(0, 2, 2, 1);
}


template <class T>
typename EstimateSimilarityTransformation2D<T>::value_type EstimateSimilarityTransformation2D<T>::getRotationAngle() const
{
    return rotation_angle;
}


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


} // namespace TRTK


#endif // ESTIMATE_SIMILARITY_TRANSFORMATION_2D_HPP_7322190897