ICPPointAlign.cpp

Go to the documentation of this file.

 
#include "lvr2/registration/ICPPointAlign.hpp"
#include "lvr2/registration/EigenSVDPointAlign.hpp"
#include "lvr2/io/Timestamp.hpp"
 
#include <iomanip>
#include <chrono>
 
using namespace std;
 
namespace lvr2
{
 
ICPPointAlign::ICPPointAlign(SLAMScanPtr model, SLAMScanPtr data) :
    m_modelCloud(model), m_dataCloud(data)
{
    // Init default values
    m_maxDistanceMatch  = 25;
    m_maxIterations     = 50;
    m_epsilon           = 0.00001;
    m_verbose           = false;
 
    m_searchTree = KDTree::create(model, m_maxLeafSize);
}
 
Transformd ICPPointAlign::match()
{
    if (m_maxIterations == 0)
    {
        return Matrix4d::Identity();
    }
 
    auto start_time = chrono::steady_clock::now();
 
    double ret = 0.0, prev_ret = 0.0, prev_prev_ret = 0.0;
    EigenSVDPointAlign<double> align;
    int iteration = 0;
 
    Vector3d centroid_m = Vector3d::Zero();
    Vector3d centroid_d = Vector3d::Zero();
    Transformd transform = Matrix4d::Identity();
    Transformd delta = Matrix4d::Identity();
 
    size_t numPoints = m_dataCloud->numPoints();
 
    KDTree::Neighbor* neighbors = new KDTree::Neighbor[numPoints];
 
    for (iteration = 0; iteration < m_maxIterations; iteration++)
    {
        // Update break variables
        prev_prev_ret = prev_ret;
        prev_ret = ret;
 
        // Get point pairs
        size_t pairs = KDTree::nearestNeighbors(m_searchTree, m_dataCloud, neighbors, m_maxDistanceMatch, centroid_m, centroid_d);
 
        // Get transformation
        transform = Transformd::Identity();
        ret = align.alignPoints(m_dataCloud, neighbors, centroid_m, centroid_d, transform);
 
        // Apply transformation
        m_dataCloud->transform(transform, false);
        delta = delta * transform;
 
        if (m_verbose)
        {
            cout << timestamp << "ICP Error is " << ret << " in iteration " << iteration << " / " << m_maxIterations << " using " << pairs << " points." << endl;
        }
 
        // Check minimum distance
        if ((fabs(ret - prev_ret) < m_epsilon) && (fabs(ret - prev_prev_ret) < m_epsilon))
        {
            break;
        }
    }
 
    delete[] neighbors;
 
    auto duration = chrono::steady_clock::now() - start_time;
    cout << setw(6) << (int)(duration.count() / 1e6) << " ms, ";
    cout << "Error: " << fixed << setprecision(3) << setw(7) << ret;
    if (iteration < m_maxIterations)
    {
        cout << " after " << iteration << " Iterations";
    }
    cout << endl;
    if (m_verbose)
    {
        cout << "Result: " << endl << m_dataCloud->deltaPose() << endl;
    }
 
    return delta;
}
 
void ICPPointAlign::setMaxMatchDistance(double d)
{
    m_maxDistanceMatch = d;
}
 
void ICPPointAlign::setMaxIterations(int i)
{
    m_maxIterations = i;
}
 
void ICPPointAlign::setMaxLeafSize(int m)
{
    m_maxLeafSize = m;
}
 
void ICPPointAlign::setEpsilon(double e)
{
    m_epsilon = e;
}
void ICPPointAlign::setVerbose(bool verbose)
{
    m_verbose = verbose;
}
 
double ICPPointAlign::getMaxMatchDistance() const
{
    return m_maxDistanceMatch;
}
 
int ICPPointAlign::getMaxIterations() const
{
    return m_maxIterations;
}
 
int ICPPointAlign::getMaxLeafSize() const
{
    return m_maxLeafSize;
}
 
double ICPPointAlign::getEpsilon() const
{
    return m_epsilon;
}
 
bool ICPPointAlign::getVerbose() const
{
    return m_verbose;
}
 
} /* namespace lvr2 */