PointCloudObjectLocalizer.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    VisionX::ArmarXObjects::PointCloudObjectLocalizer
 * @author     Markus Grotz ( markus dot grotz at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PointCloudObjectLocalizer.h"
 
#include <pcl/PointIndices.h>
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
 
using namespace armarx;
 
namespace visionx
{
 
 
    void
    PointCloudObjectLocalizer::onInitPointCloudProcessor()
    {
        std::scoped_lock lock(pointCloudMutex);
 
        pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS);
 
        providerName = getProperty<std::string>("providerName").getValue();
 
        ARMARX_LOG << "using point cloud provider " << providerName << flush;
 
        agentName = getProperty<std::string>("agentName").getValue();
 
        sourceNodeName = getProperty<std::string>("sourceNodeName").getValue();
 
        icpOnly = getProperty<bool>("icpOnly").getValue();
 
        icp.setMaximumIterations(100);
        icp.setMaxCorrespondenceDistance(500.0);
 
        modelKeypoints.reset(new pcl::PointCloud<PointT>);
 
        std::string modelFileName = getProperty<std::string>("modelFileName").getValue();
        armarx::CMakePackageFinder finder("VisionX");
        ArmarXDataPath::addDataPaths(finder.getDataDir());
        pcl::PointCloud<PointT>::Ptr cloudPtr(new pcl::PointCloud<PointT>());
        if (!ArmarXDataPath::getAbsolutePath(modelFileName, modelFileName))
        {
            ARMARX_FATAL << "Could not find point cloud file in ArmarXDataPath: " << modelFileName;
            return;
        }
        else if (pcl::io::loadPCDFile<PointT>(modelFileName.c_str(), *cloudPtr) == -1)
        {
            ARMARX_FATAL << "unable to load point cloud from file " << modelFileName;
            return;
        }
 
        float leafSize = getProperty<float>("sceneLeafSize").getValue();
        grid.setLeafSize(Eigen::Vector3f(leafSize, leafSize, leafSize));
 
        grid.setInputCloud(cloudPtr);
        grid.filter(*modelKeypoints);
 
        if (!icpOnly)
 
        {
            modelDescriptors.reset(new pcl::PointCloud<PointD>());
 
            pcl::PointCloud<pcl::Normal>::Ptr pointCloudModelNormals(
                new pcl::PointCloud<pcl::Normal>);
            ARMARX_DEBUG << "computing normals";
            normalEstimation.setInputCloud(cloudPtr);
            normalEstimation.setRadiusSearch(10.0);
            normalEstimation.compute(*pointCloudModelNormals);
 
            ARMARX_DEBUG << "computing features";
            featureEstimation.setInputCloud(modelKeypoints);
            featureEstimation.setInputNormals(pointCloudModelNormals);
            featureEstimation.setSearchSurface(cloudPtr);
            featureEstimation.setRadiusSearch(20.0);
            featureEstimation.compute(*modelDescriptors);
 
            matchSearchTree.setInputCloud(modelDescriptors);
 
            clusterer.setGCSize(getProperty<double>("recGCSize").getValue());
            clusterer.setGCThreshold(getProperty<int>("recGCThreshold").getValue());
            clusterer.setInputCloud(modelKeypoints);
        }
 
 
        offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
 
        usingPointCloudProvider(providerName);
    }
 
    void
    PointCloudObjectLocalizer::onConnectPointCloudProcessor()
    {
        std::scoped_lock lock(pointCloudMutex);
 
        debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(
            getProperty<std::string>("DebugDrawerTopic").getValue());
 
        enableResultPointClouds<PointT>("Input");
        enableResultPointClouds<PointT>("BeforeICPResult");
        enableResultPointClouds<PointT>("AfterICPResult");
        enableResultPointClouds<PointT>("BestMatch");
    }
 
    void
    PointCloudObjectLocalizer::onDisconnectPointCloudProcessor()
    {
    }
 
    void
    PointCloudObjectLocalizer::onExitPointCloudProcessor()
    {
    }
 
    void
    PointCloudObjectLocalizer::process()
    {
 
        std::scoped_lock lock(pointCloudMutex);
 
        pcl::PointCloud<PointL>::Ptr cloudPtr(new pcl::PointCloud<PointL>());
 
        if (!waitForPointClouds(providerName, 10000))
        {
            ARMARX_WARNING << "Timeout or error while waiting for point cloud data"
                           << armarx::flush;
            return;
        }
        else
        {
            getPointClouds(providerName, cloudPtr);
        }
 
        float bestFitnessScore = std::numeric_limits<float>::max();
        Eigen::Matrix4f bestTransform = Eigen::Matrix4f::Identity();
 
        pcl::PointCloud<PointT>::Ptr afterICPResultPointCloud(new pcl::PointCloud<PointT>());
        pcl::PointCloud<PointT>::Ptr beforeICPResultPointCloud(new pcl::PointCloud<PointT>());
 
        std::map<uint32_t, pcl::PointIndices> labeledPointMap;
        visionx::tools::fillLabelMap<pcl::PointCloud<PointL>::Ptr>(cloudPtr, labeledPointMap);
 
        for (auto& it : labeledPointMap)
        {
 
            ARMARX_LOG << " checking label " << it.first;
 
            pcl::PointCloud<PointT>::Ptr segment(new pcl::PointCloud<PointT>());
            copyPointCloud(*cloudPtr, it.second, *segment);
 
            pcl::PointCloud<PointT>::Ptr transformedCloudPtr(new pcl::PointCloud<PointT>());
            pcl::PointCloud<PointT>::Ptr cloudDownsampledPtr(new pcl::PointCloud<PointT>());
            pcl::PointCloud<pcl::Normal>::Ptr normalPtr(new pcl::PointCloud<pcl::Normal>());
            pcl::PointCloud<PointD>::Ptr descriptorPtr(new pcl::PointCloud<PointD>());
            pcl::CorrespondencesPtr correspondencesPtr(new pcl::Correspondences());
 
            pcl::copyPointCloud(*segment, *transformedCloudPtr);
 
            grid.setInputCloud(transformedCloudPtr);
            grid.filter(*cloudDownsampledPtr);
 
            provideResultPointClouds(cloudDownsampledPtr, "Input");
 
            std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>> transformations;
 
            if (icpOnly)
            {
                Eigen::Vector4f centroid;
                pcl::compute3DCentroid(*cloudDownsampledPtr, centroid);
 
                for (float i = 0.0; i < 360.0; i += 45.0)
                {
                    Eigen::Affine3f transform(
                        Eigen::AngleAxisf(i * 180.0 / M_PI, Eigen::Vector3f::UnitZ()));
 
                    Eigen::Matrix4f transform2 = transform.matrix();
                    transform2.col(3) = centroid;
                    transformations.push_back(transform2);
                }
            }
            else
            {
                ARMARX_LOG << "computing normals";
                normalEstimation.setInputCloud(transformedCloudPtr);
                normalEstimation.compute(*normalPtr);
 
                ARMARX_LOG << "computing features";
                featureEstimation.setInputCloud(cloudDownsampledPtr);
                featureEstimation.setInputNormals(normalPtr);
                featureEstimation.setSearchSurface(transformedCloudPtr);
                featureEstimation.compute(*descriptorPtr);
                ARMARX_LOG << "found " << descriptorPtr->size() << " features";
 
                int i = 0;
                const int k = 1;
                for (PointD& d : descriptorPtr->points)
                {
                    // if (!std::isfinite(d.descriptor[0]))  //skipping NaNs
                    // {
                    //     continue;
                    // }
 
                    std::vector<int> k_indices(k);
                    std::vector<float> k_sqr_distances(k);
                    int numNeighborsFound =
                        matchSearchTree.nearestKSearch(d, k, k_indices, k_sqr_distances);
 
                    if (numNeighborsFound == 1) // && k_sqr_distances[0] < 0.25f)
                    {
                        pcl::Correspondence corr(k_indices[0], i, k_sqr_distances[0]);
                        correspondencesPtr->push_back(corr);
                    }
                    i++;
                }
 
                std::vector<pcl::Correspondences> clusteredCorrespondences;
 
                clusterer.setSceneCloud(cloudDownsampledPtr);
                clusterer.setModelSceneCorrespondences(correspondencesPtr);
                clusterer.recognize(transformations, clusteredCorrespondences);
 
                ARMARX_LOG << " found " << transformations.size() << " transformations";
            }
 
 
            for (Eigen::Matrix4f& t : transformations)
            {
                pcl::PointCloud<PointT>::Ptr transformedModelPtr3(new pcl::PointCloud<PointT>());
                pcl::transformPointCloud(*modelKeypoints, *transformedModelPtr3, t);
                visionx::tools::colorizeSegment(transformedModelPtr3);
                *beforeICPResultPointCloud += *transformedModelPtr3;
 
                icp.setInputTarget(cloudDownsampledPtr);
                icp.setInputSource(modelKeypoints);
                pcl::PointCloud<PointT>::Ptr registered(new pcl::PointCloud<PointT>);
                icp.align(*registered, t);
 
                if (icp.hasConverged())
                {
                    float fitnessScore = icp.getFitnessScore();
                    Eigen::Matrix4f transform = icp.getFinalTransformation();
 
                    ARMARX_LOG << "transformation after ICP " << transform;
                    ARMARX_LOG << "fitness score " << fitnessScore;
 
                    pcl::PointCloud<PointT>::Ptr transformedModelPtr2(
                        new pcl::PointCloud<PointT>());
                    pcl::transformPointCloud(*modelKeypoints, *transformedModelPtr2, transform);
                    visionx::tools::colorizeSegment(transformedModelPtr2);
                    *afterICPResultPointCloud += *transformedModelPtr2;
 
                    if (fitnessScore <= bestFitnessScore)
                    {
                        bestFitnessScore = fitnessScore;
                        bestTransform = transform;
 
                        std::scoped_lock lock(localizeMutex);
 
                        //pose = new armarx::FramedPose(bestTransform, poseInRootFrame->frame, agentName);
                        //FramedPosePtr::dynamicCast(pose)->changeFrame(robotStateComponent->getSynchronizedRobot(), sourceNodeName);
 
                        pose = new armarx::FramedPose(bestTransform, sourceNodeName, agentName);
 
                        debugDrawerPrx->setPoseVisu(
                            "PointCloudLocalizer", "label " + it.first, pose);
                    }
                }
            }
        }
 
        provideResultPointClouds(beforeICPResultPointCloud, "BeforeICPResult");
        provideResultPointClouds(afterICPResultPointCloud, "AfterICPResult");
 
        if (bestFitnessScore < 2000.0)
        {
            ARMARX_LOG << "best transformation " << bestTransform;
            pcl::PointCloud<PointT>::Ptr BestResultPointCloud(new pcl::PointCloud<PointT>());
            pcl::transformPointCloud(*modelKeypoints, *BestResultPointCloud, bestTransform);
            provideResultPointClouds(BestResultPointCloud, "BestMatch");
        }
    }
 
    memoryx::ObjectLocalizationResultList
    PointCloudObjectLocalizer::localizeObjectClasses(
        const memoryx::ObjectClassNameList& objectClassNames,
        const Ice::Current& c)
    {
        std::scoped_lock lock(localizeMutex);
        ARMARX_LOG << "localizing object...";
 
        memoryx::ObjectLocalizationResultList resultList;
 
        std::string objectClassName = getProperty<std::string>("objectClassName").getValue();
 
        for (std::string name : objectClassNames)
        {
            if (name == objectClassName && pose)
            {
                memoryx::ObjectLocalizationResult result;
                result.position = pose->getPosition();
                result.orientation = pose->getOrientation();
                result.objectClassName = objectClassName;
                result.recognitionCertainty = 1.0f;
                Eigen::Vector3f mean;
                mean << 0.0, 0.0, 0.0;
                Eigen::Matrix3f covar = Eigen::Matrix3f::Identity() * 100;
                memoryx::MultivariateNormalDistributionBasePtr dummy(
                    new memoryx::MultivariateNormalDistribution(mean, covar));
                result.positionNoise = dummy;
                resultList.push_back(result);
            }
        }
 
        return resultList;
    }
 
    void
    PointCloudObjectLocalizer::getLocation(FramedOrientationBasePtr& orientation,
                                           FramedPositionBasePtr& position,
                                           const Ice::Current& c)
    {
        std::scoped_lock lock(localizeMutex);
 
        orientation = this->pose->getOrientation();
        position = this->pose->getPosition();
    }
 
    PropertyDefinitionsPtr
    PointCloudObjectLocalizer::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(
            new PointCloudObjectLocalizerPropertyDefinitions(getConfigIdentifier()));
    }
} // namespace visionx