PointCloudRegistration.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 "PointCloudRegistration.h"
 
#include "OLPTools.h"
 
// OpenMP
#include <omp.h>
 
// system
#include <sys/time.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
CPointCloudRegistration::CPointCloudRegistration()
{
    // create parallel instances of CColorICP
    const int nParallelityFactor = omp_get_num_procs();
    m_pColorICPInstances = new CColorICP*[nParallelityFactor];
 
    for (int i = 0; i < nParallelityFactor; i++)
    {
        m_pColorICPInstances[i] = new CColorICP();
        m_pColorICPInstances[i]->SetParameters(OLP_ICP_COLOR_DISTANCE_WEIGHT,
                                               OLP_ICP_CUTOFF_DISTANCE);
    }
}
 
CPointCloudRegistration::~CPointCloudRegistration()
{
    const int nParallelityFactor = omp_get_num_procs();
 
    for (int i = 0; i < nParallelityFactor; i++)
    {
        delete m_pColorICPInstances[i];
    }
 
    delete m_pColorICPInstances;
}
 
// set the point cloud in which the object should be searched
void
CPointCloudRegistration::SetScenePointcloud(const std::vector<CHypothesisPoint*>& aScenePoints)
{
    // convert points to simple format for ICP
    std::vector<CColorICP::CPointXYZRGBI> aScenePointsConverted;
    aScenePointsConverted.resize(aScenePoints.size());
 
    for (size_t i = 0; i < aScenePoints.size(); i++)
    {
        aScenePointsConverted.at(i) = CColorICP::ConvertToXYZRGBI(aScenePoints.at(i));
    }
 
    // update the parallel instances of CColorICP
    const int nParallelityFactor = omp_get_num_procs();
 
    for (int i = 0; i < nParallelityFactor; i++)
    {
        m_pColorICPInstances[i]->SetScenePointcloud(aScenePointsConverted);
    }
}
 
float
CPointCloudRegistration::EstimateTransformation(
    const std::vector<CHypothesisPoint*>& aObjectPoints,
    const Vec3d center,
    Mat3d& mRotation,
    Vec3d& vTranslation,
    const Vec3d upwardsVector,
    const int nEffortLevel,
    const std::vector<Vec3d>* pPossibleLocationOffsets,
    std::vector<CColorICP::CPointXYZRGBI>* pNearestNeighbors,
    std::vector<float>* pPointMatchDistances,
    const std::vector<CHypothesisPoint*>* pAdditionalPoints,
    const float maxAcceptedDistance)
{
    timeval tStart, tEnd;
    gettimeofday(&tStart, 0);
    const int nParallelityFactor = omp_get_num_procs();
    omp_set_num_threads(nParallelityFactor);
 
    // create grid of initial positions around the possible object locations
    std::vector<Vec3d> aPossibleLocationOffsets;
    aPossibleLocationOffsets.push_back(center); // original position
 
    if (pPossibleLocationOffsets)
    {
        for (size_t i = 0; i < pPossibleLocationOffsets->size(); i++)
        {
            aPossibleLocationOffsets.push_back(pPossibleLocationOffsets->at(i));
        }
    }
 
    const int nNumPossibleLocations = aPossibleLocationOffsets.size();
 
    ARMARX_VERBOSE_S << "Trying to match object around " << nNumPossibleLocations
                     << " different possible locations";
 
    for (size_t i = 0; i < aPossibleLocationOffsets.size(); i++)
    {
        //ARMARX_VERBOSE_S << aPossibleLocationOffsets.at(i).x << "  " << aPossibleLocationOffsets.at(i).y << "  " << aPossibleLocationOffsets.at(i).z;
    }
 
 
    float fShiftTemp;
    int nNumSamplesPerDimensionTemp;
 
    switch (nEffortLevel)
    {
        case 0:
        case 1:
        case 2:
            fShiftTemp = 0;
            nNumSamplesPerDimensionTemp = 1;
            break;
 
        case 3:
            fShiftTemp = 100;
            nNumSamplesPerDimensionTemp = 2;
            break;
 
        case 4:
            fShiftTemp = 70;
            nNumSamplesPerDimensionTemp = 3;
            break;
 
        case 5:
            fShiftTemp = 80;
            nNumSamplesPerDimensionTemp = 4;
            break;
 
        case 6:
            fShiftTemp = 70;
            nNumSamplesPerDimensionTemp = 5;
            break;
 
        default:
            fShiftTemp = 70;
            nNumSamplesPerDimensionTemp = 6;
            break;
    }
 
    bool tryDifferentOrientations = (Math3d::Length(upwardsVector) > 0) && (nEffortLevel >= 2);
    const int nNumOrientations = tryDifferentOrientations ? 3 : 1;
    Mat3d baseRotationMatrices[3];
    Math3d::SetMat(baseRotationMatrices[0], Math3d::unit_mat);
    Math3d::SetRotationMat(baseRotationMatrices[1], upwardsVector, -20.0 * M_PI / 180.0);
    Math3d::SetRotationMat(baseRotationMatrices[2], upwardsVector, 20.0 * M_PI / 180.0);
 
    const float fShift = fShiftTemp; // unit shift values for the sample points
    const int nNumSamplesPerDimension = nNumSamplesPerDimensionTemp;
    const int nNumSamplesPerDimension2 = nNumSamplesPerDimension * nNumSamplesPerDimension;
    const int nNumSamplesPerDimension3 =
        nNumSamplesPerDimension * nNumSamplesPerDimension * nNumSamplesPerDimension;
    const int nNumSamples = nNumOrientations * nNumSamplesPerDimension3 * nNumPossibleLocations;
    const float fMiddle = 0.5f * (nNumSamplesPerDimension - 1);
    Vec3d* pShiftVectors = new Vec3d[nNumSamples];
    Mat3d* rotationMatrices = new Mat3d[nNumSamples];
 
    for (int n = 0; n < nNumPossibleLocations; n++)
    {
        const Vec3d vLocationOffset = aPossibleLocationOffsets.at(n);
 
        for (int i = 0; i < nNumSamplesPerDimension; i++)
        {
            for (int j = 0; j < nNumSamplesPerDimension; j++)
            {
                for (int k = 0; k < nNumSamplesPerDimension; k++)
                {
                    for (int l = 0; l < nNumOrientations; l++)
                    {
                        const int nIndex = n * nNumSamplesPerDimension3 * nNumOrientations +
                                           i * nNumSamplesPerDimension2 * nNumOrientations +
                                           j * nNumSamplesPerDimension * nNumOrientations +
                                           k * nNumOrientations + l;
                        pShiftVectors[nIndex].x = vLocationOffset.x + (i - fMiddle) * fShift;
                        pShiftVectors[nIndex].y = vLocationOffset.y + (j - fMiddle) * fShift;
                        pShiftVectors[nIndex].z = vLocationOffset.z + (k - fMiddle) * fShift;
                        Math3d::SetMat(rotationMatrices[nIndex], baseRotationMatrices[l]);
                    }
                }
            }
        }
    }
 
 
    // downsample object cloud if necessary
    size_t maxNumPoints = 200 + nEffortLevel * 200;
    std::vector<CHypothesisPoint*> aObjectPointsDownsampled;
 
    if (aObjectPoints.size() > maxNumPoints)
    {
        const float ratio = (float)maxNumPoints / (float)aObjectPoints.size();
        const int randomThreshold = 1000.0f * ratio;
 
        for (size_t i = 0; i < aObjectPoints.size(); i++)
        {
            if (rand() % 1000 <= randomThreshold)
            {
                aObjectPointsDownsampled.push_back(aObjectPoints.at(i));
            }
        }
 
        ARMARX_VERBOSE_S << "Object pointcloud was downsampled from " << aObjectPoints.size()
                         << " to " << aObjectPointsDownsampled.size() << " points";
    }
    else
    {
        aObjectPointsDownsampled = aObjectPoints;
    }
 
 
    // try ICP with different start positions
 
    Mat3d mBestRotation = Math3d::unit_mat;
    Mat3d mSecondBestRotation = Math3d::unit_mat;
    Vec3d vBestTranslation = {0, 0, 0};
    Vec3d vSecondBestTranslation = {0, 0, 0};
    float fBestDistance = FLT_MAX;
    float fSecondBestDistance = FLT_MAX;
    std::vector<float> distances;
    std::vector<Vec3d> positions;
    int numSamplesAlreadyDone = 0;
 
#pragma omp parallel for schedule(dynamic, 1)
    for (int k = 0; k < nNumSamples; k++)
    {
        const int nThreadNumber = omp_get_thread_num();
 
        std::vector<CColorICP::CPointXYZRGBI> objectPointCloudPar;
        objectPointCloudPar.resize(aObjectPointsDownsampled.size());
        CColorICP::CPointXYZRGBI vPointPar;
 
        for (size_t i = 0; i < aObjectPointsDownsampled.size(); i++)
        {
            vPointPar = CColorICP::ConvertToXYZRGBI(aObjectPointsDownsampled.at(i));
            vPointPar.x -= center.x;
            vPointPar.y -= center.y;
            vPointPar.z -= center.z;
            objectPointCloudPar.at(i) = vPointPar;
        }
 
        if (tryDifferentOrientations)
        {
            RotatePoints(objectPointCloudPar, rotationMatrices[k]);
        }
 
        for (size_t i = 0; i < objectPointCloudPar.size(); i++)
        {
            objectPointCloudPar.at(i).x += pShiftVectors[k].x;
            objectPointCloudPar.at(i).y += pShiftVectors[k].y;
            objectPointCloudPar.at(i).z += pShiftVectors[k].z;
        }
 
        Mat3d newRotation, newRotationGlobal;
        Vec3d newTranslation, newTranslationGlobal;
        float fNewObjectDistance = m_pColorICPInstances[nThreadNumber]->SearchObject(
            objectPointCloudPar,
            newRotation,
            newTranslation,
            std::min(fBestDistance, maxAcceptedDistance));
 
        GetCompleteTransformation(newRotation,
                                  newTranslation,
                                  rotationMatrices[k],
                                  pShiftVectors[k],
                                  center,
                                  newRotationGlobal,
                                  newTranslationGlobal);
 
#pragma omp critical
        {
            distances.push_back(fNewObjectDistance);
            positions.push_back(newTranslationGlobal);
 
            // if better solution found, save it
            if (fNewObjectDistance < fBestDistance)
            {
                fSecondBestDistance = fBestDistance;
                Math3d::SetMat(mSecondBestRotation, mBestRotation);
                Math3d::SetVec(vSecondBestTranslation, vBestTranslation);
 
                fBestDistance = fNewObjectDistance;
                Math3d::SetMat(mBestRotation, newRotationGlobal);
                Math3d::SetVec(vBestTranslation, newTranslationGlobal);
            }
            else if (fNewObjectDistance < fSecondBestDistance)
            {
                fSecondBestDistance = fNewObjectDistance;
                Math3d::SetMat(mSecondBestRotation, newRotationGlobal);
                Math3d::SetVec(vSecondBestTranslation, newTranslationGlobal);
            }
 
            numSamplesAlreadyDone++;
 
            if (numSamplesAlreadyDone % (3 * nNumSamplesPerDimension3) == 0)
            {
                ARMARX_VERBOSE_S << (int)((float)numSamplesAlreadyDone * 100 / (float)nNumSamples)
                                 << "% done";
            }
        }
    }
 
 
    // apply best found transformation, remove object points that do not match in the new scene, and do one more iteration of ICP
 
    Vec3d vRefinementTranslation, vTemp;
    Mat3d mRefinementRotation;
 
 
    // convert the points belonging to the object
    CColorICP::CPointXYZRGBI vPoint;
    std::vector<CColorICP::CPointXYZRGBI> objectPointCloudConverted;
 
    for (size_t i = 0; i < aObjectPoints.size(); i++)
    {
        Math3d::MulMatVec(mBestRotation, aObjectPoints.at(i)->vPosition, vBestTranslation, vTemp);
        vPoint.x = vTemp.x;
        vPoint.y = vTemp.y;
        vPoint.z = vTemp.z;
        vPoint.r = aObjectPoints.at(i)->fColorR;
        vPoint.g = aObjectPoints.at(i)->fColorG;
        vPoint.b = aObjectPoints.at(i)->fColorB;
        vPoint.i = aObjectPoints.at(i)->fIntensity;
        objectPointCloudConverted.push_back(vPoint);
    }
 
    // determine "bad" points
    std::vector<float> aPointMatchDistances;
    m_pColorICPInstances[0]->GetPointMatchDistances(objectPointCloudConverted,
                                                    aPointMatchDistances);
 
    // ICP using only the good points
    objectPointCloudConverted.clear();
 
    for (size_t i = 0; i < aObjectPoints.size(); i++)
    {
        if (aPointMatchDistances.at(i) < OLP_TOLERANCE_CONCURRENT_MOTION)
        {
            Math3d::MulMatVec(
                mBestRotation, aObjectPoints.at(i)->vPosition, vBestTranslation, vTemp);
            vPoint.x = vTemp.x;
            vPoint.y = vTemp.y;
            vPoint.z = vTemp.z;
            vPoint.r = aObjectPoints.at(i)->fColorR;
            vPoint.g = aObjectPoints.at(i)->fColorG;
            vPoint.b = aObjectPoints.at(i)->fColorB;
            vPoint.i = aObjectPoints.at(i)->fIntensity;
            objectPointCloudConverted.push_back(vPoint);
        }
    }
 
    m_pColorICPInstances[0]->SearchObject(
        objectPointCloudConverted, mRefinementRotation, vRefinementTranslation, fBestDistance);
 
    // save the result
    // rotation is Rot * initial rotation
    Mat3d mOldBestRot;
    Math3d::SetMat(mOldBestRot, mBestRotation);
    Math3d::MulMatMat(mRefinementRotation, mOldBestRot, mBestRotation);
    // translation is Rot*shift+trans
    Vec3d vOldBestTransl;
    Math3d::SetVec(vOldBestTransl, vBestTranslation);
    Math3d::MulMatVec(
        mRefinementRotation, vOldBestTransl, vRefinementTranslation, vBestTranslation);
 
 
    // get the point match distances for the resulting transformation
    if (pPointMatchDistances && pNearestNeighbors)
    {
        Vec3d vTemp;
        objectPointCloudConverted.clear();
 
        for (size_t i = 0; i < aObjectPoints.size(); i++)
        {
            Math3d::MulMatVec(
                mBestRotation, aObjectPoints.at(i)->vPosition, vBestTranslation, vTemp);
            vPoint.x = vTemp.x;
            vPoint.y = vTemp.y;
            vPoint.z = vTemp.z;
            vPoint.r = aObjectPoints.at(i)->fColorR;
            vPoint.g = aObjectPoints.at(i)->fColorG;
            vPoint.b = aObjectPoints.at(i)->fColorB;
            vPoint.i = aObjectPoints.at(i)->fIntensity;
            objectPointCloudConverted.push_back(vPoint);
        }
 
        if (pAdditionalPoints)
        {
            for (size_t i = 0; i < pAdditionalPoints->size(); i++)
            {
                Math3d::MulMatVec(
                    mBestRotation, pAdditionalPoints->at(i)->vPosition, vBestTranslation, vTemp);
                vPoint.x = vTemp.x;
                vPoint.y = vTemp.y;
                vPoint.z = vTemp.z;
                vPoint.r = pAdditionalPoints->at(i)->fColorR;
                vPoint.g = pAdditionalPoints->at(i)->fColorG;
                vPoint.b = pAdditionalPoints->at(i)->fColorB;
                vPoint.i = pAdditionalPoints->at(i)->fIntensity;
                objectPointCloudConverted.push_back(vPoint);
            }
        }
 
        pNearestNeighbors->clear();
        pPointMatchDistances->clear();
        m_pColorICPInstances[0]->GetNearestNeighbors(
            objectPointCloudConverted, *pNearestNeighbors, *pPointMatchDistances);
    }
 
 
    // set return values
    Math3d::SetMat(mRotation, mBestRotation);
    Math3d::SetVec(vTranslation, vBestTranslation);
 
 
    // compare best distance to bad matches
    std::vector<float> badMatchDistances;
 
    for (size_t i = 0; i < distances.size(); i++)
    {
        float dist = Math3d::Distance(vBestTranslation, positions.at(i));
 
        if (dist > 300)
        {
            badMatchDistances.push_back(distances.at(i));
        }
    }
 
    float averageBadMatchDistance = 0;
 
    for (size_t i = 0; i < badMatchDistances.size(); i++)
    {
        averageBadMatchDistance += badMatchDistances.at(i);
    }
 
    if (badMatchDistances.size() > 0)
    {
        averageBadMatchDistance /= badMatchDistances.size();
    }
 
 
    ARMARX_VERBOSE_S << "Distance: " << fBestDistance
                     << "  average bad match distance: " << averageBadMatchDistance;
 
 
    // clean up
    delete[] pShiftVectors;
    delete[] rotationMatrices;
 
 
    gettimeofday(&tEnd, 0);
    long tTimeDiff =
        (1000 * tEnd.tv_sec + tEnd.tv_usec / 1000) - (1000 * tStart.tv_sec + tStart.tv_usec / 1000);
    ARMARX_VERBOSE_S << "Time for pointcloud registration: " << tTimeDiff << " ms";
 
 
    return fBestDistance;
}
 
bool
CPointCloudRegistration::CheckObjectMatchAtOriginalPosition(const CObjectHypothesis* pHypothesis,
                                                            float& distance,
                                                            const int nEffortLevel)
{
    // get the object points
    std::vector<CHypothesisPoint*> aObjectPoints;
 
    for (size_t i = 0; i < pHypothesis->aConfirmedPoints.size(); i++)
    {
        aObjectPoints.push_back(pHypothesis->aConfirmedPoints.at(i));
    }
 
    // dont use too many candidate points
    const size_t nNewPointsLimit = (pHypothesis->aConfirmedPoints.size() > OLP_MIN_NUM_FEATURES)
                                       ? 3 * pHypothesis->aConfirmedPoints.size() / 2
                                       : pHypothesis->aNewPoints.size();
 
    for (size_t i = 0; i < pHypothesis->aNewPoints.size() && i < nNewPointsLimit; i++)
    {
        aObjectPoints.push_back(pHypothesis->aNewPoints.at(i));
    }
 
    // downsample object cloud if necessary
    size_t maxNumPoints = 200 + nEffortLevel * 200;
    std::vector<CHypothesisPoint*> aObjectPointsDownsampled;
 
    if (aObjectPoints.size() > maxNumPoints)
    {
        const float ratio = (float)maxNumPoints / (float)aObjectPoints.size();
        const int randomThreshold = 1000.0f * ratio;
 
        for (size_t i = 0; i < aObjectPoints.size(); i++)
        {
            if (rand() % 1000 <= randomThreshold)
            {
                aObjectPointsDownsampled.push_back(aObjectPoints.at(i));
            }
        }
 
        ARMARX_VERBOSE_S << "Object pointcloud was downsampled from " << aObjectPoints.size()
                         << " to " << aObjectPointsDownsampled.size() << " points";
    }
    else
    {
        aObjectPointsDownsampled = aObjectPoints;
    }
 
    // convert the points belonging to the object
    std::vector<CColorICP::CPointXYZRGBI> objectPointCloudConverted;
 
    for (size_t i = 0; i < aObjectPointsDownsampled.size(); i++)
    {
        objectPointCloudConverted.push_back(
            CColorICP::ConvertToXYZRGBI(aObjectPointsDownsampled.at(i)));
    }
 
    Mat3d mRotation;
    Vec3d vTranslation;
    distance =
        m_pColorICPInstances[0]->SearchObject(objectPointCloudConverted, mRotation, vTranslation);
 
 
    // check if the object seems to have moved
    Vec3d vObjectCenter = {0, 0, 0};
 
    for (size_t i = 0; i < aObjectPointsDownsampled.size(); i++)
    {
        Math3d::AddToVec(vObjectCenter, aObjectPointsDownsampled.at(i)->vPosition);
    }
 
    Math3d::MulVecScalar(vObjectCenter, 1.0f / aObjectPointsDownsampled.size(), vObjectCenter);
    Vec3d vTemp1;
    Math3d::MulMatVec(mRotation, vObjectCenter, vTranslation, vTemp1);
    float fTranslationLength = Math3d::Distance(vTemp1, vObjectCenter);
 
    // check for rotation
    float fAngle;
    Math3d::GetAxisAndAngle(mRotation, vTemp1, fAngle);
 
 
    bool bRet = (fTranslationLength < 0.33f * OLP_MINIMAL_MOTION_MEASURE) &&
                (distance < 0.33f * OLP_TOLERANCE_CONCURRENT_MOTION) && (fAngle < 10 * M_PI / 180);
 
    ARMARX_VERBOSE_S << "Check at original position: Translation: " << fTranslationLength
                     << "  Rotation: " << 180 * fAngle / M_PI << "deg";
 
    return bRet;
}
 
/*
bool CPointCloudRegistration::EstimateTransformationPCL(const std::vector<CHypothesisPoint*>& aOldPoints, const std::vector<CHypothesisPoint*>& aNewPoints, Mat3d& mRotation, Vec3d& vTranslation)
{
#ifdef OLP_USE_PCL
 
    // copy points to PCL pointclouds
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr aOldPointsPCL (new pcl::PointCloud<pcl::PointXYZRGBA>);
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr aNewPointsPCL (new pcl::PointCloud<pcl::PointXYZRGBA>);
 
    aOldPointsPCL->resize(aOldPoints.size());
    aNewPointsPCL->resize(aNewPoints.size());
    for (int i=0; i<(int)aOldPoints.size(); i++)
    {
        aOldPointsPCL->points[i].x = aOldPoints.at(i)->vPosition.x/1000;    // convert to meters
        aOldPointsPCL->points[i].y = aOldPoints.at(i)->vPosition.y/1000;
        aOldPointsPCL->points[i].z = aOldPoints.at(i)->vPosition.z/1000;
        aOldPointsPCL->points[i].r = (int)(255*aOldPoints.at(i)->fIntensity*aOldPoints.at(i)->fColorR);
        aOldPointsPCL->points[i].g = (int)(255*aOldPoints.at(i)->fIntensity*aOldPoints.at(i)->fColorG);
        aOldPointsPCL->points[i].b = (int)(255*aOldPoints.at(i)->fIntensity*aOldPoints.at(i)->fColorB);
        aOldPointsPCL->points[i].a = 1;
    }
    for (int i=0; i<(int)aNewPoints.size(); i++)
    {
        aNewPointsPCL->points[i].x = aNewPoints.at(i)->vPosition.x/1000;
        aNewPointsPCL->points[i].y = aNewPoints.at(i)->vPosition.y/1000;
        aNewPointsPCL->points[i].z = aNewPoints.at(i)->vPosition.z/1000;
        aNewPointsPCL->points[i].r = (int)(255*aNewPoints.at(i)->fIntensity*aNewPoints.at(i)->fColorR);
        aNewPointsPCL->points[i].g = (int)(255*aNewPoints.at(i)->fIntensity*aNewPoints.at(i)->fColorG);
        aNewPointsPCL->points[i].b = (int)(255*aNewPoints.at(i)->fIntensity*aNewPoints.at(i)->fColorB);
        aNewPointsPCL->points[i].a = 1;
    }
 
    // i.e. this is an unorganized pointcloud
    aOldPointsPCL->width = aOldPoints.size();
    aOldPointsPCL->height = 1;
    // set to false if points may contain inf or nan values
    aOldPointsPCL->is_dense = true;
 
    // do ICP
    pcl::IterativeClosestPoint<pcl::PointXYZRGBA, pcl::PointXYZRGBA> icp;
    icp.setInputCloud(aOldPointsPCL);
    icp.setInputTarget(aNewPointsPCL);
    pcl::PointCloud<pcl::PointXYZRGBA> Final;
    icp.align(Final);
 
    bool bSuccess = icp.hasConverged();
    float fScore = icp.getFitnessScore();
    Eigen::Matrix4f mTrafo = icp.getFinalTransformation();
 
    mRotation.r1 = mTrafo(0,0);
    mRotation.r2 = mTrafo(0,1);
    mRotation.r3 = mTrafo(0,2);
    mRotation.r4 = mTrafo(1,0);
    mRotation.r5 = mTrafo(1,1);
    mRotation.r6 = mTrafo(1,2);
    mRotation.r7 = mTrafo(2,0);
    mRotation.r8 = mTrafo(2,1);
    mRotation.r9 = mTrafo(2,2);
 
    vTranslation.x = mTrafo(0,3);
    vTranslation.y = mTrafo(1,3);
    vTranslation.z = mTrafo(2,3);
 
    cout << "Success: " << bSuccess << " Score:  " << fScore << "   "; // << "   Transformation:" << endl << mTrafo << endl;
 
    return bSuccess;
#else
    return false;
#endif
}
*/
 
 
float
CPointCloudRegistration::EstimateTransformation(
    const CObjectHypothesis* pHypothesis,
    Mat3d& mRotation,
    Vec3d& vTranslation,
    const Vec3d upwardsVector,
    const int nEffortLevel,
    const std::vector<Vec3d>* pPossibleLocations,
    std::vector<CColorICP::CPointXYZRGBI>* pNearestNeighbors,
    std::vector<float>* pPointMatchDistances,
    const float maxAcceptedDistance)
{
#ifdef OLP_USE_DEPTH_MAP
 
    // hypothesis points that will be used for matching
    std::vector<CHypothesisPoint*> aHypothesisPoints = pHypothesis->aConfirmedPoints;
 
 
    // fill up with candidate points, but dont use too many
    size_t maxNumPoints = 200 + nEffortLevel * 200;
    size_t nNewPointsLimit;
 
    if (aHypothesisPoints.size() >= maxNumPoints)
    {
        nNewPointsLimit = 0;
    }
    else
    {
        nNewPointsLimit = maxNumPoints - aHypothesisPoints.size();
 
        if ((pHypothesis->aConfirmedPoints.size() > OLP_MIN_NUM_FEATURES) &&
            (pHypothesis->aConfirmedPoints.size() < nNewPointsLimit))
        {
            nNewPointsLimit = pHypothesis->aConfirmedPoints.size();
        }
    }
 
    for (size_t i = 0; i < pHypothesis->aNewPoints.size() && i < nNewPointsLimit; i++)
    {
        aHypothesisPoints.push_back(pHypothesis->aNewPoints.at(i));
    }
 
 
    // additional points that will not be used for matching
    std::vector<CHypothesisPoint*> aAdditionalPoints;
 
    for (size_t i = nNewPointsLimit; i < pHypothesis->aNewPoints.size(); i++)
    {
        aAdditionalPoints.push_back(pHypothesis->aNewPoints.at(i));
    }
 
    for (size_t i = 0; i < pHypothesis->aDoubtablePoints.size(); i++)
    {
        aAdditionalPoints.push_back(pHypothesis->aDoubtablePoints.at(i));
    }
 
 
    float fDistance = EstimateTransformation(aHypothesisPoints,
                                             pHypothesis->vCenter,
                                             mRotation,
                                             vTranslation,
                                             upwardsVector,
                                             nEffortLevel,
                                             pPossibleLocations,
                                             pNearestNeighbors,
                                             pPointMatchDistances,
                                             &aAdditionalPoints,
                                             maxAcceptedDistance);
 
    //Vec3d vTemp1, vTemp2;
    //Math3d::MulMatVec(mRotation, pHypothesis->vCenter, vTemp1);
    //Math3d::MulVecScalar(vTranslation, 1, vTemp2);
    //Math3d::AddToVec(vTemp1, vTemp2);
    //float fDist = Math3d::Distance(vTemp1, pHypothesis->vCenter);
    //ARMARX_VERBOSE_S << "Translation: (%.1f, %.1f, %.1f), length %.1f mm\n\n", (vTemp1.x-pHypothesis->vCenter.x), (vTemp1.y-pHypothesis->vCenter.y), (vTemp1.z-pHypothesis->vCenter.z), fDist);
 
    return fDistance;
#else
    return FLT_MAX;
#endif
}
 
void
CPointCloudRegistration::RotatePoints(std::vector<CColorICP::CPointXYZRGBI>& points,
                                      const Mat3d rot)
{
    Vec3d center = {0, 0, 0};
    //    for (size_t i=0; i<points.size(); i++)
    //    {
    //        center.x += points.at(i).x;
    //        center.y += points.at(i).y;
    //        center.z += points.at(i).z;
    //    }
    //    if (points.size()>0)
    //    {
    //        center.x /= points.size();
    //        center.y /= points.size();
    //        center.z /= points.size();
    //    }
 
    for (size_t i = 0; i < points.size(); i++)
    {
        Vec3d point = {
            points.at(i).x - center.x, points.at(i).y - center.y, points.at(i).z - center.z};
        Vec3d pointRot;
        Math3d::MulMatVec(rot, point, pointRot);
        points.at(i).x = pointRot.x + center.x;
        points.at(i).y = pointRot.y + center.y;
        points.at(i).z = pointRot.z + center.z;
    }
}
 
void
CPointCloudRegistration::GetCompleteTransformation(Mat3d rotICP,
                                                   Vec3d transICP,
                                                   Mat3d rotShift,
                                                   Vec3d transShift,
                                                   Vec3d center,
                                                   Mat3d& completeRotation,
                                                   Vec3d& completeTranslation)
{
    Math3d::MulMatMat(rotICP, rotShift, completeRotation);
 
    Vec3d temp1, temp2;
    Math3d::MulMatVec(completeRotation, center, temp1);
    Math3d::MulMatVec(rotICP, transShift, temp2);
    Math3d::SubtractVecVec(temp2, temp1, completeTranslation);
    Math3d::AddToVec(completeTranslation, transICP);
}
 
CColorICP::CPointXYZRGBI
CPointCloudRegistration::ConvertPclToXYZRGBI(pcl::PointXYZRGBA point)
{
    CColorICP::CPointXYZRGBI result;
    result.x = point.x;
    result.y = point.y;
    result.z = point.z;
    float intensity = point.r + point.g + point.b + 3;
    float intensityInverse = 1.0f / intensity;
    result.r = intensityInverse * (point.r + 1);
    result.g = intensityInverse * (point.g + 1);
    result.b = intensityInverse * (point.b + 1);
    result.i = intensity / (3.0f * 256.0f);
    return result;
}