MaskRCNNPointCloudObjectLocalizer.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::MaskRCNNPointCloudObjectLocalizer
 * @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 "MaskRCNNPointCloudObjectLocalizer.h"
 
#include <pcl/PointIndices.h>
#include <pcl/common/centroid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/sample_consensus/mlesac.h>
#include <pcl/segmentation/extract_clusters.h>
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
using namespace armarx;
 
namespace visionx
{
    void
    MaskRCNNPointCloudObjectLocalizer::onInitPointCloudProcessor()
    {
        pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS);
 
 
        agentName = getProperty<std::string>("agentName").getValue();
 
        sourceNodeName = getProperty<std::string>("sourceNodeName").getValue();
 
        offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
 
        offeringTopic("ServiceRequests");
    }
 
    void
    MaskRCNNPointCloudObjectLocalizer::onConnectPointCloudProcessor()
    {
        serviceTopic = getTopic<armarx::RequestableServiceListenerInterfacePrx>("ServiceRequests");
 
        enableResultPointClouds<PointL>();
    }
 
    void
    MaskRCNNPointCloudObjectLocalizer::onDisconnectPointCloudProcessor()
    {
    }
 
    void
    MaskRCNNPointCloudObjectLocalizer::onExitPointCloudProcessor()
    {
    }
 
    void
    MaskRCNNPointCloudObjectLocalizer::process()
    {
        usleep(100000);
    }
 
    memoryx::ObjectLocalizationResultList
    MaskRCNNPointCloudObjectLocalizer::localizeObjectClasses(
        const memoryx::ObjectClassNameList& objectClassNames,
        const Ice::Current& c)
    {
        //    boost::mutex::scoped_lock lock(localizeMutex);
        ARMARX_VERBOSE << "localizing objects " << objectClassNames;
        serviceTopic->requestService("MaskRCNNImageProcessor", 1000);
        std::scoped_lock lock2(pointCloudMutex);
        memoryx::ObjectLocalizationResultList resultList;
        pcl::PointCloud<PointL>::Ptr cloudPtr(new pcl::PointCloud<PointL>());
        pcl::PointCloud<PointL>::Ptr resultCloud(new pcl::PointCloud<PointL>());
 
 
        if (!waitForPointClouds(10000))
        {
            ARMARX_WARNING << "Timeout or error while waiting for point cloud data"
                           << armarx::flush;
            return resultList;
        }
        else
        {
            getPointClouds(cloudPtr);
        }
        ARMARX_VERBOSE << "Got pointcloud";
        uint32_t backgroundLabel = getProperty<uint32_t>("BackgroundLabelId").getValue();
        auto mapping = armarx::Split(getProperty<std::string>("ObjectNameIdMap"), ";", true, true);
        std::map<std::string, int> nameIdMap;
        std::map<int, std::string> idNameMap;
        for (auto entry : mapping)
        {
            auto pair = armarx::Split(entry, ":");
            ARMARX_CHECK_EQUAL(pair.size(), 2) << entry;
            nameIdMap[pair.at(0)] = armarx::toInt(pair.at(1));
            idNameMap[armarx::toInt(pair.at(1))] = pair.at(0);
        }
 
 
        cloudPtr->is_dense = false;
        std::vector<int> index;
        ARMARX_INFO << "Cloud size with NaN: " << cloudPtr->size();
        pcl::removeNaNFromPointCloud(*cloudPtr, *cloudPtr, index);
        ARMARX_INFO << "Cloud size without NaN: " << cloudPtr->size();
 
 
        std::map<uint32_t, pcl::PointIndices> labeledPointMap;
        visionx::tools::fillLabelMap<pcl::PointCloud<PointL>::Ptr>(cloudPtr, labeledPointMap);
        std::map<std::string, Eigen::Vector3f> positionMap;
 
 
        pcl::search::KdTree<PointL>::Ptr tree(new pcl::search::KdTree<PointL>);
        tree->setInputCloud(cloudPtr);
 
        pcl::EuclideanClusterExtraction<PointL> ec;
        ec.setClusterTolerance(20); // 2cm
        ec.setMinClusterSize(100);
        ec.setMaxClusterSize(25000);
        ec.setInputCloud(cloudPtr);
        ec.setSearchMethod(tree);
 
        //    std::set<int> labelIds;
        for (auto& it : labeledPointMap)
        {
            auto label = it.first;
            if (label == backgroundLabel)
            {
                //            ARMARX_VERBOSE << "Skipping background label with " << allPoints.size() << " points";
                continue;
            }
 
            if (!idNameMap.count(label))
            {
                ARMARX_INFO << "Unknown label: " << label;
                continue;
            }
            auto labelName = idNameMap.at(label);
            ARMARX_VERBOSE << "Checking cloud of " << labelName;
            if (std::find(objectClassNames.begin(), objectClassNames.end(), labelName) ==
                objectClassNames.end())
            {
                ARMARX_VERBOSE << "Object was not asked for: " << labelName;
                continue;
            }
 
            auto labelIndices = it.second.indices;
 
            pcl::IndicesPtr i(new std::vector<int>(it.second.indices));
            ec.setIndices(i);
            std::vector<pcl::PointIndices> cluster_indices;
            ec.extract(cluster_indices);
            ARMARX_VERBOSE << "Found " << cluster_indices.size() << " clusters";
 
            if (!cluster_indices.size())
            {
                ARMARX_VERBOSE << "Skipping " << labelName;
                continue;
            }
 
            int j = 0;
            size_t maxClusterSize = 0;
            size_t indexBiggestCluster = 0;
            for (auto& cluster : cluster_indices)
            {
                ARMARX_VERBOSE << "Cluster size: " << cluster.indices.size();
                if (cluster.indices.size() > maxClusterSize)
                {
                    indexBiggestCluster = j;
                    maxClusterSize = cluster.indices.size();
                }
                j++;
            }
            labelIndices = cluster_indices.at(indexBiggestCluster).indices;
 
            ARMARX_DEBUG << " checking label " << labelName;
            pcl::PointCloud<PointL>::Ptr segment(new pcl::PointCloud<PointL>());
            copyPointCloud(*cloudPtr, labelIndices, *segment);
            *resultCloud += *segment;
 
 
            Eigen::Vector4f centroid;
            pcl::compute3DCentroid(*segment, centroid);
            positionMap[labelName] = centroid.head<3>();
        }
 
        ARMARX_VERBOSE << "object positions for " << positionMap;
        for (auto pair : positionMap)
        {
            auto name = pair.first;
            Eigen::Vector3f position = pair.second;
 
            memoryx::ObjectLocalizationResult result;
            result.position = new FramedPosition(position, sourceNodeName, agentName);
            //        ARMARX_VERBOSE << name << ": " << result.position->output();
            Eigen::Matrix3f id = Eigen::Matrix3f::Identity();
            result.orientation = new FramedOrientation(id, GlobalFrame, "");
            result.objectClassName = name;
            result.recognitionCertainty = 0.6;
            result.timeStamp = new TimestampVariant(cloudPtr->header.stamp);
 
            Eigen::Vector3f cov(500, 500, 500);
            result.positionNoise = new memoryx::MultivariateNormalDistribution(
                Eigen::Vector3f::Zero(), cov.asDiagonal());
            resultList.push_back(result);
        }
 
        provideResultPointClouds(resultCloud);
 
        return resultList;
    }
 
    void
    MaskRCNNPointCloudObjectLocalizer::getLocation(FramedOrientationBasePtr& orientation,
                                                   FramedPositionBasePtr& position,
                                                   const Ice::Current& c)
    {
        throw LocalException("NYI");
    }
 
    PropertyDefinitionsPtr
    MaskRCNNPointCloudObjectLocalizer::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(
            new MaskRCNNPointCloudObjectLocalizerPropertyDefinitions(getConfigIdentifier()));
    }
} // namespace visionx