D:/Programmierung/C/TRTK/include/TRTK/Icp.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

    See license.txt for more information.

    Version 0.3.0 (2014-07-04)
*/

#ifndef ICP_HPP_7908314317
#define ICP_HPP_7908314317


#include <cmath>
#include <cstddef>
#include <limits>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <flann/flann.hpp>

#include "Coordinate.hpp"
#include "ErrorObj.hpp"
#include "EstimateTransformation3D.hpp"
#include "Signals.hpp"
#include "Tools.hpp"
#include "Transform2D.hpp"
#include "Transform3D.hpp"


namespace TRTK
{


//                            IcpInterface                            //


template <class ValueType>
class IcpInterface
{
public:
    typedef Eigen::Matrix<ValueType, Eigen::Dynamic, Eigen::Dynamic> Matrix;

    virtual ~IcpInterface() = 0;

    virtual void setSourcePoints(const std::vector<Coordinate<ValueType> > & source_points) = 0;    
    virtual void setTargetPoints(const std::vector<Coordinate<ValueType> > & target_points) = 0;    
    virtual void setInitialEstimation(const Matrix & transformation) = 0;                           
    virtual void setEstimationAlgorithm(EstimateTransformation3D<ValueType> & estimator) = 0;       
    virtual void setMaximumNumberIterations(unsigned number) = 0;                                   
    virtual void setMaximumRMSE(ValueType value) = 0;                                               
    virtual void setEpsApproximate(float value) = 0;                                                

    virtual ValueType compute() = 0;                                                                

    virtual void abortComputation() = 0;                                                            

    virtual const Matrix & getTransformation() = 0;                                                 
};


template<class ValueType>
IcpInterface<ValueType>::~IcpInterface()
{
}


//                              IcpBase                               //


template<class ValueType>
class IcpBase : public IcpInterface<ValueType>
{
public:
    typedef IcpInterface<ValueType> super;
    typedef typename super::Matrix Matrix;

    IcpBase(unsigned dimension);
    ~IcpBase();

    void setSourcePoints(const std::vector<Coordinate<ValueType> > & source_points);    
    void setTargetPoints(const std::vector<Coordinate<ValueType> > & target_points);    
    void setInitialEstimation(const Matrix & transformation);
    void setEstimationAlgorithm(EstimateTransformation3D<ValueType> & estimator);
    void setMaximumNumberIterations(unsigned number);
    void setMaximumRMSE(ValueType value);
    void setEpsApproximate(float value);

    virtual ValueType compute() = 0; // returns the RMSE

    void abortComputation();

    const Matrix & getTransformation();

    Signal<unsigned> progress;  

protected:
    unsigned dimension;                                         

    const std::vector<Coordinate<ValueType> > * source_points;  

    flann::Matrix<ValueType> target_points;                     
    flann::Matrix<ValueType> query_point;                       
    flann::Matrix<ValueType> distances;                         
    flann::Matrix<int> indices;                                 
    flann::Index<flann::L2<ValueType> > * tree;                 
    float eps;                                                  

    EstimateTransformation3D<ValueType> * estimator;            

    unsigned maximum_number_iterations;                         

    ValueType maximum_rms_error;                                

    Matrix transformation;                                      

    volatile bool abort;                                        
};


template<class ValueType>
IcpBase<ValueType>::IcpBase(unsigned dimension) :
    dimension(dimension),
    tree(NULL),
    eps(0),
    estimator(NULL),
    maximum_number_iterations(20),
    maximum_rms_error(0),
    abort(false)
{
}


template<class ValueType>
IcpBase<ValueType>::~IcpBase()
{
    if (tree != NULL) delete tree;

    delete[] target_points.ptr();
    delete[] query_point.ptr();
    delete[] distances.ptr();
    delete[] indices.ptr();
}


template<class ValueType>
void IcpBase<ValueType>::abortComputation()
{
    abort = true;
}


template<class ValueType>
inline const typename IcpBase<ValueType>::Matrix & IcpBase<ValueType>::getTransformation()
{
    return transformation;
}


template<class ValueType>
void IcpBase<ValueType>::setEpsApproximate(float value)
{
    assert(eps >= 0);
    eps = value;
}


template<class ValueType>
void IcpBase<ValueType>::setEstimationAlgorithm(EstimateTransformation3D<ValueType> & estimator)
{
    this->estimator = &estimator;
}


template<class ValueType>
void IcpBase<ValueType>::setInitialEstimation(const Matrix & transformation)
{
    this->transformation = transformation;
}


template<class ValueType>
void IcpBase<ValueType>::setMaximumNumberIterations(unsigned number)
{
    maximum_number_iterations = number;
}


template<class ValueType>
void IcpBase<ValueType>::setMaximumRMSE(ValueType value)
{
    maximum_rms_error = value;
}


template<class ValueType>
void IcpBase<ValueType>::setSourcePoints(const std::vector<Coordinate<ValueType> > & source_points)
{
    this->source_points = &source_points;
}


template<class ValueType>
void IcpBase<ValueType>::setTargetPoints(const std::vector<Coordinate<ValueType> > & target_points)
{
    if (target_points.size() == 0)
    {
        throw std::runtime_error("IcpBase::setTargetPoints(): The input data may not be emtpy.");
    }

    // Copy the target points.

    const unsigned number_points = target_points.size();

    delete[] this->target_points.ptr();
    this->target_points = flann::Matrix<ValueType>(new ValueType[number_points * dimension], number_points, dimension);

    for (unsigned i = 0; i < number_points; ++i)
    {
        for (unsigned j = 0; j < dimension; ++j)
        {
            this->target_points[i][j] = target_points[i][j];
        }
    }

    // Setup tree.

    if (tree != NULL) delete tree;

    tree = new flann::Index<flann::L2<ValueType> >(this->target_points, flann::KDTreeSingleIndexParams());
    tree->buildIndex();
}


//                               Icp3D                                //


template <class ValueType>
class Icp3D : public IcpBase<ValueType>
{
public:
    Icp3D();
    ValueType compute();

private:
    typedef IcpBase<ValueType> super;

    using super::abort;
    using super::distances;
    using super::estimator;
    using super::indices;
    using super::maximum_number_iterations;
    using super::maximum_rms_error;
    using super::query_point;
    using super::source_points;
    using super::target_points;
    using super::transformation;
    using super::tree;
};


template<class ValueType>
Icp3D<ValueType>::Icp3D() : IcpBase<ValueType>(3)
{
    transformation = Transform3D<ValueType>().getTransformationMatrix();

    distances = flann::Matrix<ValueType>(new ValueType[1], 1, 1);
    indices = flann::Matrix<int>(new int[1], 1, 1);
    query_point = flann::Matrix<ValueType>(new ValueType[3], 1, 3);
}


template<class ValueType>
ValueType Icp3D<ValueType>::compute()
{
    if (tree->size() == 0)
    {
        throw std::range_error("Icp3D::compute(): No target points are given.");
    }

    // Initialize some variables.

    using std::ceil;
    using std::max;
    using std::min;

    abort = false;

    const unsigned size = source_points->size();

    std::vector<Coordinate<ValueType> > transformed_source_points;
    std::vector<Coordinate<ValueType> > target_points;

    Coordinate<ValueType> target_point(0, 0, 0);

    estimator->setSourcePoints(*source_points);

    std::vector<ValueType> rmse(maximum_number_iterations); // used to compute the progress

    // Iterate.

    unsigned i = 0;

    while (i < maximum_number_iterations && !abort)
    {
        transformed_source_points.clear();
        transformed_source_points.reserve(size);

        target_points.clear();
        target_points.reserve(size);

        // Transform the source points with the actual transformation estimation
        // and search for the corresponding points in the target set.

        Transform3D<ValueType> transform(transformation);

        for (unsigned n = 0; n < size; ++n)
        {
            Coordinate<ValueType> transformed_source_point = transform * (*source_points)[n];

            transformed_source_points.push_back(transformed_source_point);

            const Coordinate<ValueType> & point = transformed_source_points[n];

            query_point[0][0] = point.x();
            query_point[0][1] = point.y();
            query_point[0][2] = point.z();

            tree->knnSearch(query_point, indices, distances, 1, flann::SearchParams(128, super::eps));

            target_point.x() = super::target_points[indices[0][0]][0];
            target_point.y() = super::target_points[indices[0][0]][1];
            target_point.z() = super::target_points[indices[0][0]][2];

            target_points.push_back(target_point);
        }

        // Estimate a new transformation.

        estimator->setTargetPoints(target_points);
        estimator->compute();

        transformation = estimator->getTransformationMatrix();

        // Compute the actual progress [0, ..., 100] using a simple heuristic.
        //
        // 1.  A lower bound is given by the current iteration and the maximum
        //     number of iterations.
        //
        // 2.  The convergence rate of the ICP is assumed to be linear. Estimate
        //     the iteration N when the RMS will be smaller than the given root
        //     mean square error bound. Note: N cannot be greater than the
        //     maximum number of iterations.

        unsigned progress = 0;

        unsigned lower_bound = unsigned(100.0 * (i + 1) / maximum_number_iterations);

        rmse[i] = estimator->getRMS();

        if (i > 0)
        {
            // rmse_bound > y = m * x + b,   m := (rmse[i] - rmse[0]) / i,   m < 0,   b := rmse[0]
            // ==> N := ceil(x) = ceil((rmse_bound - rmse[0]) / ((rmse[i] - rmse[0]) / i))

            using std::numeric_limits;
            ValueType epsilon = numeric_limits<ValueType>::epsilon();

            ValueType m = (rmse[i] - rmse[0]) / i - epsilon;
            ValueType N = (maximum_rms_error - rmse[0]) / m;
            N = N > 0 ? ceil(N) : 0;

            progress = N > 0 ? unsigned(100.0 * i / N) : 100; //if the current RMS is lower than the given error bound (last case), we are done (progress = 100%)
            progress = min<unsigned>(progress, 100); // progress can at most be 100
        }

        progress = max(lower_bound, progress);

        super::progress.send(progress);

        // Iterate + stop criterion.

        if (rmse[i++] < maximum_rms_error)
        {
            if (progress < 100)
            {
                super::progress.send(100);
            }

            break;
        }
    }

    return estimator->getRMS();
}


//                       IcpTrimmed3D                                 //



template <class ValueType>
class IcpTrimmed3D : public IcpBase<ValueType>
{
public:
    IcpTrimmed3D();
    ValueType compute();

    void setMinimumOverlap(ValueType value);


private:
    typedef IcpBase<ValueType> super;

    using super::abort;
    using super::distances;
    using super::estimator;
    using super::indices;
    using super::maximum_number_iterations;
    using super::maximum_rms_error;
    using super::query_point;
    using super::source_points;
    using super::target_points;
    using super::transformation;
    using super::tree;

    ValueType m_minimumOverlap;
};

template<class ValueType>
IcpTrimmed3D<ValueType>::IcpTrimmed3D() :
    IcpBase<ValueType>(3),
    m_minimumOverlap(0.3)
{
    transformation = Transform3D<ValueType>().getTransformationMatrix();

    distances = flann::Matrix<ValueType>(new ValueType[1], 1, 1);
    indices = flann::Matrix<int>(new int[1], 1, 1);
    query_point = flann::Matrix<ValueType>(new ValueType[3], 1, 3);
}


template<class ValueType>
ValueType IcpTrimmed3D<ValueType>::compute()
{
    if (tree->size() == 0)
    {
        throw std::range_error("IcpTrimmed3D::compute(): No target points are given.");
    }

    // Initialize some variables.

    using std::ceil;
    using std::max;
    using std::min;

    abort = false;

    const unsigned size = source_points->size();

    std::vector<Coordinate<ValueType> > transformed_source_points;
    std::vector<Coordinate<ValueType> > trimmed_source_points;
    std::vector<Coordinate<ValueType> > target_points;
    std::vector<Coordinate<ValueType> > trimmed_target_points;
    std::vector<ValueType>              individual_distances;

    Coordinate<ValueType> target_point(0, 0, 0);

    //estimator->setSourcePoints(*source_points);

    std::vector<ValueType> rmse(maximum_number_iterations); // used to compute the progress

    // Iterate.

    unsigned i = 0;

    while (i < maximum_number_iterations && !abort)
    {
        transformed_source_points.clear();
        transformed_source_points.reserve(size);

        trimmed_source_points.clear();
        trimmed_source_points.reserve(size);

        target_points.clear();
        target_points.reserve(size);

        individual_distances.clear();
        individual_distances.reserve(size);

        // Transform the source points with the actual transformation estimation
        // and search for the corresponding points in the target set.

        Transform3D<ValueType> transform(transformation);

        for (unsigned n = 0; n < size; ++n)
        {
            Coordinate<ValueType> transformed_source_point = transform * (*source_points)[n];

            transformed_source_points.push_back(transformed_source_point);

            const Coordinate<ValueType> & point = transformed_source_points[n];

            query_point[0][0] = point.x();
            query_point[0][1] = point.y();
            query_point[0][2] = point.z();

            tree->knnSearch(query_point, indices, distances, 1, flann::SearchParams(128, super::eps));

            target_point.x() = super::target_points[indices[0][0]][0];
            target_point.y() = super::target_points[indices[0][0]][1];
            target_point.z() = super::target_points[indices[0][0]][2];

            // compute distance of transformed source and target
            ValueType d = (transformed_source_point - target_point).squaredNorm();
            individual_distances.push_back(d);
            trimmed_source_points.push_back((*source_points)[n]);
            target_points.push_back(target_point);
        }

        estimator->setSourcePoints(trimmed_source_points);
        estimator->setTargetPoints(target_points);

        // sorting takes too long for large point clouds
        // use approximation instead

        if (m_minimumOverlap < 1)
        {
            // find maximumAllowedDistance:
            // find minimum and maximum
            double maxDistance = 0;
            double minDistance = std::numeric_limits<double>::max();

            if (m_minimumOverlap > 0.5)
            {
                for (unsigned n = 0; n < size; ++n)
                {
                    if (individual_distances[n]>maxDistance) maxDistance = individual_distances[n];
                    if (individual_distances[n]<minDistance) minDistance = individual_distances[n];
                }
            }
            else
            {
                // improve accuracy: consider distances smaller than the average only
                for (unsigned n = 0; n < size; ++n)
                {
                    maxDistance += individual_distances[n];
                    if (individual_distances[n]<minDistance) minDistance = individual_distances[n];
                }
                maxDistance /= size;
            }

            // find maximum allowed distance
            double maximumAllowedDistance;
            double minimumNumPoints = m_minimumOverlap*size;

            double distanceDistribution[10000];
            for (int i = 0; i<10000; i++)
            {
                distanceDistribution[i] = 0;
            }
            for (unsigned n = 0; n < size; ++n)
            {
                if (individual_distances[n]<=maxDistance)
                {
                    distanceDistribution[(int) ((individual_distances[n]-minDistance)/(maxDistance-minDistance)*9999)]++;
                }
            }
            double distanceDistributionAccumulated[10000];
            distanceDistributionAccumulated[0] = distanceDistribution[0];
            int i;
            for (i = 1; i<10000; i++)
            {
                distanceDistributionAccumulated[i] = distanceDistributionAccumulated[i-1] + distanceDistribution[i];
                if (distanceDistributionAccumulated[i] >= minimumNumPoints)
                    break;
            }
            maximumAllowedDistance = ((double) i+0.5)/9999 * (maxDistance-minDistance) + minDistance;

            trimmed_target_points.clear();
            trimmed_target_points.reserve(size);

            trimmed_source_points.clear();

            for (unsigned n = 0; n < size; ++n)
            {
                if (individual_distances[n]<=maximumAllowedDistance)
                {
                    trimmed_source_points.push_back((*source_points)[n]);
                    trimmed_target_points.push_back(target_points[n]);
                }
            }

            estimator->setSourcePoints(trimmed_source_points);
            estimator->setTargetPoints(trimmed_target_points);
        }

         // Estimate a new transformation.
        estimator->compute();

        transformation = estimator->getTransformationMatrix();

        // Compute the actual progress [0, ..., 100] using a simple heuristic.
        //
        // 1.  A lower bound is given by the current iteration and the maximum
        //     number of iterations.
        //
        // 2.  The convergence rate of the ICP is assumed to be linear. Estimate
        //     the iteration N when the RMS will be smaller than the given root
        //     mean square error bound. Note: N cannot be greater than the
        //     maximum number of iterations.

        unsigned progress = 0;

        unsigned lower_bound = unsigned(100.0 * (i + 1) / maximum_number_iterations);

        rmse[i] = estimator->getRMS();

        if (i > 0)
        {
            // rmse_bound > y = m * x + b,   m := (rmse[i] - rmse[0]) / i,   m < 0,   b := rmse[0]
            // ==> N := ceil(x) = ceil((rmse_bound - rmse[0]) / ((rmse[i] - rmse[0]) / i))

            using std::numeric_limits;
            ValueType epsilon = numeric_limits<ValueType>::epsilon();

            ValueType m = (rmse[i] - rmse[0]) / i - epsilon;
            ValueType N = (maximum_rms_error - rmse[0]) / m;
            N = N > 0 ? ceil(N) : 0;

            progress = N > 0 ? unsigned(100.0 * i / N) : 100; //if the current RMS is lower than the given error bound (last case), we are done (progress = 100%)
            progress = min<unsigned>(progress, 100); // progress can at most be 100
        }

        progress = max(lower_bound, progress);

        super::progress.send(progress);

        // Iterate + stop criterion.

        if (rmse[i++] < maximum_rms_error)
        {
            if (progress < 100)
            {
                super::progress.send(100);
            }

            break;
        }
    }

    return estimator->getRMS();
}


template<class ValueType>
void IcpTrimmed3D<ValueType>::setMinimumOverlap(ValueType value)
{
    m_minimumOverlap = value;
}


//                         RandomSampleIcp3D                          //


template <class ValueType>
class RandomSampleIcp3D : public IcpBase<ValueType>
{
public:
    RandomSampleIcp3D();
    ValueType compute();                                            

    void setMaximumNumberIterationsFirstStage(unsigned value);      
    void setMaximumNumberIterationsSecondStage(unsigned value);     
    void setPercentage(unsigned value);                             

private:
    typedef IcpBase<ValueType> super;
    typedef typename super::Matrix Matrix;

    using super::abort;
    using super::distances;
    using super::estimator;
    using super::indices;
    using super::maximum_number_iterations;
    using super::maximum_rms_error;
    using super::query_point;
    using super::source_points;
    using super::target_points;
    using super::transformation;
    using super::tree;

    unsigned maximum_number_iterations_first_stage;
    unsigned percentage;
};


template<class ValueType>
RandomSampleIcp3D<ValueType>::RandomSampleIcp3D() :
    IcpBase<ValueType>(3),
    maximum_number_iterations_first_stage(20),
    percentage(50)
{
    transformation = Transform3D<ValueType>().getTransformationMatrix();

    distances = flann::Matrix<ValueType>(new ValueType[1], 1, 1);
    indices = flann::Matrix<int>(new int[1], 1, 1);
    query_point = flann::Matrix<ValueType>(new ValueType[3], 1, 3);

    maximum_number_iterations = 20;
}


template<class ValueType>
ValueType RandomSampleIcp3D<ValueType>::compute()
{
    if (tree->size() == 0)
    {
        throw std::range_error("RandomSampleIcp3D::compute(): No target points are given.");
    }

    // Initialize some variables.

    using std::ceil;
    using std::max;
    using std::min;
    using std::numeric_limits;

    abort = false;

    const unsigned size = source_points->size();

    std::vector<Coordinate<ValueType> > source_points; // note, this hides the class member
    std::vector<Coordinate<ValueType> > transformed_source_points;
    std::vector<Coordinate<ValueType> > target_points;

    Coordinate<ValueType> target_point(0, 0, 0);

    Matrix best_estimate = transformation;

    ValueType least_rmse = numeric_limits<ValueType>::max();

    unsigned maximum_number_iterations_per_stage = max(maximum_number_iterations, maximum_number_iterations_first_stage);
    unsigned maximum_number_iterations_overall = maximum_number_iterations + maximum_number_iterations_first_stage;

    std::vector<ValueType> rmse(maximum_number_iterations_per_stage); // used to compute the progress

    ValueType ratio = ValueType(maximum_number_iterations_first_stage) / ValueType(maximum_number_iterations_overall);

    // PHASE 1 --> Randomly selected subsets are taken to "feed" the classical ICP algorithm.

    // Iterate.

    unsigned i = 0;

    while (i < maximum_number_iterations_first_stage && !abort)
    {
        source_points.clear();
        source_points.reserve(size);

        transformed_source_points.clear();
        transformed_source_points.reserve(size);

        target_points.clear();
        target_points.reserve(size);

        // Transform the source points with the actual transformation estimation
        // and search for the corresponding points in the target set.

        Transform3D<ValueType> transform(transformation);

        for (unsigned n = 0; n < size; ++n)
        {
            if (Tools::rand(0u, 100u) > percentage) continue; // randomly select points

            Coordinate<ValueType> source_point = (*this->source_points)[n];

            source_points.push_back(source_point);

            Coordinate<ValueType> transformed_source_point = transform * source_point;

            transformed_source_points.push_back(transformed_source_point);

            const Coordinate<ValueType> & point = transformed_source_point;

            query_point[0][0] = point.x();
            query_point[0][1] = point.y();
            query_point[0][2] = point.z();

            tree->knnSearch(query_point, indices, distances, 1, flann::SearchParams(128, super::eps));

            target_point.x() = super::target_points[indices[0][0]][0];
            target_point.y() = super::target_points[indices[0][0]][1];
            target_point.z() = super::target_points[indices[0][0]][2];

            target_points.push_back(target_point);
        }

        // Estimate a new transformation.

        estimator->setSourcePoints(source_points);
        estimator->setTargetPoints(target_points);
        estimator->compute();

        transformation = estimator->getTransformationMatrix();

        // Store the estimation if its RMSE is lesser than a previous one.

        if (estimator->getRMS() < least_rmse)
        {
            least_rmse = estimator->getRMS();
            best_estimate = estimator->getTransformationMatrix();
        }

        // Compute the actual progress [0, ..., 100] using a simple heuristic.
        //
        // 1.  A lower bound is given by the current iteration and the maximum
        //     number of iterations.
        //
        // 2.  The convergence rate of the ICP is assumed to be linear. Estimate
        //     the iteration N when the RMS will be smaller than the given root
        //     mean square error bound. Note: N cannot be greater than the
        //     maximum number of iterations.

        unsigned progress = 0;

        unsigned lower_bound = unsigned(100.0 * (i + 1) / maximum_number_iterations_first_stage);

        rmse[i] = estimator->getRMS();

        if (i > 0)
        {
            // rmse_bound > y = m * x + b,   m := (rmse[i] - rmse[0]) / i,   m < 0,   b := rmse[0]
            // ==> N := ceil(x) = ceil((rmse_bound - rmse[0]) / ((rmse[i] - rmse[0]) / i))

            ValueType epsilon = numeric_limits<ValueType>::epsilon();

            ValueType m = (rmse[i] - rmse[0]) / i - epsilon;
            ValueType N = (maximum_rms_error - rmse[0]) / m;
            N = N > 0 ? ceil(N) : 0;

            progress = N > 0 ? unsigned(100.0 * i / N) : 100; //if the current RMS is lower than the given error bound (last case), we are done (progress = 100%)
            progress = min<unsigned>(progress, 100); // progress can at most be 100
        }

        progress = max(lower_bound, progress);

        super::progress.send(unsigned(progress * ratio));

        // Iterate + stop criterion.

        if (rmse[i++] < maximum_rms_error)
        {
            if (progress < 100)
            {
                super::progress.send(unsigned(100 * ratio));
            }

            break;
        }
    }

    // Use the best transformation estimate for the next phase.

    transformation = best_estimate;

    // Clean up some no longer used variables.

    source_points.clear();
    source_points.reserve(0);

    // PHASE 2

    estimator->setSourcePoints(*this->source_points);

    // Iterate.

    i = 0;

    while (i < maximum_number_iterations && !abort)
    {
        transformed_source_points.clear();
        transformed_source_points.reserve(size);

        target_points.clear();
        target_points.reserve(size);

        // Transform the source points with the actual transformation estimation
        // and search for the corresponding points in the target set.

        Transform3D<ValueType> transform(transformation);

        for (unsigned n = 0; n < size; ++n)
        {
            Coordinate<ValueType> transformed_source_point = transform * (*this->source_points)[n];

            transformed_source_points.push_back(transformed_source_point);

            const Coordinate<ValueType> & point = transformed_source_points[n];

            query_point[0][0] = point.x();
            query_point[0][1] = point.y();
            query_point[0][2] = point.z();

            tree->knnSearch(query_point, indices, distances, 1, flann::SearchParams(128, super::eps));

            target_point.x() = super::target_points[indices[0][0]][0];
            target_point.y() = super::target_points[indices[0][0]][1];
            target_point.z() = super::target_points[indices[0][0]][2];

            target_points.push_back(target_point);
        }

        // Estimate a new transformation.

        estimator->setTargetPoints(target_points);
        estimator->compute();

        transformation = estimator->getTransformationMatrix();

        // Compute the actual progress [0, ..., 100] using a simple heuristic.
        //
        // 1.  A lower bound is given by the current iteration and the maximum
        //     number of iterations.
        //
        // 2.  The convergence rate of the ICP is assumed to be linear. Estimate
        //     the iteration N when the RMS will be smaller than the given root
        //     mean square error bound. Note: N cannot be greater than the
        //     maximum number of iterations.

        unsigned progress = 0;

        unsigned lower_bound = unsigned(100.0 * (i + 1) / maximum_number_iterations);

        rmse[i] = estimator->getRMS();

        if (i > 0)
        {
            // rmse_bound > y = m * x + b,   m := (rmse[i] - rmse[0]) / i,   m < 0,   b := rmse[0]
            // ==> N := ceil(x) = ceil((rmse_bound - rmse[0]) / ((rmse[i] - rmse[0]) / i))

            ValueType epsilon = numeric_limits<ValueType>::epsilon();

            ValueType m = (rmse[i] - rmse[0]) / i - epsilon;
            ValueType N = (maximum_rms_error - rmse[0]) / m;
            N = N > 0 ? ceil(N) : 0;

            progress = N > 0 ? unsigned(100.0 * i / N) : 100; //if the current RMS is lower than the given error bound (last case), we are done (progress = 100%)
            progress = min<unsigned>(progress, 100); // progress can at most be 100
        }

        progress = max(lower_bound, progress);

        super::progress.send(unsigned(100 * ratio + progress * (1 - ratio)));

        // Iterate + stop criterion.

        if (rmse[i++] < maximum_rms_error)
        {
            if (progress < 100)
            {
                super::progress.send(100);
            }

            break;
        }
    }

    return estimator->getRMS();
}


template<class ValueType>
void RandomSampleIcp3D<ValueType>::setMaximumNumberIterationsFirstStage(unsigned value)
{
    maximum_number_iterations_first_stage = value;
}


template<class ValueType>
void RandomSampleIcp3D<ValueType>::setMaximumNumberIterationsSecondStage(unsigned value)
{
    maximum_number_iterations = value;
}


template<class ValueType>
void RandomSampleIcp3D<ValueType>::setPercentage(unsigned value)
{
    assert(percentage <= 100);
    percentage = std::min(value, 100u);
}


} // namespace TRTK


#endif // ICP_HPP_7908314317