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

Go to the documentation of this file.

/*
    Estimation and computation of a trivariate quadratic polynomial.

    Copyright (C) 2010 - 2014 Christoph Haenisch

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

    See license.txt for more information.

    Version 0.2.0 (2013-03-28)
*/

#ifndef TRIVARIATE_QUADRATIC_POLYNOMIAL_HPP_0512325448
#define TRIVARIATE_QUADRATIC_POLYNOMIAL_HPP_0512325448


#include <cassert>
#include <cmath>
#include <vector>

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

#include "Coordinate.hpp"
#include "ErrorObj.hpp"
#include "Polynomial.hpp"
#include "Range.hpp"


namespace TRTK
{


template <class T>
class TrivariateQuadraticPolynomial : public Polynomial<T>
{
private:

    typedef Polynomial<T> super;

protected:

    typedef typename super::VectorXT VectorXT;
    typedef typename super::Vector3T Vector3T;
    typedef typename super::coordinate_type coordinate_type;

public:

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef T value_type;
    typedef typename super::MatrixXT MatrixXT;
    typedef typename super::Point Point;

    using super::DIVISION_BY_ZERO;
    using super::INVALID_ARGUMENT;
    using super::NOT_ENOUGH_POINTS;
    using super::UNEQUAL_NUMBER_OF_POINTS;
    using super::UNKNOWN_ERROR;
    using super::WRONG_COORDINATE_SIZE;

    TrivariateQuadraticPolynomial();

    TrivariateQuadraticPolynomial(const MatrixXT &);

    TrivariateQuadraticPolynomial(const TrivariateQuadraticPolynomial &);

    template <class U>
    TrivariateQuadraticPolynomial(const TrivariateQuadraticPolynomial<U> &);

    virtual ~TrivariateQuadraticPolynomial();

    MatrixXT & getCoefficients();
    const MatrixXT & getCoefficients() const;

    T estimate(Range<Point> source_points,
               Range<Point> target_points,
               Range<T> weights = Range<T>());

    T estimate(Iterator<Point> source_points_first,
               Iterator<Point> source_points_last,
               Iterator<Point> target_points_first,
               Iterator<Point> target_points_last,
               Iterator<T> weights_first = Iterator<T>(),
               Iterator<T> weights_last = Iterator<T>());

    Point operator*(const Point &) const;
    Point transform(const Point &) const;

    Polynomial<T> & reset();

private:

    MatrixXT m_matrix;
};


template <class T>
TrivariateQuadraticPolynomial<T>::TrivariateQuadraticPolynomial() :
    m_matrix(MatrixXT(3, 10))
{
    m_matrix << 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 1, 0, 0, 0, 0, 0, 0;
}


template <class T>
TrivariateQuadraticPolynomial<T>::TrivariateQuadraticPolynomial(const MatrixXT & matrix)
{
    assert(matrix.rows() == 3 && matrix.cols() == 10);
    m_matrix = matrix;
}


template <class T>
TrivariateQuadraticPolynomial<T>::TrivariateQuadraticPolynomial(const TrivariateQuadraticPolynomial & other)
{
    m_matrix =  other.m_matrix;
}


template <class T>
template <class U>
TrivariateQuadraticPolynomial<T>::TrivariateQuadraticPolynomial(const TrivariateQuadraticPolynomial<U> & other)
{
    m_matrix = other.getCoefficients().template cast<T>();
}


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


template <class T>
typename TrivariateQuadraticPolynomial<T>::MatrixXT & TrivariateQuadraticPolynomial<T>::getCoefficients()
{
    return m_matrix;
}


template <class T>
const typename TrivariateQuadraticPolynomial<T>::MatrixXT & TrivariateQuadraticPolynomial<T>::getCoefficients() const
{
    return m_matrix;
}


template <class T>
T TrivariateQuadraticPolynomial<T>::estimate(Range<Point> source_points, Range<Point> target_points, Range<T> weights)
{
    return estimate(source_points.begin(),
                    source_points.end(),
                    target_points.begin(),
                    target_points.end(),
                    weights.begin(),
                    weights.end());
}


template <class T>
T TrivariateQuadraticPolynomial<T>::estimate(Iterator<Point> source_points_first,
                                             Iterator<Point> source_points_last,
                                             Iterator<Point> target_points_first,
                                             Iterator<Point> target_points_last,
                                             Iterator<T> weights_first,
                                             Iterator<T> weights_last)
{
    const int number_points = distance(source_points_first, source_points_last);

    if (distance(target_points_first, target_points_last) != number_points)
    {
        ErrorObj error;
        error.setClassName("TrivariateQuadraticPolynomial<T>");
        error.setFunctionName("estimate");
        error.setErrorMessage("Point sets must have the same cardinality.");
        error.setErrorCode(UNEQUAL_NUMBER_OF_POINTS);
        throw error;
    }

    if (weights_first && weights_last)
    {
        if (distance(weights_first, weights_last) != number_points)
        {
            ErrorObj error;
            error.setClassName("TrivariateQuadraticPolynomial<T>");
            error.setFunctionName("estimate");
            error.setErrorMessage("Point sets must have the same cardinality.");
            error.setErrorCode(UNEQUAL_NUMBER_OF_POINTS);
            throw error;
        }
    }

    if (number_points < 10)
    {
        ErrorObj error;
        error.setClassName("TrivariateQuadraticPolynomial<T>");
        error.setFunctionName("estimate");
        error.setErrorMessage("Not enough points to estimate the transformation.");
        error.setErrorCode(NOT_ENOUGH_POINTS);
        throw error;
    }

    /*

    Explanation of the algorithm:

    The trivariate quadratic function can be described as follows:

    | x' |     | m_1,1  m_1,2  ...  m_1,10 |
    | y  |  =  | m_2,1  m_2,2  ...  m_2,10 |  *  | 1  x  y  ...  y^2  yz  z^2 |^T
    | z' |     | m_3,1  m_3,2  ...  m_3,10 |

    The equation can be rewritten as:

    | x' |     | 1  ...  z^2  0  ...  0    0  ...  0   |     | m_1,1  |
    | y' |  =  | 0  ...  0    1  ...  z^2  0  ...  0   |  *  | ...    |
    | z' |     | 0  ...  0    0  ...  0    1  ...  z^2 |     | m_3,10 |

    Or in short:  b = A * x          (here, x is meant to be (m_1,1, ..., m_3,10)^T)

    By using more and more points (source_1, target_1), ..., (source_n, target_n)

         | A(1) |         | b(1) |
    A := | ...  | ,  b := | ...  |
         | A(n) |         | b(n) |

    this leads to an over-determined system. However, this can be easily solved
    using the pseudo inverse:

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

    Thus, we have found the sought coefficients.

    Update: We now use weighted least squares so that x = inverse(A^T * W * A) * A^T * W * b
            where W is a diagonal matrix

    */

    // Fill the matrix A with the above described entries (different order as above).

    MatrixXT A(3 * number_points, 30);

    A.setZero();

    int i = 0;
    for (Iterator<Point> it = source_points_first; it != source_points_last; ++it, ++i)
    {
        T x = (*it).x();
        T y = (*it).y();
        T z = (*it).z();

        A(0 * number_points + i, 0) = 1;
        A(0 * number_points + i, 1) = x;
        A(0 * number_points + i, 2) = y;
        A(0 * number_points + i, 3) = z;
        A(0 * number_points + i, 4) = x * x;
        A(0 * number_points + i, 5) = x * y;
        A(0 * number_points + i, 6) = x * z;
        A(0 * number_points + i, 7) = y * y;
        A(0 * number_points + i, 8) = y * z;
        A(0 * number_points + i, 9) = z * z;

        A(1 * number_points + i, 10) = 1;
        A(1 * number_points + i, 11) = x;
        A(1 * number_points + i, 12) = y;
        A(1 * number_points + i, 13) = z;
        A(1 * number_points + i, 14) = x * x;
        A(1 * number_points + i, 15) = x * y;
        A(1 * number_points + i, 16) = x * z;
        A(1 * number_points + i, 17) = y * y;
        A(1 * number_points + i, 18) = y * z;
        A(1 * number_points + i, 19) = z * z;

        A(2 * number_points + i, 20) = 1;
        A(2 * number_points + i, 21) = x;
        A(2 * number_points + i, 22) = y;
        A(2 * number_points + i, 23) = z;
        A(2 * number_points + i, 24) = x * x;
        A(2 * number_points + i, 25) = x * y;
        A(2 * number_points + i, 26) = x * z;
        A(2 * number_points + i, 27) = y * y;
        A(2 * number_points + i, 28) = y * z;
        A(2 * number_points + i, 29) = z * z;
    }

    // Fill the vector b.

    VectorXT b(3 * number_points);

    i = 0;
    for (Iterator<Point> it = target_points_first; it != target_points_last; ++it, ++i)
    {
        b(0 * number_points + i) = (*it).x();
        b(1 * number_points + i) = (*it).y();
        b(2 * number_points + i) = (*it).z();
    }

    // Compute the vector x whose components are the sought coefficients.

    VectorXT x;
    VectorXT W = VectorXT::Ones(3 * number_points); // diagonal weighting matrix (W.asDiagonal())

    if (weights_first && weights_last)
    {
        // Weighted Least Squares

        int i = 0;
        for (Iterator<T> it = weights_first; it != weights_last; ++it, ++i)
        {
            W(0 * number_points + i) = *it;
            W(1 * number_points + i) = *it;
            W(2 * number_points + i) = *it;
        }

        x = (A.transpose() * W.asDiagonal() * A).inverse() * (A.transpose() * (W.asDiagonal() * b));
    }
    else
    {
        // Ordinary Least Squares

        MatrixXT pseudo_inverse = (A.transpose() * A).inverse() * A.transpose();
        x = pseudo_inverse * b;
    }

    // Reinterprete the solution vector as a row-major matrix

    // m_matrix = Eigen::Map<MatrixXT, Eigen::Unaligned, Eigen::Stride<1, 10> >(x.data(), 3, 10);
    m_matrix = Eigen::Map<MatrixXT>(x.data(), 10, 3).transpose();

    // Compute the RMSE.

    using std::sqrt;

    if (weights_first && weights_last)
    {
        return  (W.cwiseSqrt().asDiagonal() * (A * x - b)).norm() / sqrt(T(number_points));
    }
    else
    {
        return  (A * x - b).norm() / sqrt(T(number_points));
    }
}


template <class T>
typename TrivariateQuadraticPolynomial<T>::Point TrivariateQuadraticPolynomial<T>::operator* (const Point & point) const
{
    assert(point.size() == 3);

    return transform(point);
}


template <class T>
Polynomial<T> & TrivariateQuadraticPolynomial<T>::reset()
{
    m_matrix << 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 1, 0, 0, 0, 0, 0, 0;

    return *this;
}


template <class T>
typename TrivariateQuadraticPolynomial<T>::Point TrivariateQuadraticPolynomial<T>::transform(const Point & point) const
{
    assert(point.size() == 3);

    Eigen::Matrix<T, 10, 1> vec;

    const T & x = point.x();
    const T & y = point.y();
    const T & z = point.z();

    vec(0) = 1;
    vec(1) = x;
    vec(2) = y;
    vec(3) = z;
    vec(4) = x * x;
    vec(5) = x * y;
    vec(6) = x * z;
    vec(7) = y * y;
    vec(8) = y * z;
    vec(9) = z * z;

    return Point(m_matrix * vec);
}


} // namespace TRTK


#endif // TRIVARIATE_QUADRATIC_POLYNOMIAL_HPP_0512325448