D:/Programmierung/C/TRTK/include/TRTK/EstimateProjectiveTransformation3D.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.2.4 (2013-08-20)
*/

#ifndef ESTIMATE_PROJECTIVE_TRANSFORMATION_3D_HPP_1231333218
#define ESTIMATE_PROJECTIVE_TRANSFORMATION_3D_HPP_1231333218


#include <cmath>
#include <vector>

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

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


namespace TRTK
{


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

    typedef EstimateTransformation3D<T> super;

public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef T value_type;

    typedef typename super::ArrayXT ArrayXT;
    typedef typename super::MatrixXT MatrixXT;
    typedef typename super::VectorXT VectorXT;
    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;

    EstimateProjectiveTransformation3D();

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

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

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

    virtual ~EstimateProjectiveTransformation3D();

    virtual void compute();

    void setMaxIterations(int value);
    void setMaxRMS(value_type value);

private:

    int m_max_iterations;
    static const int MAX_ITERATIONS = 10;

    value_type m_max_RMS;
    static const value_type MAX_RMS;
};


template <class T>
const T EstimateProjectiveTransformation3D<T>::MAX_RMS = 0.1;


template <class T>
EstimateProjectiveTransformation3D<T>::EstimateProjectiveTransformation3D() :
    m_max_iterations(MAX_ITERATIONS),
    m_max_RMS(MAX_RMS)
{
}


template <class T>
EstimateProjectiveTransformation3D<T>::EstimateProjectiveTransformation3D(const std::vector<Coordinate<T> > & source_points,
                                                                          const std::vector<Coordinate<T> > & target_points) :
    m_max_iterations(MAX_ITERATIONS),
    m_max_RMS(MAX_RMS)
{
    super::setSourcePoints(source_points);
    super::setTargetPoints(target_points);
}


template <class T>
EstimateProjectiveTransformation3D<T>::EstimateProjectiveTransformation3D(const std::vector<Vector3T> & source_points,
                                                                          const std::vector<Vector3T> & target_points) :
    m_max_iterations(MAX_ITERATIONS),
    m_max_RMS(MAX_RMS)
{
    super::setSourcePoints(source_points);
    super::setTargetPoints(target_points);
}


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


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


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

    /*

    Explanation of the algorithm (in 2D):

    By using homogeneous coordinates an projective transformation can be described as follows:

        | a_11  a_12  a_13 |     | source_x |    | target_x' |
        | a_21  a_22  a_23 |  *  | source_y | =  | target_y' |
        | a_31  a_32  1    |     | 1        |    | target_z' |.   <-- note: target_z' is *not* known

    Then the sought target coordinates are:

        | target_x |     | target_x' / target_z' |
        | target_y |  =  | target_y' / target_z' |
        | target_z |     | 1                     |.

    The matrix A can be normalized such that component a_33 is equal to one, i.e. A := A' / a'_33, because of the
    subsequent normalisation step. Hence A has only 8 DoF. Since the normalisation step is nonlinear, we use
    Newton's method (after Simpson) to solve for the entries a_11, ..., a_32.

    The equation can be rewritten as a function f of the sought variables a_11, ..., a_32:

              | f_1 |                       1                        | a_11 * source_x + a_12 * source_y + a_13 |
        f  =  |     |  =  --------------------------------------  *  |                                          |
              | f_2 |      a_31 * source_x + a_32 * source_y + 1     | a_21 * source_x + a_22 * source_y + a_23 |.

    Then we try to find the zero-crossing of the residual (and we are done):

        residual(a_11, ..., a_32) = residual(A) := f(a_11, ..., a_32) - |target_x, target_y|^T.

    This can be done iteratively by computing

        A_new = A_old - Jacobian(residual)^(-1) * residual

    where

                   | c_3*source_x   c_3*source_y   c_3   0              0              0     -c_1*c_4*source_x  -c_1*c_4*source_y |
        Jacobian = |                                                                                                              |
                   | 0              0              0     c_3*source_x   c_3*source_y   c_3   -c_2*c_4*source_x  -c_2*c_4*source_y |

    and

        c_1 = a_11 * source_x + a_12 * source_y + a_13
        c_2 = a_21 * source_x + a_22 * source_y + a_23
        c_3 = 1 / (a_31 * source_x + a_31 * source_y + 1)
        c_4 = c_3^2.

    Instead of inverting the Jacobian directly we rewrite the above equation:

        delta_A := A_new - A_old = - Jacobian(residual)^(-1) * residual

    and solve the system of linear equations with respect to delta_A:

        delta_A * Jacobian = -residual.

    By using the method of least squares (or the Moore-Penrose pseudoinverse) we get

        delta_A = - (Jacobian^T * Jacobian)^(-1) * (Jacobian^T * residual).

    Then (in each iteration step) the new solution vector becomes:

        A_new = A_old + delta_A.

    */

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

    // initialize solution matrix

    Matrix4T A;
    A.setIdentity();

    const ArrayXT & source_x = super::m_source_points.block(0, 0, 1, number_points).transpose();
    const ArrayXT & source_y = super::m_source_points.block(1, 0, 1, number_points).transpose();
    const ArrayXT & source_z = super::m_source_points.block(2, 0, 1, number_points).transpose();

    const ArrayXT & target_x = super::m_target_points.block(0, 0, 1, number_points).transpose();
    const ArrayXT & target_y = super::m_target_points.block(1, 0, 1, number_points).transpose();
    const ArrayXT & target_z = super::m_target_points.block(2, 0, 1, number_points).transpose();

    for (int i = 0; i < m_max_iterations; ++i)
    {
        // construct Jacobian matrix

        ArrayXT ones = ArrayXT::Ones(number_points);

        ArrayXT c_1(number_points, 1);
        ArrayXT c_2(number_points, 1);
        ArrayXT c_3(number_points, 1);
        ArrayXT c_4(number_points, 1);
        ArrayXT c_5(number_points, 1);

        c_1 = A(0, 0) * source_x + A(0, 1) * source_y + A(0, 2) * source_z + A(0, 3) * ones;
        c_2 = A(1, 0) * source_x + A(1, 1) * source_y + A(1, 2) * source_z + A(1, 3) * ones;
        c_3 = A(2, 0) * source_x + A(2, 1) * source_y + A(2, 2) * source_z + A(2, 3) * ones;
        c_4 = (A(3, 0) * source_x + A(3, 1) * source_y + A(3, 2) * source_z + ones).inverse();
        c_5 = c_4 * c_4;

        MatrixXT Jacobian(3 * number_points, 15);

        Jacobian.setZero();

        Jacobian.block(0 * number_points,  0, number_points, 1) = c_4 * source_x;
        Jacobian.block(0 * number_points,  1, number_points, 1) = c_4 * source_y;
        Jacobian.block(0 * number_points,  2, number_points, 1) = c_4 * source_z;
        Jacobian.block(0 * number_points,  3, number_points, 1) = c_4;
        Jacobian.block(0 * number_points, 12, number_points, 1) = -c_1 * c_5 * source_x;
        Jacobian.block(0 * number_points, 13, number_points, 1) = -c_1 * c_5 * source_y;
        Jacobian.block(0 * number_points, 14, number_points, 1) = -c_1 * c_5 * source_z;

        Jacobian.block(1 * number_points,  4, number_points, 1) = c_4 * source_x;
        Jacobian.block(1 * number_points,  5, number_points, 1) = c_4 * source_y;
        Jacobian.block(1 * number_points,  6, number_points, 1) = c_4 * source_z;
        Jacobian.block(1 * number_points,  7, number_points, 1) = c_4;
        Jacobian.block(1 * number_points, 12, number_points, 1) = -c_2 * c_5 * source_x;
        Jacobian.block(1 * number_points, 13, number_points, 1) = -c_2 * c_5 * source_y;
        Jacobian.block(1 * number_points, 14, number_points, 1) = -c_2 * c_5 * source_z;

        Jacobian.block(2 * number_points,  8, number_points, 1) = c_4 * source_x;
        Jacobian.block(2 * number_points,  9, number_points, 1) = c_4 * source_y;
        Jacobian.block(2 * number_points, 10, number_points, 1) = c_4 * source_z;
        Jacobian.block(2 * number_points, 11, number_points, 1) = c_4;
        Jacobian.block(2 * number_points, 12, number_points, 1) = -c_3 * c_5 * source_x;
        Jacobian.block(2 * number_points, 13, number_points, 1) = -c_3 * c_5 * source_y;
        Jacobian.block(2 * number_points, 14, number_points, 1) = -c_3 * c_5 * source_z;

        // compute residual vector

        VectorXT residual(3 * number_points);

        residual.block(0 * number_points, 0, number_points, 1)  = c_1 * c_4 - target_x;
        residual.block(1 * number_points, 0, number_points, 1)  = c_2 * c_4 - target_y;
        residual.block(2 * number_points, 0, number_points, 1)  = c_3 * c_4 - target_z;

        // compute difference vector

        VectorXT delta_A = - ((Jacobian.transpose() * Jacobian).inverse() * (Jacobian.transpose() * residual));

        // compute estimate of transformation matrix

        A(0, 0) += delta_A( 0);
        A(0, 1) += delta_A( 1);
        A(0, 2) += delta_A( 2);
        A(0, 3) += delta_A( 3);
        A(1, 0) += delta_A( 4);
        A(1, 1) += delta_A( 5);
        A(1, 2) += delta_A( 6);
        A(1, 3) += delta_A( 7);
        A(2, 0) += delta_A( 8);
        A(2, 1) += delta_A( 9);
        A(2, 2) += delta_A(10);
        A(2, 3) += delta_A(11);
        A(3, 0) += delta_A(12);
        A(3, 1) += delta_A(13);
        A(3, 2) += delta_A(14);

        // compute MSE as stop criterion

        Transform3D<T> transform(A);

        value_type RMS = 0;

        for (int n = 0; n < number_points; ++n)
        {
            Vector3T source = Vector3T(source_x(n), source_y(n), source_z(n));
            Vector3T target = Vector3T(target_x(n), target_y(n), target_z(n));

            RMS += (transform * source - target).squaredNorm();
        }

        using std::sqrt;

        RMS = sqrt(RMS / number_points);

        if (RMS < m_max_RMS) break;
    }

    super::m_transformation_matrix = A;
}


template <class T>
void EstimateProjectiveTransformation3D<T>::setMaxIterations(int value)
{
    m_max_iterations = value;
}


template <class T>
void EstimateProjectiveTransformation3D<T>::setMaxRMS(value_type value)
{
    m_max_RMS = value;
}


} // namespace TRTK


#endif // ESTIMATE_PROJECTIVE_TRANSFORMATION_3D_HPP_1231333218