ICP.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "ICP.h"
 
// IVT
#include <cmath>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <DataStructures/KdTree/KdTree.h>
#include <Tracking/ICP.h>
 
using namespace visionx;
 
ICP::ICP()
{
    SetParameters();
    m_pKdTree = new CKdTree();
 
    m_nNumScenePoints = 0;
    m_pValues = new float*[1];
}
 
ICP::~ICP()
{
    delete m_pKdTree;
 
    for (int i = 0; i < m_nNumScenePoints; i++)
    {
        delete[] m_pValues[i];
    }
 
    delete[] m_pValues;
}
 
void
ICP::SetScenePointcloud(const std::vector<Eigen::Vector3f>& aScenePoints)
{
    // clean up
    m_aScenePoints.clear();
 
    for (int i = 0; i < m_nNumScenePoints; i++)
    {
        delete[] m_pValues[i];
    }
 
    delete[] m_pValues;
    delete m_pKdTree;
 
    // copy points
    const int nSize = aScenePoints.size();
 
    for (int i = 0; i < nSize; i++)
    {
        m_aScenePoints.push_back(aScenePoints.at(i));
    }
 
    m_pKdTree = new CKdTree(nSize);
 
    m_pValues = new float*[nSize];
    m_nNumScenePoints = nSize;
 
    for (int i = 0; i < nSize; i++)
    {
        m_pValues[i] = new float[3];
        m_pValues[i][0] = aScenePoints.at(i)[0];
        m_pValues[i][1] = aScenePoints.at(i)[1];
        m_pValues[i][2] = aScenePoints.at(i)[2];
    }
 
    m_pKdTree->Build(m_pValues, 0, nSize - 1, m_nKdTreeBucketSize, 3, 0);
}
 
void
ICP::SetObjectPointcloud(const std::vector<Eigen::Vector3f>& aObjectPoints)
{
    m_aObjectPoints.clear();
 
    for (size_t i = 0; i < aObjectPoints.size(); i++)
    {
        m_aObjectPoints.push_back(aObjectPoints.at(i));
    }
}
 
float
ICP::SearchObject(Mat3d& mRotation, Vec3d& vTranslation, const float fBestDistanceUntilNow)
{
    Math3d::SetMat(mRotation, Math3d::unit_mat);
    Math3d::SetVec(vTranslation, 0, 0, 0);
 
    if (m_aObjectPoints.size() < 3)
    {
        ARMARX_WARNING_S << "CColorICP::SearchObject: not enough object points!";
        return -1;
    }
 
    if (m_aScenePoints.size() < 3)
    {
        ARMARX_WARNING_S << "CColorICP::SearchObject: not enough scene points!";
        return -1;
    }
 
    float fLastDistance = FLT_MAX;
    float fDistance = 0, fPointDistance;
    Vec3d vPoint, vPointTransformed;
    float* pPointData = new float[3];
    float* pNearestNeighbor;
    Vec3d* pPointPositions = new Vec3d[m_aObjectPoints.size()];
    Vec3d* pCorrespondingPointPositions = new Vec3d[m_aObjectPoints.size()];
    int nNumPointCorrespondences;
 
    // iterations of the ICP algorithm
    for (int n = 0; n < m_nMaxIterations; n++)
    {
        fDistance = 0;
        nNumPointCorrespondences = 0;
 
        // find nearest neighbor for each point in XYZRGB space
        for (size_t i = 0; i < m_aObjectPoints.size(); i++)
        {
            Math3d::SetVec(vPoint,
                           m_aObjectPoints.at(i)[0],
                           m_aObjectPoints.at(i)[1],
                           m_aObjectPoints.at(i)[2]);
            Math3d::MulMatVec(mRotation, vPoint, vTranslation, vPointTransformed);
 
            pPointData[0] = vPointTransformed.x;
            pPointData[1] = vPointTransformed.y;
            pPointData[2] = vPointTransformed.z;
 
            m_pKdTree->NearestNeighborBBF(pPointData, fPointDistance, pNearestNeighbor);
 
            fPointDistance = sqrtf(fPointDistance);
 
            // if the correspondence is too far away, we ignore it in the estimation of the transformation
            if (fPointDistance > m_fCutoffDistance)
            {
                fDistance += m_fCutoffDistance;
            }
            else
            {
                fDistance += fPointDistance;
                Math3d::SetVec(pPointPositions[nNumPointCorrespondences], vPoint);
                Math3d::SetVec(pCorrespondingPointPositions[nNumPointCorrespondences],
                               pNearestNeighbor[0],
                               pNearestNeighbor[1],
                               pNearestNeighbor[2]);
                nNumPointCorrespondences++;
            }
        }
 
        if (nNumPointCorrespondences < 3)
        {
            ARMARX_WARNING_S << "CColorICP::SearchObject: not enough correspondences found!";
            return -1;
        }
 
        // re-estimate the transformation between the two pointclouds using the correspondences
        CICP::CalculateOptimalTransformation(pPointPositions,
                                             pCorrespondingPointPositions,
                                             nNumPointCorrespondences,
                                             mRotation,
                                             vTranslation);
 
 
        // if the improvement is very small, finish
 
        if (0.8f * fDistance < fBestDistanceUntilNow)
        {
            // if it seems we might get a new best result, quit only if improvement is very very small
            if (1.0f - fDistance / fLastDistance < 0.01f * m_fConvergenceDelta)
            {
                break;
            }
        }
        else if ((1.0f - fDistance / fLastDistance < m_fConvergenceDelta) ||
                 (2 * n > m_nMaxIterations))
        {
            // otherwise quit if improvement is below threshold or already many iterations
            break;
        }
        else if ((0.5f * fDistance > fBestDistanceUntilNow) && (3 * n > m_nMaxIterations))
        {
            // if we dont seem to get close to the best result, quit earlier
            break;
        }
 
        fLastDistance = fDistance;
 
        //ARMARX_WARNING_S << "%.1f  ", fDistance/m_aObjectPoints.size());
    }
 
    delete[] pPointData;
    delete[] pPointPositions;
    delete[] pCorrespondingPointPositions;
 
    return fDistance / m_aObjectPoints.size();
}
 
void
ICP::SetParameters(float fCutoffDistance,
                   float fConvergenceDelta,
                   int nMaxIterations,
                   int nKdTreeBucketSize)
{
    m_fCutoffDistance = fCutoffDistance;
    m_fConvergenceDelta = fConvergenceDelta;
    m_nMaxIterations = nMaxIterations;
    m_nKdTreeBucketSize = nKdTreeBucketSize;
}
 
void
ICP::GetPointMatchDistances(std::vector<float>& aPointMatchDistances)
{
    float* pPointData = new float[3];
    float* pNearestNeighbor;
    float fDistance;
 
    for (size_t i = 0; i < m_aObjectPoints.size(); i++)
    {
        pPointData[0] = m_aObjectPoints.at(i)[0];
        pPointData[1] = m_aObjectPoints.at(i)[1];
        pPointData[2] = m_aObjectPoints.at(i)[2];
 
        m_pKdTree->NearestNeighborBBF(pPointData, fDistance, pNearestNeighbor);
 
        aPointMatchDistances.push_back(fDistance);
    }
 
    delete[] pPointData;
}
 
void
ICP::GetNearestNeighbors(std::vector<Eigen::Vector3f>& aNeighbors,
                         std::vector<float>& aPointMatchDistances)
{
    float* pPointData = new float[3];
    float* pNearestNeighbor;
    Eigen::Vector3f pNearestNeighborPoint;
    float fDistance;
 
    for (size_t i = 0; i < m_aObjectPoints.size(); i++)
    {
        pPointData[0] = m_aObjectPoints.at(i)[0];
        pPointData[1] = m_aObjectPoints.at(i)[1];
        pPointData[2] = m_aObjectPoints.at(i)[2];
 
        m_pKdTree->NearestNeighborBBF(pPointData, fDistance, pNearestNeighbor);
 
        pNearestNeighborPoint[0] = pNearestNeighbor[0];
        pNearestNeighborPoint[1] = pNearestNeighbor[1];
        pNearestNeighborPoint[2] = pNearestNeighbor[2];
 
        aNeighbors.push_back(pNearestNeighborPoint);
        aPointMatchDistances.push_back(fDistance);
    }
 
    delete[] pPointData;
}