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

Go to the documentation of this file.

/*
    Copyright (C) 2010 - 2014 Christoph Haenisch, Fabian Killus

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

    Version 0.5.0 (2012-03-19)
*/

#ifndef FIT_PLANE_HPP_5948294843
#define FIT_PLANE_HPP_5948294843


#include <cmath>
#include <vector>

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

#include "ErrorObj.hpp"
#include "Coordinate.hpp"
#include "Fit3D.hpp"

namespace TRTK
{


template <class T>
class FitPlane : public Fit3D<T>
{
private:
    typedef Fit3D<T> super;

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef T value_type;
    typedef typename super::Vector3T Vector3T;
    typedef typename super::Vector4T Vector4T;
    typedef typename super::VectorXT VectorXT;
    typedef typename super::Matrix3T Matrix3T;
    typedef typename super::MatrixXT MatrixXT;

    FitPlane();
    FitPlane(const std::vector<Coordinate<T> > & points);
    FitPlane(const std::vector<Vector3T> & points);
    FitPlane(const std::vector<Vector4T, Eigen::aligned_allocator<Vector4T> > & points);

    virtual ~FitPlane();

    unsigned getNumberPointsRequired() const;
    void compute();
    T getRMS() const;

    const Coordinate<T> & getNormal() const;
    const Coordinate<T> & getPointInPlane() const;
    T getDistanceFromOrigin() const;
    T getDistanceTo(const Coordinate<T> & point) const;

    using super::DATA_POINTS_TOO_SIMILAR;
    using super::NOT_ENOUGH_POINTS;
    using super::UNKNOWN_ERROR;
    using super::WRONG_POINT_SIZE;

private:
    Coordinate<T> m_normal;
    Coordinate<T> m_point_in_plane;
    T m_rms;
};


template <class T>
FitPlane<T>::FitPlane() :
    m_normal(0, 0, 0),
    m_point_in_plane(0, 0, 0),
    m_rms(0)
{
}


template <class T>
FitPlane<T>::FitPlane(const std::vector<Coordinate<T> > & points) :
    m_normal(0, 0, 0),
    m_point_in_plane(0, 0, 0),
    m_rms(0)
{
    super::setPoints(points);
}


template <class T>
FitPlane<T>::FitPlane(const std::vector<Vector3T> & points) :
    m_normal(0, 0, 0),
    m_point_in_plane(0, 0, 0),
    m_rms(0)
{
    super::setPoints(points);
}


template <class T>
FitPlane<T>::FitPlane(const std::vector<Vector4T, Eigen::aligned_allocator<Vector4T> > & points) :
    m_normal(0, 0, 0),
    m_point_in_plane(0, 0, 0),
    m_rms(0)
{
    super::setPoints(points);
}


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


template <class T>
void FitPlane<T>::compute()
{
    /*

    Explanation of the algorithm:

    The columns of matrix X contain the provided points on the surface of the plane.
    First we subtract the mean of the point set from each point in the set such that
    the new set of points is centered around the point of origin.

    x = X - mean(X)

    With n being the normal vector of the plane, we should ideally get x_i^T * n = 0
    for all columns x_i of the matrix x. Therefore we define an error measure as

    E = sum_i ( x_i^T * n )^2

    This can be rearranged to

    E = sum_i ( x_i^T * n )^2
      = sum_i ( (x_i^T * n)^T * (x_i^T * n) )             because x_i^T * n is a scalar
      = sum_i ( n^T * x_i * x_i^T * n )
      = n^T * sum_i ( x_i * x_i^T ) * n
      = n^T * A * n                                       with the matrix A = x * x^T

    Minimizing the above term yields the sought/unknown normal n. To avoid the trivial
    solution n = 0, we use the Lagrange formalism, i.e. Lagrange multipliers. Our
    auxiliary condition is n * n^T = 1.

    L(n, lambda) := n^T * A * n - lambda * ( n^T * n - 1)   and   Grad( L(n, lambda) ) != 0

    => 2 A n - A lambda I n = 0                           with I being the indentity matrix
    => A n = lambda n

    Thus, the minimization problem reduces to an eigenvalue problem. Since

    E = n^T * A * n = n^T * lambda * n = lambda

    the sought normal is the eigenvector corresponding to the smallest eigenvalue.

    */

    using namespace std;
    using namespace Eigen;

    using std::sqrt;

    const int number_of_points = super::m_points.cols();

    if (number_of_points < 3)
    {
        ErrorObj error;
        error.setClassName("FitPlane<T>");
        error.setFunctionName("compute");
        error.setErrorMessage("Not enough points to fit the plane.");
        error.setErrorCode(NOT_ENOUGH_POINTS);
        throw error;
    }

    MatrixXT X = super::m_points;

    // Subtract the population mean and which is a point in the plane.

    VectorXT mean = X.rowwise().mean();
    X.colwise() -= mean;

    m_point_in_plane = Coordinate<T>(mean);

    // Construct the matrix A = sum ( x_i^T * x_i ).

    Matrix3T A = X * X.transpose();

    if (A.isZero())
    {
        ErrorObj error;
        error.setClassName("FitPlane<T>");
        error.setFunctionName("compute");
        error.setErrorMessage("Data points are too similar.");
        error.setErrorCode(DATA_POINTS_TOO_SIMILAR);
        throw error;
    }

    // Compute the eigenvalues and the eigenvectors of A.

    // Note: - Since A is symmetric, there are only real eigenvalues.
    //         ==> Use SelfAdjointEigenSolver.
    //       - SelfAdjointEigenSolver normalizes the eigenvectors.

    SelfAdjointEigenSolver<Matrix3T> eigensolver(A);

    VectorXT eigen_values = eigensolver.eigenvalues();
    MatrixXT eigen_vectors = eigensolver.eigenvectors();

    // Find the absolute minimum eigenvalue and its corresponding eigenvector.
    // Do also compute the RMS from the smallest eigenvalue.

    int index = 0;
    T minimum_eigenvalue = abs(eigen_values(index));

    for (int i = 1; i < eigen_values.rows(); ++i)
    {
        if (abs(eigen_values[i]) < minimum_eigenvalue)
        {
            index = i;
            minimum_eigenvalue = abs(eigen_values[index]);
        }
    }

    m_normal = Coordinate<T>(eigen_vectors.col(index));

    T sum_of_squared_errors = minimum_eigenvalue;
    m_rms = sqrt(sum_of_squared_errors / number_of_points);
}


template <class T>
T FitPlane<T>::getDistanceFromOrigin() const
{
    using std::abs;

    // The shortest distance can be found by projecting a vector pointing
    // to an arbitrary point in the plane (we just use the population mean
    // of all points) onto the normal vector of the plane.

    return abs(m_point_in_plane.dot(m_normal));
}


template <class T>
T FitPlane<T>::getDistanceTo(const Coordinate<T> & point) const
{
    using std::abs;

    /*

    The shortest distance can be found by projecting a difference vector
    formed from the given point and an arbitrary point in the plane (we
    just use the population mean of all points) onto the normal vector of
    the plane.

    If we take the normal n and two orthonormal vectors alpha and beta
    which altogether form a new basis, then we can express the above
    difference vector d by a linear combination as follows:

        d = a * alpha + b * beta + distance_to_plane * n

    Now, a simple inner product with n yields the sought distance

        d * n = 0 + 0 + distance_to_plane * 1

    */

    return abs((point - m_point_in_plane).dot(m_normal));
}


template <class T>
const Coordinate<T> & FitPlane<T>::getNormal() const
{
    return m_normal;
}


template <class T>
inline unsigned FitPlane<T>::getNumberPointsRequired() const
{
    return 3;
}


template <class T>
const Coordinate<T> & FitPlane<T>::getPointInPlane() const
{
    return m_point_in_plane;
}


template <class T>
T FitPlane<T>::getRMS() const
{
    return m_rms;
}


} // namespace TRTK


#endif // FITSPHERE_HPP_6184653510