HypothesisValidationRGBD.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 "HypothesisValidationRGBD.h"
 
#include "ColorICP.h"
#include "OLPTools.h"
#include "ObjectRecognition.h"
#include "PointCloudRegistration.h"
 
 
// IVT
#include <cstdlib>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <Calibration/Calibration.h>
#include <Image/ByteImage.h>
#include <Math/Math3d.h>
 
void
CHypothesisValidationRGBD::ValidateInitialHypotheses(
    const std::vector<CHypothesisPoint*>& aOldDepthMapPoints,
    const std::vector<CHypothesisPoint*>& aNewDepthMapPoints,
    const CByteImage* pForegroundImage,
    const CByteImage* pHSVImage,
    const CCalibration* calibration,
    const Vec3d upwardsVector,
    CObjectHypothesisArray* pObjectHypotheses,
    CObjectHypothesisArray* pConfirmedHypotheses)
{
    // calculate foreground points
    std::vector<CHypothesisPoint*> aForegroundPoints;
    COLPTools::FilterForegroundPoints(
        aNewDepthMapPoints, pForegroundImage, calibration, aForegroundPoints);
    ARMARX_VERBOSE_S << aForegroundPoints.size() << " foreground points";
 
 
    // if something changed in the image, determine transformations and validate hypotheses
    if (aForegroundPoints.size() > 0)
    {
        // create hypotheses from points that changed
        GenerateAndAddForegroundHypotheses(
            aOldDepthMapPoints, pForegroundImage, calibration, pObjectHypotheses);
 
        // set up the point cloud registration
        CPointCloudRegistration* pPointCloudRegistration = new CPointCloudRegistration();
        pPointCloudRegistration->SetScenePointcloud(aNewDepthMapPoints);
 
        // validate all hypotheses
        for (int i = 0; i < pObjectHypotheses->GetSize(); i++)
        {
            // debug only for paper!
            //if ((*pObjectHypotheses)[i]->nHypothesisNumber > 40) continue;
 
            // check if the hypothesis is of type RGBD
            if (((*pObjectHypotheses)[i]->eType != CObjectHypothesis::eRGBD) &&
                ((*pObjectHypotheses)[i]->eType != CObjectHypothesis::eSingleColored) && false)
            {
                ARMARX_VERBOSE_S << "ValidateInitialHypothesis: Wrong hypothesis type (not RGBD or "
                                    "SingleColored). Not checking it for motion.";
            }
            else
            {
                // check hypothesis only if it lies in a region that changed
                float fForegroundRatio;
                int nForegroundPoints;
                COLPTools::CalculateForegroundRatioOfHypothesis((*pObjectHypotheses)[i],
                                                                pForegroundImage,
                                                                calibration,
                                                                fForegroundRatio,
                                                                nForegroundPoints);
 
                if (fForegroundRatio >
                    0.3f) // || 100*nForegroundPoints>OLP_IMG_WIDTH*OLP_IMG_HEIGHT)
                {
                    ARMARX_VERBOSE_S << "Estimating transformation of hypothesis "
                                     << (*pObjectHypotheses)[i]->nHypothesisNumber << " ("
                                     << 100 * fForegroundRatio << " % change, "
                                     << (*pObjectHypotheses)[i]->aNewPoints.size() << " points).";
 
                    CObjectHypothesis* pConfirmedHypothesis;
                    bool bValidated = ValidateHypothesis(pPointCloudRegistration,
                                                         aNewDepthMapPoints,
                                                         aForegroundPoints,
                                                         pForegroundImage,
                                                         pHSVImage,
                                                         calibration,
                                                         upwardsVector,
                                                         (*pObjectHypotheses)[i],
                                                         pConfirmedHypothesis,
                                                         OLP_EFFORT_POINTCLOUD_MATCHING - 1);
                    //bool bValidated = ValidateHypothesis(pPointCloudRegistration, aNewDepthMapPoints, aForegroundPoints, pForegroundImage, pHSVImage, calibration, upwardsVector, (*pObjectHypotheses)[i], pConfirmedHypothesis, OLP_EFFORT_POINTCLOUD_MATCHING);
 
                    if (bValidated)
                    {
                        pConfirmedHypotheses->AddElement(pConfirmedHypothesis);
                    }
                }
                else
                {
                    ARMARX_VERBOSE_S << "The image region containing hypothesis " << i
                                     << " did not change (" << 100 * fForegroundRatio << " %).";
                }
            }
        }
 
        delete pPointCloudRegistration;
    }
    else
    {
        ARMARX_VERBOSE_S << "No change in image detected!";
    }
}
 
void
CHypothesisValidationRGBD::RevalidateHypotheses(
    const std::vector<CHypothesisPoint*>& aNewDepthMapPoints,
    const CByteImage* pForegroundImage,
    const CByteImage* pHSVImage,
    const CCalibration* calibration,
    const Vec3d upwardsVector,
    CObjectHypothesisArray* pObjectHypotheses)
{
    // calculate foreground points
    std::vector<CHypothesisPoint*> aForegroundPoints;
    COLPTools::FilterForegroundPoints(
        aNewDepthMapPoints, pForegroundImage, calibration, aForegroundPoints);
    ARMARX_VERBOSE_S << aForegroundPoints.size() << " foreground points";
 
 
    // if something changed in the image, determine transformations and revalidate hypotheses
    if (aForegroundPoints.size() > 0)
    {
        // set up the point cloud registration
        CPointCloudRegistration* pPointCloudRegistration = new CPointCloudRegistration();
        pPointCloudRegistration->SetScenePointcloud(aNewDepthMapPoints);
 
        // validate all hypotheses
        for (int i = 0; i < pObjectHypotheses->GetSize(); i++)
        {
            // check hypothesis, no matter if it lies in a region that changed
            CObjectHypothesis* pObjectHypothesis = (*pObjectHypotheses)[i];
            ARMARX_VERBOSE_S << "Estimating transformation of hypothesis "
                             << pObjectHypothesis->nHypothesisNumber << " ("
                             << (*pObjectHypotheses)[i]->aConfirmedPoints.size() << " + "
                             << (*pObjectHypotheses)[i]->aNewPoints.size() << " points).";
            CObjectHypothesis* pConfirmedHypothesis;
            bool bValidated = ValidateHypothesis(pPointCloudRegistration,
                                                 aNewDepthMapPoints,
                                                 aForegroundPoints,
                                                 pForegroundImage,
                                                 pHSVImage,
                                                 calibration,
                                                 upwardsVector,
                                                 pObjectHypothesis,
                                                 pConfirmedHypothesis);
 
            if (bValidated)
            {
                (*pObjectHypotheses)[i] = pConfirmedHypothesis;
                delete pObjectHypothesis;
            }
            else
            {
                // just keep the hypothesis
            }
        }
 
        delete pPointCloudRegistration;
    }
    else
    {
        ARMARX_VERBOSE_S << "No change in image detected!";
    }
}
 
void
CHypothesisValidationRGBD::GenerateAndAddForegroundHypotheses(
    const std::vector<CHypothesisPoint*>& aOldDepthMapPoints,
    const CByteImage* pForegroundImage,
    const CCalibration* calibration,
    CObjectHypothesisArray* pObjectHypotheses)
{
    std::vector<CHypothesisPoint*> aForegroundPointsFromOldPoints;
    COLPTools::FilterForegroundPoints(
        aOldDepthMapPoints, pForegroundImage, calibration, aForegroundPointsFromOldPoints);
    ARMARX_VERBOSE_S << aForegroundPointsFromOldPoints.size()
                     << " points for foreground hypotheses";
 
    // cluster the points and create foreground hypotheses
    std::vector<std::vector<CHypothesisPoint*>> aForegroundPointsFromOldPointsClusters;
    COLPTools::ClusterXMeans(aForegroundPointsFromOldPoints,
                             2,
                             5,
                             OLP_CLUSTERING_FACTOR_FOREGROUND_HYPOTHESES,
                             aForegroundPointsFromOldPointsClusters);
    ARMARX_VERBOSE_S << aForegroundPointsFromOldPointsClusters.size() << " foreground hypotheses";
 
    for (size_t i = 0; i < aForegroundPointsFromOldPointsClusters.size(); i++)
    {
        COLPTools::RemoveOutliers(aForegroundPointsFromOldPointsClusters.at(i), 1.7f);
 
        if (aForegroundPointsFromOldPointsClusters.at(i).size() > OLP_MIN_NUM_FEATURES)
        {
            CObjectHypothesis* pForegroundHypo = new CObjectHypothesis();
            Vec3d vCenter = {0, 0, 0};
 
            for (size_t j = 0; j < aForegroundPointsFromOldPointsClusters.at(i).size(); j++)
            {
                pForegroundHypo->aNewPoints.push_back(
                    aForegroundPointsFromOldPointsClusters.at(i).at(j)->GetCopy());
                Math3d::AddToVec(vCenter,
                                 aForegroundPointsFromOldPointsClusters.at(i).at(j)->vPosition);
            }
 
            Math3d::MulVecScalar(vCenter,
                                 1.0 / aForegroundPointsFromOldPointsClusters.at(i).size(),
                                 pForegroundHypo->vCenter);
            pForegroundHypo->nHypothesisNumber = pObjectHypotheses->GetSize();
            pForegroundHypo->eType = CObjectHypothesis::eRGBD;
            pObjectHypotheses->AddElement(pForegroundHypo);
        }
    }
}
 
bool
CHypothesisValidationRGBD::ValidateHypothesis(
    CPointCloudRegistration* pPointCloudRegistration,
    const std::vector<CHypothesisPoint*>& aNewDepthMapPoints,
    const std::vector<CHypothesisPoint*>& aForegroundPoints,
    const CByteImage* pForegroundImage,
    const CByteImage* pHSVImage,
    const CCalibration* calibration,
    const Vec3d upwardsVector,
    CObjectHypothesis* pObjectHypothesis,
    CObjectHypothesis*& pConfirmedHypothesis,
    const int nEffortLevel)
{
    Mat3d mRot = Math3d::unit_mat;
    Vec3d vTrans = Math3d::zero_vec;
    float errorAtOriginalPosition = FLT_MAX;
 
    // check if the hypothesis moved
    if (pPointCloudRegistration->CheckObjectMatchAtOriginalPosition(
            pObjectHypothesis, errorAtOriginalPosition, nEffortLevel))
    {
        ARMARX_VERBOSE_S << "CHypothesisValidationRGBD: Initial check at old object position: the "
                            "transformation shows no motion.";
        pObjectHypothesis->aHypothesisHasMoved.push_back(false);
        return false;
    }
 
    // get possible object locations based on color region similarity
    std::vector<Vec3d> aPossibleLocations;
    CObjectRecognition::FindPossibleObjectLocations(pObjectHypothesis,
                                                    pHSVImage,
                                                    aNewDepthMapPoints,
                                                    calibration,
                                                    aPossibleLocations,
                                                    5 + 3 * OLP_EFFORT_POINTCLOUD_MATCHING);
 
    // estimate the transformation of the hypothesis
    std::vector<float> aPointMatchDistances;
    std::vector<CColorICP::CPointXYZRGBI> aNearestNeighbors;
    pPointCloudRegistration->EstimateTransformation(pObjectHypothesis,
                                                    mRot,
                                                    vTrans,
                                                    upwardsVector,
                                                    nEffortLevel,
                                                    &aPossibleLocations,
                                                    &aNearestNeighbors,
                                                    &aPointMatchDistances,
                                                    2 * errorAtOriginalPosition);
    //CPointCloudRegistration::EstimateTransformation(pObjectHypothesis, aForegroundPoints, mRot, vTrans, &aNearestNeighbors, &aPointMatchDistances);
    Math3d::SetVec(pObjectHypothesis->vLastTranslation, vTrans);
    Math3d::SetMat(pObjectHypothesis->mLastRotation, mRot);
 
    // get the rotation angle
    float fRotationAngle;
    Vec3d vTemp;
    Math3d::GetAxisAndAngle(mRot, vTemp, fRotationAngle);
    fRotationAngle *= 180.0f / M_PI;
 
    // get the translation of the object center
    Math3d::MulMatVec(mRot, pObjectHypothesis->vCenter, vTrans, vTemp);
    float fDistance = Math3d::Distance(vTemp, pObjectHypothesis->vCenter);
 
    // accept only transformations with significant motion
    float fMotionMeasure = 2 * fRotationAngle + fDistance; // 0.5 deg rotation <=> 1mm translation
    ARMARX_VERBOSE_S << "Motion measure: " << fMotionMeasure << " (" << fDistance << "mm, "
                     << fRotationAngle << "deg)";
 
    if ((fMotionMeasure > OLP_MINIMAL_MOTION_MEASURE) &&
        (2 * fDistance > OLP_MINIMAL_MOTION_MEASURE))
    {
        // debug
        int nNoForeground = 0;
        int nLargeDistance = 0;
 
 
        // create transformed hypothesis
        CObjectHypothesis* pTransformedHypo = new CObjectHypothesis();
        pTransformedHypo->nHypothesisNumber = pObjectHypothesis->nHypothesisNumber;
        pTransformedHypo->eType = pObjectHypothesis->eType;
        Math3d::SetVec(pTransformedHypo->vLastTranslation, vTrans);
        Math3d::SetMat(pTransformedHypo->mLastRotation, mRot);
        CHypothesisPoint* pPoint;
 
 
        // check for each candidate point if it is in the area of the image that changed; and if yes, check if it concurred with the transformation
        for (size_t j = 0; j < pObjectHypothesis->aNewPoints.size(); j++)
        {
            pPoint = pObjectHypothesis->aNewPoints.at(j)->GetCopy();
 
            // check if old and transformed position of the point are in the image region that changed
            bool bIsInForeground =
                COLPTools::PointIsInForeground(pPoint, pForegroundImage, calibration);
            Math3d::MulMatVec(mRot, pPoint->vPosition, vTrans, pPoint->vPosition);
            bIsInForeground &=
                COLPTools::PointIsInForeground(pPoint, pForegroundImage, calibration);
 
            if (bIsInForeground)
            {
                // check if the point matches well after the transformation
                if (aPointMatchDistances.at(pObjectHypothesis->aConfirmedPoints.size() + j) <
                    OLP_TOLERANCE_CONCURRENT_MOTION)
                {
                    pTransformedHypo->aConfirmedPoints.push_back(pPoint);
                }
                else
                {
                    delete pPoint;
                    nLargeDistance++;
                }
            }
            else
            {
                delete pPoint;
                nNoForeground++;
            }
        }
 
 
        // check the confirmed points
        for (size_t j = 0; j < pObjectHypothesis->aConfirmedPoints.size(); j++)
        {
            pPoint = pObjectHypothesis->aConfirmedPoints.at(j)->GetCopy();
 
            // check if old and transformed position of the point are in the image region that changed
            bool bIsInForeground =
                COLPTools::PointIsInForeground(pPoint, pForegroundImage, calibration);
            Math3d::MulMatVec(mRot, pPoint->vPosition, vTrans, pPoint->vPosition);
            bIsInForeground &=
                COLPTools::PointIsInForeground(pPoint, pForegroundImage, calibration);
 
            if (bIsInForeground)
            {
                // check if the point matches well after the transformation
                if (aPointMatchDistances.at(j) < OLP_TOLERANCE_CONCURRENT_MOTION)
                {
                    pTransformedHypo->aConfirmedPoints.push_back(pPoint);
                }
                else
                {
                    // interpolate with nearest neighbor
                    CColorICP::CPointXYZRGBI pNeighbor = aNearestNeighbors.at(j);
                    pPoint->vPosition.x = 0.5f * (pPoint->vPosition.x + pNeighbor.x);
                    pPoint->vPosition.y = 0.5f * (pPoint->vPosition.y + pNeighbor.y);
                    pPoint->vPosition.z = 0.5f * (pPoint->vPosition.z + pNeighbor.z);
                    pPoint->fColorR = 0.5f * (pPoint->fColorR + pNeighbor.r);
                    pPoint->fColorG = 0.5f * (pPoint->fColorG + pNeighbor.g);
                    pPoint->fColorB = 0.5f * (pPoint->fColorB + pNeighbor.b);
                    pPoint->fIntensity = 0.5f * (pPoint->fIntensity + pNeighbor.i);
 
                    pTransformedHypo->aNewPoints.push_back(pPoint);
                    nLargeDistance++;
                }
            }
            else
            {
                if (aPointMatchDistances.at(j) < OLP_TOLERANCE_CONCURRENT_MOTION)
                {
                    pTransformedHypo->aDoubtablePoints.push_back(pPoint);
                    nNoForeground++;
                }
                else
                {
                    delete pPoint;
                    nNoForeground++;
                }
            }
        }
 
        // check the doubtable points
        for (size_t j = 0; j < pObjectHypothesis->aDoubtablePoints.size(); j++)
        {
            pPoint = pObjectHypothesis->aDoubtablePoints.at(j)->GetCopy();
 
            // check if old and transformed position of the point are in the image region that changed
            bool bIsInForeground =
                COLPTools::PointIsInForeground(pPoint, pForegroundImage, calibration);
            Math3d::MulMatVec(mRot, pPoint->vPosition, vTrans, pPoint->vPosition);
            bIsInForeground &=
                COLPTools::PointIsInForeground(pPoint, pForegroundImage, calibration);
 
            if (bIsInForeground)
            {
                // check if the point matches well after the transformation
                if (aPointMatchDistances.at(pObjectHypothesis->aConfirmedPoints.size() +
                                            pObjectHypothesis->aNewPoints.size() + j) <
                    OLP_TOLERANCE_CONCURRENT_MOTION)
                {
                    //pTransformedHypo->aNewPoints.push_back(pPoint);
                    pTransformedHypo->aConfirmedPoints.push_back(pPoint);
                }
                else
                {
                    delete pPoint;
                    nLargeDistance++;
                }
            }
            else
            {
                delete pPoint;
                nNoForeground++;
            }
        }
 
        ARMARX_VERBOSE_S << "Discarded features: no foreground " << nNoForeground << ", bad match "
                         << nLargeDistance;
 
 
        // if the hypothesis still looks good after the transformation, clean up and add new candidate points
 
        if (pTransformedHypo->aConfirmedPoints.size() > OLP_MIN_NUM_FEATURES)
        {
            // fuse very close points
 
            int nTooClose = 0;
            std::vector<Vec2d> aPointPositions2D;
            aPointPositions2D.resize(pTransformedHypo->aConfirmedPoints.size());
 
            for (size_t i = 0; i < pTransformedHypo->aConfirmedPoints.size(); i++)
            {
                calibration->WorldToImageCoordinates(
                    pTransformedHypo->aConfirmedPoints.at(i)->vPosition,
                    aPointPositions2D.at(i),
                    false);
            }
 
            for (size_t i = 0; i < pTransformedHypo->aConfirmedPoints.size(); i++)
            {
                const Vec2d vPos2D = aPointPositions2D.at(i);
 
                for (size_t j = i + 1; j < pTransformedHypo->aConfirmedPoints.size(); j++)
                {
                    if (Math2d::Distance(vPos2D, aPointPositions2D.at(j)) <
                        0.4f * OLP_DEPTH_MAP_PIXEL_DISTANCE)
                    {
                        CHypothesisPoint* fusedPoint =
                            FuseTwoPoints(pTransformedHypo->aConfirmedPoints.at(i),
                                          pTransformedHypo->aConfirmedPoints.at(j));
                        delete pTransformedHypo->aConfirmedPoints.at(i);
                        pTransformedHypo->aConfirmedPoints.at(i) = fusedPoint;
                        delete pTransformedHypo->aConfirmedPoints.at(j);
                        pTransformedHypo->aConfirmedPoints.at(j) =
                            pTransformedHypo->aConfirmedPoints.at(
                                pTransformedHypo->aConfirmedPoints.size() - 1);
                        pTransformedHypo->aConfirmedPoints.pop_back();
                        aPointPositions2D.at(j) =
                            aPointPositions2D.at(aPointPositions2D.size() - 1);
                        nTooClose++;
                    }
                }
            }
 
            ARMARX_VERBOSE_S << "Discarded " << nTooClose
                             << " points because they were too close to another one";
 
 
            // add new candidate points
 
            std::vector<CHypothesisPoint*> aNewCandidatePoints;
            std::vector<Vec2d> aForegroundPoints2D, aConfirmedPoints2D, aNewAndDoubtablePoints2D;
            std::vector<Vec3d> aForegroundPoints3D, aConfirmedPoints3D;
#pragma omp sections
            {
#pragma omp section
                {
                    for (size_t i = 0; i < aForegroundPoints.size(); i++)
                    {
                        Vec3d point3D = aForegroundPoints.at(i)->vPosition;
                        aForegroundPoints3D.push_back(point3D);
                        Vec2d point2D;
                        calibration->WorldToImageCoordinates(point3D, point2D, false);
                        aForegroundPoints2D.push_back(point2D);
                    }
                }
 
#pragma omp section
                {
                    for (size_t i = 0; i < pTransformedHypo->aConfirmedPoints.size(); i++)
                    {
                        Vec3d point3D = pTransformedHypo->aConfirmedPoints.at(i)->vPosition;
                        aConfirmedPoints3D.push_back(point3D);
                        Vec2d point2D;
                        calibration->WorldToImageCoordinates(point3D, point2D, false);
                        aConfirmedPoints2D.push_back(point2D);
                    }
                }
 
#pragma omp section
                {
                    for (size_t i = 0; i < pTransformedHypo->aDoubtablePoints.size(); i++)
                    {
                        Vec2d point2D;
                        calibration->WorldToImageCoordinates(
                            pTransformedHypo->aDoubtablePoints.at(i)->vPosition, point2D, false);
                        aNewAndDoubtablePoints2D.push_back(point2D);
                    }
 
                    for (size_t i = 0; i < pTransformedHypo->aNewPoints.size(); i++)
                    {
                        Vec2d point2D;
                        calibration->WorldToImageCoordinates(
                            pTransformedHypo->aNewPoints.at(i)->vPosition, point2D, false);
                        aNewAndDoubtablePoints2D.push_back(point2D);
                    }
                }
            }
 
 
            Mat3d mRotInv;
            Math3d::Transpose(mRot, mRotInv);
 
            const float fMinDistanceLimit2D = 0.9f * OLP_DEPTH_MAP_PIXEL_DISTANCE;
 
#pragma omp parallel for
            for (size_t j = 0; j < aForegroundPoints.size(); j++)
            {
                // check if the old position of the point was in foreground
                Vec3d vOldPointPosition;
                Math3d::SubtractVecVec(aForegroundPoints.at(j)->vPosition, vTrans, vTemp);
                Math3d::MulMatVec(mRotInv, vTemp, vOldPointPosition);
 
                if (COLPTools::PointIsInForeground(
                        vOldPointPosition, pForegroundImage, calibration))
                {
                    // add only if it is close to the object, but not too close to the points already in the hypothesis
                    Vec2d vPos2D = aForegroundPoints2D.at(j);
                    Vec3d vPos3D = aForegroundPoints3D.at(j);
                    float fDist2D, fDist3D;
                    float fMinDist2D = FLT_MAX;
                    float fMinDist3D = FLT_MAX;
 
                    for (size_t k = 0; k < aConfirmedPoints2D.size(); k++)
                    {
                        fDist2D = Math2d::Distance(vPos2D, aConfirmedPoints2D.at(k));
 
                        if (fDist2D < fMinDist2D)
                        {
                            fMinDist2D = fDist2D;
                        }
 
                        fDist3D = Math3d::Distance(vPos3D, aConfirmedPoints3D.at(k));
 
                        if (fDist3D < fMinDist3D)
                        {
                            fMinDist3D = fDist3D;
                        }
                    }
 
                    if ((fMinDist2D < OLP_MAX_DISTANCE_FOR_ADDING_FOREGROUND_CANDIDATE_2D) &&
                        (fMinDist3D < OLP_MAX_DISTANCE_FOR_ADDING_FOREGROUND_CANDIDATE_3D) &&
                        (fMinDist2D > fMinDistanceLimit2D))
                    {
                        for (size_t k = 0; k < aNewAndDoubtablePoints2D.size(); k++)
                        {
                            fDist2D = Math2d::Distance(vPos2D, aNewAndDoubtablePoints2D.at(k));
 
                            if (fDist2D < fMinDist2D)
                            {
                                fMinDist2D = fDist2D;
                            }
                        }
 
                        if (fMinDist2D > fMinDistanceLimit2D)
                        {
#pragma omp critical
                            {
                                aNewCandidatePoints.push_back(aForegroundPoints.at(j)->GetCopy());
                            }
                        }
                    }
                }
            }
 
            for (size_t i = 0; i < aNewCandidatePoints.size(); i++)
            {
                //                float fDist2;
                //                float fMinDist = FLT_MAX;
                //                const Vec2d vPos2D = aCandidatePositions2D.at(i);
                //                for (size_t j=0; j<i; j++)
                //                {
                //                    const Vec2d vPos2DComp = aCandidatePositions2D.at(j);
                //                    fDist2 = (vPos2D.x-vPos2DComp.x)*(vPos2D.x-vPos2DComp.x) + (vPos2D.y-vPos2DComp.y)*(vPos2D.y-vPos2DComp.y);
                //                    if (fDist2<fMinDist) fMinDist = fDist2;
                //                }
 
                //if ((sqrt(fMinDist) > fMinDistanceLimit) || (rand()%3==0))
                //if (fMinDist > 0.98*fMinDistanceLimit*fMinDistanceLimit)
                {
                    pTransformedHypo->aNewPoints.push_back(aNewCandidatePoints.at(i));
                }
            }
 
 
            // copy visible points
            pTransformedHypo->aVisibleConfirmedPoints.resize(
                pTransformedHypo->aConfirmedPoints.size());
 
            for (size_t i = 0; i < pTransformedHypo->aConfirmedPoints.size(); i++)
            {
                pTransformedHypo->aVisibleConfirmedPoints.at(i) =
                    pTransformedHypo->aConfirmedPoints.at(i)->GetCopy();
            }
 
 
#ifdef OLP_ADD_POINTS_FROM_SEGMENTED_REGION
            CByteImage* pSegmentationMask = NULL;
            COLPTools::CreateObjectSegmentationMask(
                pTransformedHypo, calibration, pSegmentationMask);
            COLPTools::FilterForegroundPoints(aNewDepthMapPoints,
                                              pSegmentationMask,
                                              calibration,
                                              pTransformedHypo->aConfirmedPoints);
 
            for (size_t i = pTransformedHypo->aVisibleConfirmedPoints.size();
                 i < pTransformedHypo->aConfirmedPoints.size();
                 i++)
            {
                pTransformedHypo->aVisibleConfirmedPoints.push_back(
                    pTransformedHypo->aConfirmedPoints.at(i)->GetCopy());
            }
#endif
 
 
            // update the position of the center of the hypothesis
            Vec3d vNewCenter = {0, 0, 0};
 
            for (size_t i = 0; i < pTransformedHypo->aVisibleConfirmedPoints.size(); i++)
            {
                vNewCenter.x += pTransformedHypo->aVisibleConfirmedPoints.at(i)->vPosition.x;
                vNewCenter.y += pTransformedHypo->aVisibleConfirmedPoints.at(i)->vPosition.y;
                vNewCenter.z += pTransformedHypo->aVisibleConfirmedPoints.at(i)->vPosition.z;
            }
 
            if (pTransformedHypo->aVisibleConfirmedPoints.size() > 0)
            {
                pTransformedHypo->vCenter.x =
                    vNewCenter.x / (float)pTransformedHypo->aVisibleConfirmedPoints.size();
                pTransformedHypo->vCenter.y =
                    vNewCenter.y / (float)pTransformedHypo->aVisibleConfirmedPoints.size();
                pTransformedHypo->vCenter.z =
                    vNewCenter.z / (float)pTransformedHypo->aVisibleConfirmedPoints.size();
            }
 
            // update the maximal extent
            pTransformedHypo->fMaxExtent = 0;
            float fDist;
 
            for (size_t i = 0; i < pTransformedHypo->aVisibleConfirmedPoints.size(); i++)
            {
                fDist =
                    Math3d::Distance(pTransformedHypo->vCenter,
                                     pTransformedHypo->aVisibleConfirmedPoints.at(i)->vPosition);
 
                if (fDist > pTransformedHypo->fMaxExtent)
                {
                    pTransformedHypo->fMaxExtent = fDist;
                }
            }
 
 
            // return validated hypothesis
            pConfirmedHypothesis = pTransformedHypo;
            pConfirmedHypothesis->aHypothesisHasMoved.push_back(true);
            pObjectHypothesis->aHypothesisHasMoved.push_back(true);
            return true;
        }
        else
        {
            ARMARX_VERBOSE_S
                << "CHypothesisValidationRGBD: not enough points left after transformation";
            delete pTransformedHypo;
            pObjectHypothesis->aHypothesisHasMoved.push_back(false);
            return false;
        }
    }
    else
    {
        ARMARX_VERBOSE_S << "CHypothesisValidationRGBD: the transformation shows no motion.";
        pObjectHypothesis->aHypothesisHasMoved.push_back(false);
        return false;
    }
}
 
CHypothesisPoint*
CHypothesisValidationRGBD::FuseTwoPoints(CHypothesisPoint* p1, CHypothesisPoint* p2)
{
    CHypothesisPoint* result = p1->GetCopy();
    result->vPosition.x = 0.5f * (p1->vPosition.x + p2->vPosition.x);
    result->vPosition.y = 0.5f * (p1->vPosition.y + p2->vPosition.y);
    result->vPosition.z = 0.5f * (p1->vPosition.z + p2->vPosition.z);
 
    result->vOldPosition.x = 0.5f * (p1->vOldPosition.x + p2->vOldPosition.x);
    result->vOldPosition.y = 0.5f * (p1->vOldPosition.y + p2->vOldPosition.y);
    result->vOldPosition.z = 0.5f * (p1->vOldPosition.z + p2->vOldPosition.z);
 
    result->fColorR = 0.5f * (p1->fColorR + p2->fColorR);
    result->fColorG = 0.5f * (p1->fColorG + p2->fColorG);
    result->fColorB = 0.5f * (p1->fColorB + p2->fColorB);
    result->fIntensity = 0.5f * (p1->fIntensity + p2->fIntensity);
 
    result->fMembershipProbability =
        0.5f * (p1->fMembershipProbability + p2->fMembershipProbability);
 
    return result;
}