ColorICP.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 "ColorICP.h"
 
#include "ObjectLearningByPushingDefinitions.h"
 
// IVT
#include <cmath>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <DataStructures/KdTree/KdTree.h>
#include <Tracking/ICP.h>
 
CColorICP::CColorICP()
{
    SetParameters();
    m_pKdTree = new CKdTree();
 
    m_nNumScenePoints = 0;
    m_pValues = new float*[1];
}
 
CColorICP::~CColorICP()
{
    delete m_pKdTree;
 
    for (int i = 0; i < m_nNumScenePoints; i++)
    {
        delete[] m_pValues[i];
    }
 
    delete[] m_pValues;
}
 
void
CColorICP::SetScenePointcloud(std::vector<CColorICP::CPointXYZRGBI> 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[7];
        m_pValues[i][0] = aScenePoints.at(i).x;
        m_pValues[i][1] = aScenePoints.at(i).y;
        m_pValues[i][2] = aScenePoints.at(i).z;
        m_pValues[i][3] = m_fColorWeight * aScenePoints.at(i).r;
        m_pValues[i][4] = m_fColorWeight * aScenePoints.at(i).g;
        m_pValues[i][5] = m_fColorWeight * aScenePoints.at(i).b;
        m_pValues[i][6] = m_fColorWeight * aScenePoints.at(i).i;
    }
 
    bool bUseIntensity = OLP_EFFORT_POINTCLOUD_MATCHING > 0;
 
    if (bUseIntensity)
    {
        m_pKdTree->Build(m_pValues, 0, nSize - 1, m_nKdTreeBucketSize, 7, 0);
    }
    else
    {
        m_pKdTree->Build(m_pValues, 0, nSize - 1, m_nKdTreeBucketSize, 6, 1);
    }
}
 
float
CColorICP::SearchObject(const std::vector<CPointXYZRGBI>& aObjectPoints,
                        Mat3d& mRotation,
                        Vec3d& vTranslation,
                        const float fBestDistanceUntilNow)
{
    Math3d::SetMat(mRotation, Math3d::unit_mat);
    Math3d::SetVec(vTranslation, 0, 0, 0);
 
    if (aObjectPoints.size() < 3)
    {
        ARMARX_VERBOSE_S << "CColorICP::SearchObject: not enough object points!";
        return FLT_MAX;
    }
 
    if (m_aScenePoints.size() < 3)
    {
        ARMARX_WARNING_S << "CColorICP::SearchObject: not enough scene points!";
        return FLT_MAX;
    }
 
    float fLastDistance = FLT_MAX;
    float fDistance = 0, fPointDistance;
    Vec3d vPoint, vPointTransformed;
    float* pPointData = new float[7];
    float* pNearestNeighbor;
    Vec3d* pPointPositions = new Vec3d[aObjectPoints.size()];
    Vec3d* pCorrespondingPointPositions = new Vec3d[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 < aObjectPoints.size(); i++)
        {
            Math3d::SetVec(
                vPoint, aObjectPoints.at(i).x, aObjectPoints.at(i).y, aObjectPoints.at(i).z);
            Math3d::MulMatVec(mRotation, vPoint, vTranslation, vPointTransformed);
 
            pPointData[0] = vPointTransformed.x;
            pPointData[1] = vPointTransformed.y;
            pPointData[2] = vPointTransformed.z;
            pPointData[3] = m_fColorWeight * aObjectPoints.at(i).r;
            pPointData[4] = m_fColorWeight * aObjectPoints.at(i).g;
            pPointData[5] = m_fColorWeight * aObjectPoints.at(i).b;
            pPointData[6] = m_fColorWeight * aObjectPoints.at(i).i;
 
            m_pKdTree->NearestNeighborBBF(pPointData, fPointDistance, pNearestNeighbor);
            //FindNNBruteForce(pPointData, fPointDistance, pNearestNeighbor);
 
            fPointDistance = sqrtf(fPointDistance);
 
            //            if (i%10==0)
            //            {
            //                ARMARX_WARNING_S << "(%.1f %.1f %.1f  %.1f %.1f %.1f) , (%.1f %.1f %.1f  %.1f %.1f %.1f) :  %.1f\n",
            //                       pPointData[0], pPointData[1], pPointData[2], pPointData[3], pPointData[4], pPointData[5],
            //                       pNearestNeighbor[0], pNearestNeighbor[1], pNearestNeighbor[2], pNearestNeighbor[3], pNearestNeighbor[4], pNearestNeighbor[5], 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_IMPORTANT_S << "CColorICP::SearchObject: less than 3 correspondences found - no "
                                  "transformation determined";
            return FLT_MAX;
        }
 
        // 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/aObjectPoints.size());
    }
 
    delete[] pPointData;
    delete[] pPointPositions;
    delete[] pCorrespondingPointPositions;
 
    return fDistance / aObjectPoints.size();
}
 
void
CColorICP::SetParameters(float fColorWeight,
                         float fCutoffDistance,
                         float fConvergenceDelta,
                         int nMaxIterations,
                         int nKdTreeBucketSize)
{
    m_fColorWeight = fColorWeight;
    m_fCutoffDistance = fCutoffDistance;
    m_fConvergenceDelta = fConvergenceDelta;
    m_nMaxIterations = nMaxIterations;
    m_nKdTreeBucketSize = nKdTreeBucketSize;
}
 
void
CColorICP::FindNNBruteForce(const float* pPoint, float& fSquaredDistance, float*& pNeighbor)
{
    float fMinDist = FLT_MAX;
    float fDist;
    int nBestIndex = -1;
 
    for (int i = 0; i < m_nNumScenePoints; i++)
    {
        fDist = 0;
 
        for (int j = 0; j < 6; j++)
        {
            fDist += (pPoint[j] - m_pValues[i][j]) * (pPoint[j] - m_pValues[i][j]);
        }
 
        if (fDist < fMinDist)
        {
            fMinDist = fDist;
            nBestIndex = i;
        }
    }
 
    fSquaredDistance = fMinDist;
    pNeighbor = m_pValues[nBestIndex];
}
 
void
CColorICP::GetPointMatchDistances(const std::vector<CPointXYZRGBI>& aObjectPoints,
                                  std::vector<float>& aPointMatchDistances)
{
    float* pPointData = new float[7];
    float* pNearestNeighbor;
    float fDistance;
 
    for (size_t i = 0; i < aObjectPoints.size(); i++)
    {
        pPointData[0] = aObjectPoints.at(i).x;
        pPointData[1] = aObjectPoints.at(i).y;
        pPointData[2] = aObjectPoints.at(i).z;
        pPointData[3] = m_fColorWeight * aObjectPoints.at(i).r;
        pPointData[4] = m_fColorWeight * aObjectPoints.at(i).g;
        pPointData[5] = m_fColorWeight * aObjectPoints.at(i).b;
        pPointData[6] = m_fColorWeight * aObjectPoints.at(i).i;
 
        m_pKdTree->NearestNeighborBBF(pPointData, fDistance, pNearestNeighbor);
 
        aPointMatchDistances.push_back(fDistance);
    }
 
    delete[] pPointData;
}
 
void
CColorICP::GetNearestNeighbors(const std::vector<CPointXYZRGBI>& aObjectPoints,
                               std::vector<CColorICP::CPointXYZRGBI>& aNeighbors,
                               std::vector<float>& aPointMatchDistances)
{
    float* pPointData = new float[7];
    float* pNearestNeighbor;
    CColorICP::CPointXYZRGBI pNearestNeighborPoint;
    float fDistance;
    const float fColorWeightFactor = 1.0f / m_fColorWeight;
 
    for (size_t i = 0; i < aObjectPoints.size(); i++)
    {
        pPointData[0] = aObjectPoints.at(i).x;
        pPointData[1] = aObjectPoints.at(i).y;
        pPointData[2] = aObjectPoints.at(i).z;
        pPointData[3] = m_fColorWeight * aObjectPoints.at(i).r;
        pPointData[4] = m_fColorWeight * aObjectPoints.at(i).g;
        pPointData[5] = m_fColorWeight * aObjectPoints.at(i).b;
        pPointData[6] = m_fColorWeight * aObjectPoints.at(i).i;
 
        m_pKdTree->NearestNeighborBBF(pPointData, fDistance, pNearestNeighbor);
 
        pNearestNeighborPoint.x = pNearestNeighbor[0];
        pNearestNeighborPoint.y = pNearestNeighbor[1];
        pNearestNeighborPoint.z = pNearestNeighbor[2];
        pNearestNeighborPoint.r = fColorWeightFactor * pNearestNeighbor[3];
        pNearestNeighborPoint.g = fColorWeightFactor * pNearestNeighbor[4];
        pNearestNeighborPoint.b = fColorWeightFactor * pNearestNeighbor[5];
        pNearestNeighborPoint.i = fColorWeightFactor * pNearestNeighbor[6];
 
        aNeighbors.push_back(pNearestNeighborPoint);
        aPointMatchDistances.push_back(fDistance);
    }
 
    delete[] pPointData;
}
 
CColorICP::CPointXYZRGBI
CColorICP::ConvertToXYZRGBI(CHypothesisPoint* pPoint)
{
    CColorICP::CPointXYZRGBI ret;
    ret.x = pPoint->vPosition.x;
    ret.y = pPoint->vPosition.y;
    ret.z = pPoint->vPosition.z;
    ret.r = pPoint->fColorR;
    ret.g = pPoint->fColorG;
    ret.b = pPoint->fColorB;
    ret.i = pPoint->fIntensity;
 
    return ret;
}
 
CHypothesisPoint*
CColorICP::ConvertFromXYZRGBI(CPointXYZRGBI pPoint)
{
    CHypothesisPoint* pRet = new CHypothesisPoint();
    pRet->ePointType = CHypothesisPoint::eDepthMapPoint;
    Math3d::SetVec(pRet->vPosition, pPoint.x, pPoint.y, pPoint.z);
    pRet->fColorR = pPoint.r;
    pRet->fColorG = pPoint.g;
    pRet->fColorB = pPoint.b;
    pRet->fIntensity = pPoint.i;
    pRet->fMembershipProbability = 0.8f;
    return pRet;
}
 
void
CColorICP::TransformPointXYZRGBI(CPointXYZRGBI& pPoint, Mat3d mRotation, Vec3d vTranslation)
{
    const float x = pPoint.x;
    const float y = pPoint.y;
    const float z = pPoint.z;
    pPoint.x = mRotation.r1 * x + mRotation.r2 * y + mRotation.r3 * z + vTranslation.x;
    pPoint.y = mRotation.r4 * x + mRotation.r5 * y + mRotation.r6 * z + vTranslation.y;
    pPoint.z = mRotation.r7 * x + mRotation.r8 * y + mRotation.r9 * z + vTranslation.z;
}