SegmentedPointCloudFusion.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 "SegmentedPointCloudFusion.h"
 
#include "ColorICP.h"
#include "OLPTools.h"
#include <omp.h>
 
// system
#include <sys/time.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
namespace CSegmentedPointCloudFusion
{
    void
    FusePointClouds(std::vector<CObjectHypothesis*> aHypotheses,
                    CObjectHypothesis*& pFusedHypothesis)
    {
        Transformation3d tTrafo;
        tTrafo.rotation = Math3d::unit_mat;
        tTrafo.translation = Math3d::zero_vec;
        std::vector<Transformation3d> aTransformations;
        aTransformations.resize(aHypotheses.size());
 
        for (size_t i = 0; i < aTransformations.size(); i++)
        {
            aTransformations.at(i) = tTrafo;
        }
 
        FusePointClouds(aHypotheses, aTransformations, pFusedHypothesis);
    }
 
    void
    FusePointClouds(std::vector<CObjectHypothesis*> aHypotheses,
                    std::vector<Transformation3d> aEstimatedTransformations,
                    CObjectHypothesis*& pFusedHypothesis)
    {
        std::vector<std::vector<CHypothesisPoint*>> aPointClouds;
        aPointClouds.resize(aHypotheses.size());
 
        for (size_t i = 0; i < aHypotheses.size(); i++)
        {
            aPointClouds.at(i).resize(aHypotheses.at(i)->aConfirmedPoints.size());
 
            for (size_t j = 0; j < aHypotheses.at(i)->aConfirmedPoints.size(); j++)
            {
                aPointClouds.at(i).at(j) = aHypotheses.at(i)->aConfirmedPoints.at(j);
            }
        }
 
        FusePointClouds(aPointClouds, aEstimatedTransformations, pFusedHypothesis);
    }
 
    void
    FusePointClouds(std::vector<std::vector<CHypothesisPoint*>> aHypothesisPoints,
                    CObjectHypothesis*& pFusedHypothesis)
    {
        Transformation3d tTrafo;
        tTrafo.rotation = Math3d::unit_mat;
        tTrafo.translation = Math3d::zero_vec;
        std::vector<Transformation3d> aTransformations;
        aTransformations.resize(aHypothesisPoints.size());
 
        for (size_t i = 0; i < aTransformations.size(); i++)
        {
            aTransformations.at(i) = tTrafo;
        }
 
        FusePointClouds(aHypothesisPoints, aTransformations, pFusedHypothesis);
    }
 
    // this one does the real work
    void
    FusePointClouds(std::vector<std::vector<CHypothesisPoint*>> aHypothesisPoints,
                    std::vector<Transformation3d> aEstimatedTransformations,
                    CObjectHypothesis*& pFusedHypothesis)
    {
        bool bPairwiseFusion = true;
 
        if (aHypothesisPoints.size() < 2)
        {
            ARMARX_WARNING_S << "FusePointClouds: At least two point clouds required!";
            return;
        }
 
        timeval tStart, tEnd;
        gettimeofday(&tStart, 0);
 
        // transform points with estimated transformations
        Transformation3d tAccumulatedTransform, tInverseTransform;
        tAccumulatedTransform.rotation = Math3d::unit_mat;
        tAccumulatedTransform.translation = Math3d::zero_vec;
 
        for (size_t i = 0; i < aHypothesisPoints.size() - 1; i++)
        {
            Math3d::MulMatMat(aEstimatedTransformations.at(i).rotation,
                              tAccumulatedTransform.rotation,
                              tAccumulatedTransform.rotation);
            Math3d::MulMatVec(aEstimatedTransformations.at(i).rotation,
                              tAccumulatedTransform.translation,
                              aEstimatedTransformations.at(i).translation,
                              tAccumulatedTransform.translation);
            Math3d::Transpose(tAccumulatedTransform.rotation, tInverseTransform.rotation);
            Math3d::MulMatVec(tInverseTransform.rotation,
                              tAccumulatedTransform.translation,
                              tInverseTransform.translation);
            Math3d::MulVecScalar(tInverseTransform.translation, -1, tInverseTransform.translation);
 
            for (size_t j = 0; j < aHypothesisPoints.at(i).size(); j++)
            {
                Math3d::MulMatVec(tInverseTransform.rotation,
                                  aHypothesisPoints.at(i).at(j)->vPosition,
                                  tInverseTransform.translation,
                                  aHypothesisPoints.at(i).at(j)->vPosition);
            }
        }
 
        // convert the points to simple format for ICP
        std::vector<std::vector<CColorICP::CPointXYZRGBI>> aPointClouds;
        aPointClouds.resize(aHypothesisPoints.size());
 
        for (size_t i = 0; i < aHypothesisPoints.size(); i++)
        {
            aPointClouds.at(i).resize(aHypothesisPoints.at(i).size());
 
            for (size_t j = 0; j < aHypothesisPoints.at(i).size(); j++)
            {
                aPointClouds.at(i).at(j) =
                    CColorICP::ConvertToXYZRGBI(aHypothesisPoints.at(i).at(j));
            }
        }
 
        // different orientations
        const int nNumDifferentInitializations = 7;
        std::vector<Mat3d> aRotations;
        aRotations.resize(nNumDifferentInitializations);
        const float fAngle = M_PI / 6.0f;
        Math3d::SetMat(aRotations.at(0), Math3d::unit_mat);
        Math3d::SetRotationMatX(aRotations.at(1), fAngle);
        Math3d::SetRotationMatX(aRotations.at(2), -fAngle);
        Math3d::SetRotationMatY(aRotations.at(3), fAngle);
        Math3d::SetRotationMatY(aRotations.at(4), -fAngle);
        Math3d::SetRotationMatZ(aRotations.at(5), fAngle);
        Math3d::SetRotationMatZ(aRotations.at(6), -fAngle);
 
 
        // create parallel ICP instances
        const int nParallelityFactor = omp_get_num_procs();
        omp_set_num_threads(nParallelityFactor);
        CColorICP** pColorICPInstances = new CColorICP*[nParallelityFactor];
 
        for (int i = 0; i < nParallelityFactor; i++)
        {
            pColorICPInstances[i] = new CColorICP();
            pColorICPInstances[i]->SetParameters(OLP_ICP_COLOR_DISTANCE_WEIGHT,
                                                 OLP_ICP_CUTOFF_DISTANCE);
        }
 
 
        int nFusionResultIndex = -1;
 
        ARMARX_VERBOSE_S << "Fusing " << aPointClouds.size() << " segmentations";
 
        if (bPairwiseFusion)
        {
            nFusionResultIndex = 0;
            std::vector<Transformation3d> aPairwiseTransformations;
            aPairwiseTransformations.resize(aPointClouds.size() - 1);
            std::vector<float> aMinDistances;
            aMinDistances.resize(aPointClouds.size() - 1);
 
            for (size_t i = 0; i < aPointClouds.size() - 1; i++)
            {
                aPairwiseTransformations.at(i).rotation = Math3d::unit_mat;
                aPairwiseTransformations.at(i).translation = Math3d::zero_vec;
                aMinDistances.at(i) = FLT_MAX;
            }
 
 
// determine the best transformation, starting from different initial transformations
#pragma omp parallel for schedule(static, 1)
            for (int k = 0; k < nNumDifferentInitializations * ((int)aPointClouds.size() - 1); k++)
            {
                const int n = k / nNumDifferentInitializations;
                const int i = k % nNumDifferentInitializations;
                const int nThreadNumber = omp_get_thread_num();
 
                // set second point cloud as scene point cloud (because it is probably bigger than the one before)
                pColorICPInstances[nThreadNumber]->SetScenePointcloud(aPointClouds.at(n + 1));
 
                // rotate points
                std::vector<CColorICP::CPointXYZRGBI> aRotatedPointCloud;
                aRotatedPointCloud.resize(aPointClouds.at(n).size());
 
                for (size_t j = 0; j < aPointClouds.at(n).size(); j++)
                {
                    aRotatedPointCloud.at(j) = aPointClouds.at(n).at(j);
                    CColorICP::TransformPointXYZRGBI(
                        aRotatedPointCloud.at(j), aRotations.at(i), Math3d::zero_vec);
                }
 
                Mat3d mRotation;
                Vec3d vTranslation;
                float fDist = pColorICPInstances[nThreadNumber]->SearchObject(
                    aRotatedPointCloud, mRotation, vTranslation);
 
#pragma omp critical
                if (fDist < aMinDistances.at(n))
                {
                    Math3d::MulMatMat(
                        mRotation, aRotations.at(i), aPairwiseTransformations.at(n).rotation);
                    Math3d::SetVec(aPairwiseTransformations.at(n).translation, vTranslation);
                    aMinDistances.at(n) = fDist;
                }
            }
 
            // combine all transformed point clouds
            int nOverallNumberOfPoints = 0;
            int nIndexOffset = aPointClouds.at(0).size();
 
            for (size_t i = 0; i < aPointClouds.size(); i++)
            {
                nOverallNumberOfPoints += aPointClouds.at(i).size();
            }
 
            aPointClouds.at(nFusionResultIndex).resize(nOverallNumberOfPoints);
            tAccumulatedTransform.rotation = Math3d::unit_mat;
            tAccumulatedTransform.translation = Math3d::zero_vec;
 
            for (size_t i = 0; i < aPointClouds.size() - 1; i++)
            {
                Math3d::MulMatMat(aPairwiseTransformations.at(i).rotation,
                                  tAccumulatedTransform.rotation,
                                  tAccumulatedTransform.rotation);
                Math3d::MulMatVec(aPairwiseTransformations.at(i).rotation,
                                  tAccumulatedTransform.translation,
                                  aPairwiseTransformations.at(i).translation,
                                  tAccumulatedTransform.translation);
                Math3d::Transpose(tAccumulatedTransform.rotation, tInverseTransform.rotation);
                Math3d::MulMatVec(tInverseTransform.rotation,
                                  tAccumulatedTransform.translation,
                                  tInverseTransform.translation);
                Math3d::MulVecScalar(
                    tInverseTransform.translation, -1, tInverseTransform.translation);
 
#pragma omp parallel for schedule(static, 100)
                for (size_t j = 0; j < aPointClouds.at(i + 1).size(); j++)
                {
                    CColorICP::TransformPointXYZRGBI(aPointClouds.at(i + 1).at(j),
                                                     tInverseTransform.rotation,
                                                     tInverseTransform.translation);
                    aPointClouds.at(nFusionResultIndex).at(nIndexOffset + j) =
                        aPointClouds.at(i + 1).at(j);
                }
 
                if (i > 0)
                {
                    nIndexOffset += aPointClouds.at(i).size();
                }
            }
        }
        else
        {
            const int nStartIndex = aHypothesisPoints.size() / 2;
            nFusionResultIndex = nStartIndex;
            int nLowIndex = nStartIndex - 1;
            int nHighIndex = nStartIndex + 1;
 
            // match and add earlier and later segmentations alternately
            while (nLowIndex >= 0 || nHighIndex < (int)aHypothesisPoints.size())
            {
                // update fused point cloud into ICP after adding both an earlier and later segmentation
                for (int i = 0; i < nParallelityFactor; i++)
                {
                    pColorICPInstances[i]->SetScenePointcloud(aPointClouds.at(nStartIndex));
                }
 
                // match the next earlier segmentation
                if (nLowIndex >= 0)
                {
                    Mat3d mBestRotation = Math3d::unit_mat;
                    Vec3d vBestTranslation = Math3d::zero_vec;
                    float fMinDist = FLT_MAX;
 
#pragma omp parallel for schedule(static, 1)
                    for (int i = 0; i < nNumDifferentInitializations; i++)
                    {
                        const int nThreadNumber = omp_get_thread_num();
 
                        // rotate points
                        std::vector<CColorICP::CPointXYZRGBI> aRotatedPointCloud;
                        aRotatedPointCloud.resize(aPointClouds.at(nLowIndex).size());
 
                        for (size_t j = 0; j < aPointClouds.at(nLowIndex).size(); j++)
                        {
                            aRotatedPointCloud.at(j) = aPointClouds.at(nLowIndex).at(j);
                            CColorICP::TransformPointXYZRGBI(
                                aRotatedPointCloud.at(j), aRotations.at(i), Math3d::zero_vec);
                        }
 
                        Mat3d mRotation;
                        Vec3d vTranslation;
                        float fDist = pColorICPInstances[nThreadNumber]->SearchObject(
                            aRotatedPointCloud, mRotation, vTranslation);
 
#pragma omp critical
                        if (fDist < fMinDist)
                        {
                            Math3d::MulMatMat(mRotation, aRotations.at(i), mBestRotation);
                            Math3d::SetVec(vBestTranslation, vTranslation);
                            fMinDist = fDist;
                        }
                    }
 
                    ARMARX_VERBOSE_S << "Index " << nLowIndex << ": dist " << fMinDist;
 
                    for (size_t i = 0; i < aPointClouds.at(nLowIndex).size(); i++)
                    {
                        CColorICP::TransformPointXYZRGBI(
                            aPointClouds.at(nLowIndex).at(i), mBestRotation, vBestTranslation);
                        aPointClouds.at(nStartIndex).push_back(aPointClouds.at(nLowIndex).at(i));
                    }
 
                    nLowIndex--;
                }
 
                // match the next later segmentation
                if (nHighIndex < (int)aHypothesisPoints.size())
                {
                    Mat3d mBestRotation = Math3d::unit_mat;
                    Vec3d vBestTranslation = Math3d::zero_vec;
                    float fMinDist = FLT_MAX;
 
#pragma omp parallel for schedule(static, 1)
                    for (int i = 0; i < nNumDifferentInitializations; i++)
                    {
                        const int nThreadNumber = omp_get_thread_num();
 
                        // rotate points
                        std::vector<CColorICP::CPointXYZRGBI> aRotatedPointCloud;
                        aRotatedPointCloud.resize(aPointClouds.at(nHighIndex).size());
 
                        for (size_t j = 0; j < aPointClouds.at(nHighIndex).size(); j++)
                        {
                            aRotatedPointCloud.at(j) = aPointClouds.at(nHighIndex).at(j);
                            CColorICP::TransformPointXYZRGBI(
                                aRotatedPointCloud.at(j), aRotations.at(i), Math3d::zero_vec);
                        }
 
                        Mat3d mRotation;
                        Vec3d vTranslation;
                        float fDist = pColorICPInstances[nThreadNumber]->SearchObject(
                            aRotatedPointCloud, mRotation, vTranslation);
 
#pragma omp critical
                        if (fDist < fMinDist)
                        {
                            Math3d::MulMatMat(mRotation, aRotations.at(i), mBestRotation);
                            Math3d::SetVec(vBestTranslation, vTranslation);
                            fMinDist = fDist;
                        }
                    }
 
                    ARMARX_VERBOSE_S << "Index " << nHighIndex << ": dist" << fMinDist;
 
                    for (size_t i = 0; i < aPointClouds.at(nHighIndex).size(); i++)
                    {
                        CColorICP::TransformPointXYZRGBI(
                            aPointClouds.at(nHighIndex).at(i), mBestRotation, vBestTranslation);
                        aPointClouds.at(nStartIndex).push_back(aPointClouds.at(nHighIndex).at(i));
                    }
 
                    nHighIndex++;
                }
            }
        }
 
        ARMARX_VERBOSE_S << "Done";
 
 
        // return result
        if (!pFusedHypothesis)
        {
            pFusedHypothesis = new CObjectHypothesis();
        }
 
        pFusedHypothesis->eType = CObjectHypothesis::eRGBD;
        pFusedHypothesis->aConfirmedPoints.clear();
        pFusedHypothesis->aConfirmedPoints.resize(aPointClouds.at(nFusionResultIndex).size());
        pFusedHypothesis->aVisibleConfirmedPoints.clear();
        pFusedHypothesis->aVisibleConfirmedPoints.resize(
            aPointClouds.at(nFusionResultIndex).size());
 
        for (size_t i = 0; i < aPointClouds.at(nFusionResultIndex).size(); i++)
        {
            pFusedHypothesis->aConfirmedPoints.at(i) =
                CColorICP::ConvertFromXYZRGBI(aPointClouds.at(nFusionResultIndex).at(i));
            pFusedHypothesis->aVisibleConfirmedPoints.at(i) =
                CColorICP::ConvertFromXYZRGBI(aPointClouds.at(nFusionResultIndex).at(i));
        }
 
        COLPTools::GetMeanAndVariance(pFusedHypothesis->aConfirmedPoints,
                                      pFusedHypothesis->vCenter,
                                      pFusedHypothesis->fMaxExtent);
        pFusedHypothesis->fMaxExtent = 2 * sqrtf(pFusedHypothesis->fMaxExtent);
 
        for (int i = 0; i < nParallelityFactor; i++)
        {
            delete pColorICPInstances[i];
        }
 
        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 fusion: " << tTimeDiff << " ms";
    }
 
} // namespace CSegmentedPointCloudFusion