PointCloudSegmenter.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
 * @author     Eren Aksoy ( eren dot aksoy at kit dot edu )
 * @date       2015
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "PointCloudSegmenter.h"
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/observers/variant/TimestampVariant.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
#include <VisionX/components/pointcloud_core/PointCloudConversions.h>
 
#include <pcl/common/colors.h>
#include <pcl/filters/crop_box.h>
 
#include <QSettings>
 
using namespace armarx;
namespace visionx
{
    PointCloudSegmenter::PointCloudSegmenter() :
        inputCloudPtr(new pcl::PointCloud<PointO>),
        labeledColoredCloudPtr(new pcl::PointCloud<PointRGBL>)
    {
    }
 
    void PointCloudSegmenter::onInitPointCloudProcessor()
    {
        lastProcessedTimestamp = 0;
 
        // Get point cloud provider name
        providerName = getProperty<std::string>("ProviderName").getValue();
        ARMARX_INFO << "Using point cloud provider " << providerName << flush;
 
 
        // Get segmentation method
        segmentationMethod = getProperty<std::string>("segmentationMethod").getValue();
        ARMARX_INFO << "Using the segmentation method " << segmentationMethod << flush;
 
        offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
 
        enabled = getProperty<bool>("StartEnabled").getValue();
        ARMARX_INFO << "Starting " << (enabled ? "Enabled" : "Disabled");
 
        // get LCCP segmentation parameters
        minSegSize = getProperty<int>("lccpMinimumSegmentSize").getValue();
        voxelRes = getProperty<float>("lccpVoxelResolution").getValue();
        seedRes = getProperty<float>("lccpSeedResolution").getValue();
        colorImp = getProperty<float>("lccpColorImportance").getValue();
        spatialImp = getProperty<float>("lccpSpatialImportance").getValue();
        normalImp = getProperty<float>("lccpNormalImportance").getValue();
        concavityThres = getProperty<float>("lccpConcavityThreshold").getValue();
        smoothnessThes = getProperty<float>("lccpSmoothnessThreshold").getValue();
 
        // get RG segmentation parameters
        rgSmoothnessThes = getProperty<float>("rgSmoothnessThreshold").getValue();
        rgCurvatureThres = getProperty<float>("rgCurvatureThreshold").getValue();
 
        // init segmenters and update parameters
        if (segmentationMethod == "LCCP")
        {
            pLCCP = new LCCPSegClass();
            pLCCP->UpdateParameters(voxelRes, seedRes, colorImp, spatialImp, normalImp, concavityThres, smoothnessThes, minSegSize);
        }
        else if (segmentationMethod == "RG")
        {
            pRGSegmenter = new RGSegClass();
            pRGSegmenter->UpdateRegionGrowingSegmentationParameters(rgSmoothnessThes, rgCurvatureThres);
        }
        else if (segmentationMethod == "EC")
        {
            pECSegmenter = new EuclideanBasedClustering();
        }
        else if (segmentationMethod == "DS")
        {
            usingProxy(getProperty<std::string>("DeepSegmenterTopicName").getValue());
            pDeepSegmenter = new DeepSegClass();
        }
    }
 
    void PointCloudSegmenter::onConnectPointCloudProcessor()
    {
        debugDrawerTopic = getTopic<armarx::DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
        if (!debugDrawerTopic)
        {
            ARMARX_WARNING << "Failed to obtain debug drawer proxy";
        }
 
        if (segmentationMethod == "DS")
        {
            deepSegmenterTopic = getProxy<armarx::DeepSegmenterInterfacePrx>(getProperty<std::string>("DeepSegmenterTopicName").getValue());
            if (!deepSegmenterTopic)
            {
                ARMARX_WARNING << "Failed to obtain deep segmenter proxy";
            }
        }
 
        enableResultPointClouds<PointRGBL>();
 
        createRemoteGuiTab();
        RemoteGui_startRunningTask();
    }
 
    void PointCloudSegmenter::onDisconnectPointCloudProcessor()
    {
    }
 
    void PointCloudSegmenter::onExitPointCloudProcessor()
    {
        if (pLCCP)
        {
            delete pLCCP;
        }
 
        if (pRGSegmenter)
        {
            delete pRGSegmenter;
        }
 
        if (pECSegmenter)
        {
            delete pECSegmenter;
        }
 
        if (pDeepSegmenter)
        {
            delete pDeepSegmenter;
        }
    }
 
    void PointCloudSegmenter::process()
    {
        {
            std::unique_lock lock(enableMutex);
 
            if (!enabled)
            {
                return;
            }
        }
 
        if (!waitForPointClouds(providerName, 10000))
        {
            ARMARX_WARNING << "Timeout or error while waiting for point cloud data" << armarx::flush;
            return;
        }
 
 
        //if (getPointCloudFormat(providerName)->type == eColoredLabeledPoints)
        //{
        //    ARMARX_INFO << "Received RGBL point cloud: Passing on already segmented point cloud";
        //
        //    labeledColoredCloudPtr.reset(new pcl::PointCloud<pcl::PointXYZRGBL>());
        //    getPointClouds(providerName, labeledColoredCloudPtr);
        //    provideResultPointClouds(labeledColoredCloudPtr);
        //
        //    {
        //        std::unique_lock lock(timestampMutex);
        //        lastProcessedTimestamp = getPointCloudFormat(providerName)->timeProvided;
        //    }
        //
        //    return;
        //}
        //else
        //{
        getPointClouds(providerName, inputCloudPtr);
        ARMARX_INFO << "Point cloud received (size: " << inputCloudPtr->points.size() << ", providerName: " << providerName << ")";
        //}
 
        MetaPointCloudFormatPtr info = getPointCloudFormat(providerName);
        IceUtil::Int64 originalTimestamp = info->timeProvided;
 
        IceUtil::Time ts = IceUtil::Time::microSeconds(originalTimestamp);
        std::string timestampString = ts.toDateTime().substr(ts.toDateTime().find(' ') + 1);
 
        IceUtil::Time start_ts = IceUtil::Time::now();
 
        ARMARX_INFO << "Point cloud received (timestamp: " << timestampString << ", size: " << inputCloudPtr->points.size() << ", providerName: " << providerName << ")";
        if (inputCloudPtr->points.size() == 0)
        {
            return;
        }
 
        if (segmentationMethod == "LCCP")
        {
            pcl::PointCloud< pcl::PointXYZL >::Ptr labeledPCPtr = pLCCP->GetLabeledPointCloud(inputCloudPtr);
            labeledColoredCloudPtr.reset(new pcl::PointCloud<PointRGBL>());  // returns labeled cloud in XYZL format
 
            if (inputCloudPtr->size() != labeledPCPtr->size())
            {
                ARMARX_WARNING << "Point cloud size mismatch (RGB: " << inputCloudPtr->size() << ", XYZL: " << labeledPCPtr->size() << "), skipping current frame";
                return;
            }
 
 
            pcl::concatenateFields(*inputCloudPtr, *labeledPCPtr, *labeledColoredCloudPtr);
            ARMARX_INFO << " computed supervoxelcluster size: " << pLCCP->GetSuperVoxelClusterSize() ;
            ARMARX_INFO << " computed labeledCloudPtr size: " << labeledColoredCloudPtr->points.size() ;
        }
        else if (segmentationMethod == "RG")
        {
            pRGSegmenter->UpdateRegionGrowingSegmentationParameters(rgSmoothnessThes, rgCurvatureThres);
            auto colorizedLabeledCloudPtr = pRGSegmenter->ApplyRegionGrowingSegmentation(inputCloudPtr); // returns labeled cloud in XYZRGBA format
            pcl::PointCloud< pcl::PointXYZL >::Ptr labeledPCPtr(new pcl::PointCloud< pcl::PointXYZL >());
            convertFromXYZRGBAtoXYZL(colorizedLabeledCloudPtr, labeledPCPtr);    // convert form XYZRGBA to XYZL for the labeling process
            labeledColoredCloudPtr.reset(new pcl::PointCloud<PointRGBL>());
            pcl::concatenateFields(*inputCloudPtr, *labeledPCPtr, *labeledColoredCloudPtr);
        }
        else if (segmentationMethod == "EC")
        {
            labeledColoredCloudPtr = pECSegmenter->GetLabeledPointCloud(inputCloudPtr);
        }
        else if (segmentationMethod == "DS")
        {
            if (!deepSegmenterTopic) // check the proxy
            {
                ARMARX_WARNING << "Failed to obtain deep segmenter proxy";
                return;
            }
 
            if (!inputCloudPtr->isOrganized()) // the input cloud must be organized
            {
                ARMARX_WARNING << "Input point cloud is not organized!";
                return;
            }
 
            // first get rgb image from the point cloud data
            rgbImage = pDeepSegmenter->GetImageFromPointCloud(inputCloudPtr);
 
            deepSegmenterTopic->setImageDimensions(inputCloudPtr->width, inputCloudPtr->height);
            armarx::Blob deepSegmentImage = deepSegmenterTopic->segmentImage(rgbImage);
 
            labeledColoredCloudPtr = pDeepSegmenter->GetLabeledPointCloud(inputCloudPtr, deepSegmentImage);
 
        }
 
        // Provide point cloud referencing the time stamp of the original source point cloud
        labeledColoredCloudPtr->header.stamp = originalTimestamp;
 
        {
            std::unique_lock lock(enableMutex);
 
            // Do not provide result if segmenter has been switched off in the meantime
            if (enabled)
            {
                provideResultPointClouds(labeledColoredCloudPtr);
 
                {
                    std::unique_lock lock(timestampMutex);
                    lastProcessedTimestamp = originalTimestamp;
                }
            }
        }
 
        ARMARX_INFO << "Point cloud segmentation took " << (IceUtil::Time::now() - start_ts).toSecondsDouble() << " secs";
 
        if (getProperty<bool>("SingleRun").getValue())
        {
            ARMARX_WARNING << "PointCloudSegmenter configured to run only once";
            enabled = false;
        }
    }
 
    PropertyDefinitionsPtr PointCloudSegmenter::createPropertyDefinitions()
    {
        return PropertyDefinitionsPtr(new PointCloudSegmenterPropertyDefinitions(getConfigIdentifier()));
    }
 
    void PointCloudSegmenter::enablePipelineStep(const Ice::Current& c)
    {
        std::unique_lock lock(enableMutex);
        enabled = true;
 
        ARMARX_INFO << "Enabling segmentation";
    }
 
    void PointCloudSegmenter::disablePipelineStep(const Ice::Current& c)
    {
        std::unique_lock lock(enableMutex);
        enabled = false;
 
        ARMARX_INFO << "Disabling segmentation";
    }
 
    void PointCloudSegmenter::changePointCloudProvider(const std::string& providerName, const Ice::Current& c)
    {
        std::unique_lock lock(providerMutex);
 
        ARMARX_INFO << "Changing point cloud provider to '" << providerName << "'";
 
        onDisconnectComponent();
 
        releasePointCloudProvider(this->providerName);
        this->providerName = providerName;
        usingPointCloudProvider(this->providerName);
 
        onConnectComponent();
    }
 
    void PointCloudSegmenter::convertFromXYZRGBAtoXYZL(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& sourceCloudPtr, pcl::PointCloud<pcl::PointXYZL>::Ptr& targetCloudPtr)
    {
        targetCloudPtr->resize(sourceCloudPtr->points.size());
        uint32_t newLabel = 1;
 
        struct rgbValues
        {
            int rVal;
            int gVal;
            int bVal;
        };
        std::map<uint32_t, rgbValues > colorMap;
 
        // scan the source cloud
        for (size_t i = 0; i < sourceCloudPtr->points.size(); i++)
        {
            rgbValues currRGB;
            currRGB.rVal = (int) sourceCloudPtr->points[i].r;
            currRGB.gVal = (int) sourceCloudPtr->points[i].g;
            currRGB.bVal = (int) sourceCloudPtr->points[i].b;
 
            bool isFound = false;
            uint32_t foundLabel = 0;
 
            // search in the map
            std::map<uint32_t, rgbValues >::iterator iter_i;
 
            for (iter_i = colorMap.begin(); iter_i != colorMap.end(); iter_i++)
            {
                if (currRGB.rVal == iter_i->second.rVal && currRGB.gVal == iter_i->second.gVal && currRGB.bVal == iter_i->second.bVal)
                {
                    foundLabel = iter_i->first;
                    isFound = true;
                    break;
                }
            }
 
            if (!isFound)
            {
                colorMap[newLabel] = currRGB;
                foundLabel = newLabel;
                newLabel++;
            }
 
            targetCloudPtr->points[i].x = sourceCloudPtr->points[i].x;
            targetCloudPtr->points[i].y = sourceCloudPtr->points[i].y;
            targetCloudPtr->points[i].z = sourceCloudPtr->points[i].z;
            targetCloudPtr->points[i].label = foundLabel;
        }
    }
 
    bool visionx::PointCloudSegmenter::isPipelineStepEnabled(const Ice::Current& c)
    {
        std::unique_lock lock(enableMutex);
        return enabled;
    }
 
    armarx::TimestampBasePtr visionx::PointCloudSegmenter::getLastProcessedTimestamp(const Ice::Current& c)
    {
        std::unique_lock lock(timestampMutex);
        return new TimestampVariant(lastProcessedTimestamp);
    }
 
    void PointCloudSegmenter::setLccpParameters(const LccpParameters& parameters, const Ice::Current& c)
    {
        ARMARX_INFO << "Updating LCCP parameter setup (current segmentation method is " << segmentationMethod << ")";
        pLCCP->UpdateParameters(parameters.voxelResolution, parameters.seedResolution, parameters.colorImportance,
                                parameters.spatialImportance, parameters.normalImportance, parameters.concavityThreshold,
                                parameters.smoothnessThreshold, parameters.minSegmentSize);
    }
 
    void PointCloudSegmenter::loadLccpParametersFromFile(const std::string& filename, const Ice::Current& c)
    {
        std::string absolute_filename;
        ArmarXDataPath::getAbsolutePath(filename, absolute_filename);
 
        ARMARX_INFO << "Loading LCCP parameter setup from " << absolute_filename;
        QSettings config(QString::fromStdString(absolute_filename), QSettings::IniFormat);
 
        config.beginGroup("LCCP");
        pLCCP->UpdateParameters(
            config.value("VoxelResolution", getProperty<float>("lccpVoxelResolution").getValue()).toFloat(),
            config.value("SeedResolution", getProperty<float>("lccpSeedResolution").getValue()).toFloat(),
            config.value("ColorImportance", getProperty<float>("lccpColorImportance").getValue()).toFloat(),
            config.value("SpatialImportance", getProperty<float>("lccpSpatialImportance").getValue()).toFloat(),
            config.value("NormalImportance", getProperty<float>("lccpNormalImportance").getValue()).toFloat(),
            config.value("ConcavityThreshold", getProperty<float>("lccpConcavityThreshold").getValue()).toFloat(),
            config.value("SmoothnessThreshold", getProperty<float>("lccpSmoothnessThreshold").getValue()).toFloat(),
            config.value("MinSegmentSize", getProperty<int>("lccpMinimumSegmentSize").getValue()).toInt()
        );
        config.endGroup();
    }
 
 
    LccpParameters PointCloudSegmenter::getLccpParameters(const Ice::Current& c)
    {
        LccpParameters parameters;
 
        parameters.voxelResolution = pLCCP->voxel_resolution;
        parameters.seedResolution = pLCCP->seed_resolution;
        parameters.colorImportance = pLCCP->color_importance;
        parameters.spatialImportance = pLCCP->spatial_importance;
        parameters.normalImportance = pLCCP->normal_importance;
        parameters.concavityThreshold = pLCCP->concavity_tolerance_threshold;
        parameters.smoothnessThreshold = pLCCP->smoothness_threshold;
        parameters.minSegmentSize =  pLCCP->min_segment_size;
 
 
        return parameters;
    }
 
    void PointCloudSegmenter::setRgParameters(float smoothnessThreshold, float curvatureThreshold, const Ice::Current& c)
    {
        ARMARX_INFO << "Updating RG parameter setup";
        pRGSegmenter->UpdateRegionGrowingSegmentationParameters(smoothnessThreshold, curvatureThreshold);
    }
 
    void PointCloudSegmenter::loadRgParametersFromFile(const std::string& filename, const Ice::Current& c)
    {
        std::string absolute_filename;
        ArmarXDataPath::getAbsolutePath(filename, absolute_filename);
 
        ARMARX_INFO << "Loading RG parameter setup from " << absolute_filename;
        QSettings config(QString::fromStdString(absolute_filename), QSettings::IniFormat);
 
        config.beginGroup("RG");
        pRGSegmenter->UpdateRegionGrowingSegmentationParameters(
            config.value("SmoothnessThreshold", getProperty<double>("rgSmoothnessThreshold").getValue()).toDouble(),
            config.value("CurvatureThreshold", getProperty<double>("rgCurvatureThreshold").getValue()).toDouble()
        );
        config.endGroup();
    }
 
    void PointCloudSegmenter::createRemoteGuiTab()
    {
        GridLayout grid;
        int row = 0;
        {
            tab.rgSmoothnessThreshold.setRange(1.0, 20.0);
            tab.rgSmoothnessThreshold.setSteps(200);
            tab.rgSmoothnessThreshold.setValue(rgSmoothnessThes);
            tab.rgSmoothnessThreshold.setDecimals(3);
            grid.add(Label("rgSmoothnessThreshold:"), Pos{row, 0});
            grid.add(tab.rgSmoothnessThreshold, Pos{row, 1});
            row += 1;
        }
        {
            tab.rgCurvatureThreshold.setRange(1.0, 50.0);
            tab.rgCurvatureThreshold.setSteps(200);
            tab.rgCurvatureThreshold.setValue(rgCurvatureThres);
            tab.rgCurvatureThreshold.setDecimals(3);
            grid.add(Label("rgCurvatureThreshold:"), Pos{row, 0});
            grid.add(tab.rgCurvatureThreshold, Pos{row, 1});
            row += 1;
        }
 
        RemoteGui_createTab("PointCloudSegmenter", grid, &tab);
    }
 
    void PointCloudSegmenter::RemoteGui_update()
    {
        rgSmoothnessThes = tab.rgSmoothnessThreshold.getValue();
        rgCurvatureThres = tab.rgCurvatureThreshold.getValue();
    }
 
}