SegmentRansacShapeExtractor.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * 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::SegmentRansacShapeExtractor
 * @author     Rainer Kartmann ( rainer dot kartmann at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include <VirtualRobot/VirtualRobot.h>
 
#include "SegmentRansacShapeExtractor.h"
 
#include <SemanticObjectRelations/Shapes.h>
#include <SemanticObjectRelations/Hooks/VisualizerInterface.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
#include <VisionX/libraries/SemanticObjectRelations/hooks.h>
#include <VisionX/libraries/SemanticObjectRelations/ice_serialization/shape.h>
 
 
namespace visionx
{
 
    SegmentRansacShapeExtractorPropertyDefinitions::SegmentRansacShapeExtractorPropertyDefinitions(std::string prefix) :
        visionx::PointCloudProcessorPropertyDefinitions(prefix)
    {
        // ICE
 
        defineOptionalProperty<std::string>("ice.DebugObserverName", "DebugObserver",
                                            "Name of the topic the DebugObserver listens to.");
 
        defineOptionalProperty<std::string>("ice.ShapesTopicName", "ShapesTopic",
                                            "Name of the topic on which shapes are reported.");
        defineOptionalProperty<std::string>("ice.ShapesName", "SegmentRansacShapes",
                                            "Name to use when reporting the extracted shapes.");
 
 
        // EXTRACTION
 
        defineOptionalProperty<bool>(
            "mode.AsPipeline", false,
            "Extract shapes on each new point cloud and publish them to the shapes topic.");
 
        defineOptionalProperty<std::string>(
            "ransac.TargetShapes", "box, cylinder, sphere",
            "Shapes to be extracted (any combination of: box, cylinder, sphere).");
 
        defineOptionalProperty<float>(
            "ransac.maxModelError", 1000.,
            "Discard a model if its error is above this threshold.")
        .setMin(0.);
 
        defineOptionalProperty<int>(
            "ransac.maxIterations", 100,
            "Maximal number of iterations of each RANSAC run.")
        .setMin(0);
 
        defineOptionalProperty<float>(
            "ransac.distanceThresholdBox", 5.,
            "A point with a distance below this threshold is considererd an inlier. (Box)")
        .setMin(0.);
        defineOptionalProperty<float>(
            "ransac.distanceThresholdPCL", 0.5,
            "A point with a distance below this threshold is considererd an inlier. "
            "(PCL, i.e. cylinder and sphere)")
        .setMin(0.);
        defineOptionalProperty<float>(
            "ransac.outlierRate", 0.0125f,
            "The percentage of points that may be considered as outliers and may be ignored for fitting. "
            "This affects box extents, cylinder height, and error computation.")
        .setMin(0).setMax(1.);
        defineOptionalProperty<float>(
            "ransac.minInlierRate", 0.75,
            "A model is considered a good fit, if this fraction of all points are "
            "inliers for that model. (0.0 ... 1.0)")
        .setMin(0.).setMax(1.0);
        defineOptionalProperty<float>(
            "ransac.sizePenaltyFactor", 0.000125f,
            "Weight of the size penalty (relative to distance error). "
            "Size penalty inhibits shapes with huge radius (cyl, sph) or extents (box)."
            "Set to 0 to disable size penalty.")
        .setMin(0);
        defineOptionalProperty<float>(
            "ransac.sizePenaltyExponent", 2.0,
            "Exponent of the of the size penalty (as in s^x, with size measure s and exponent x). "
            "Size penalty inhibits shapes with huge radius (cyl, sph) or extents (box).")
        .setMin(0);
 
 
        // VISUALIZATION
 
        armarx::semantic::properties::defineVisualizationLevel(
            *this, armarx::semantic::properties::defaults::visualizationLevelName,
            semrel::VisuLevel::RESULT);
 
    }
 
 
    void SegmentRansacShapeExtractor::onInitPointCloudProcessor()
    {
        offeringTopicFromProperty("ice.ShapesTopicName");
        getProperty(this->shapesName, "ice.ShapesName");
 
        offeringTopicFromProperty("ice.DebugObserverName");
        debugDrawer.offeringTopic(*this);
 
 
        // Initialize shapeExtraction with parameters.
        shapeExtraction.setMaxModelError(getProperty<float>("ransac.maxModelError"));
        shapeExtraction.setMaxIterations(getProperty<int>("ransac.maxIterations"));
 
        shapeExtraction.setDistanceThresholdBox(getProperty<float>("ransac.distanceThresholdBox"));
        shapeExtraction.setDistanceThresholdPCL(getProperty<float>("ransac.distanceThresholdPCL"));
 
        shapeExtraction.setOutlierRate(getProperty<float>("ransac.outlierRate"));
 
        shapeExtraction.setMinInlierRate(getProperty<float>("ransac.minInlierRate"));
 
        shapeExtraction.setSizePenaltyFactor(getProperty<float>("ransac.sizePenaltyFactor"));
        shapeExtraction.setSizePenaltyExponent(getProperty<float>("ransac.sizePenaltyExponent"));
 
        // Target shapes
        {
            std::string targetShapes = getProperty<std::string>("ransac.TargetShapes");
 
            // Make targetShapes all lowercase
            std::transform(targetShapes.begin(), targetShapes.end(), targetShapes.begin(), ::tolower);
 
            // find != npos <=> string contained
            shapeExtraction.setBoxExtractionEnabled(targetShapes.find("box") != std::string::npos);
            shapeExtraction.setCylinderExtractionEnabled(targetShapes.find("cylinder") != std::string::npos);
            shapeExtraction.setSphereExtractionEnabled(targetShapes.find("sphere") != std::string::npos);
        }
    }
 
 
    void SegmentRansacShapeExtractor::onConnectPointCloudProcessor()
    {
        getTopicFromProperty(shapesTopic, "ice.ShapesTopicName");
 
        getTopicFromProperty(debugObserver, "ice.DebugObserverName");
        debugDrawer.getTopic(*this);
 
        armarx::semantic::setArmarXHooksAsImplementation(debugDrawer, getName());
        armarx::semantic::properties::setMinimumVisuLevel(*this);
    }
 
 
    void SegmentRansacShapeExtractor::onDisconnectPointCloudProcessor()
    {
    }
 
    void SegmentRansacShapeExtractor::onExitPointCloudProcessor()
    {
    }
 
 
    void SegmentRansacShapeExtractor::process()
    {
        // Fetch input point cloud.
        pcl::PointCloud<PointT>::Ptr receivedPointCloud(new pcl::PointCloud<PointT>());
        if (waitForPointClouds(10000))
        {
            if (!getPointClouds(receivedPointCloud))
            {
                ARMARX_WARNING << "Unable to get point cloud data.";
                return;
            }
        }
        else
        {
            ARMARX_VERBOSE << "Timeout or error while waiting for pointclouds.";
            return;
        }
 
        if (debugObserver)
        {
            debugObserver->setDebugChannel(getName(),
            {
                { "Point Cloud Size", new armarx::Variant(static_cast<int>(receivedPointCloud->size())) },
                { "Point Cloud Time", new armarx::Variant(static_cast<int>(receivedPointCloud->header.stamp)) },
            });
        }
 
        // Store new point cloud
        {
            std::scoped_lock lock(inputPointCloudMutex);
            this->inputPointCloud = receivedPointCloud;
        }
 
        if (getProperty<bool>("mode.AsPipeline"))
        {
            armarx::semantic::data::ShapeList shapes;
            {
                std::scoped_lock lockInputPointCloud(inputPointCloudMutex, extractionMutex);
 
                ARMARX_INFO << "Running shape extraction (" << inputPointCloud->size() << " points) ...";
                const semrel::ShapeExtractionResult extractionResult = shapeExtraction.extract(*inputPointCloud);
                shapes = armarx::semantic::toIce(extractionResult.objects);
            }
            if (debugObserver)
            {
                debugObserver->setDebugChannel(getName(),
                {
                    { "Num Extracted Shapes", new armarx::Variant(static_cast<int>(shapes.size())) },
                });
            }
            shapesTopic->reportShapes(shapesName, shapes);
        }
    }
 
 
    armarx::semantic::data::ShapeList SegmentRansacShapeExtractor::extractShapes(const Ice::Current&)
    {
        if (!inputPointCloud)
        {
            ARMARX_WARNING << "No point cloud received. Not running shape extraction.";
            return {};
        }
 
        // Make sure input point cloud is not being read.
        std::scoped_lock lockInputPointCloud(inputPointCloudMutex, extractionMutex);
 
        ARMARX_INFO << "Running shape extraction (" << inputPointCloud->size() << " points) ...";
        semrel::ShapeExtractionResult extractionResult = shapeExtraction.extract(*inputPointCloud);
 
        armarx::semantic::data::ShapeList result;
        // Store the result.
        {
            std::scoped_lock lock(extractedShapes.mutex);
 
            extractedShapes.shapes = std::move(extractionResult.objects);
            extractedShapes.shapesIce = armarx::semantic::toIce(extractedShapes.shapes);
 
            ARMARX_INFO << "Extracted " << extractedShapes.shapes.size() << " shapes. \n"
                        << extractedShapes.shapes;
 
            result = extractedShapes.shapesIce;
        }
 
        ARMARX_VERBOSE << "Publishing shapes... ";
        debugObserver->setDebugChannel(getName(),
        {
            { "Num Extracted Shapes", new armarx::Variant(static_cast<int>(result.size())) },
        });
 
        return result;
    }
 
    armarx::semantic::data::ShapeList SegmentRansacShapeExtractor::getExtractedShapes(const Ice::Current&)
    {
        std::lock_guard<std::mutex> lock(extractedShapes.mutex);
        return extractedShapes.shapesIce;
    }
 
 
    std::string SegmentRansacShapeExtractor::getDefaultName() const
    {
        return "SegmentRansacShapeExtractor";
    }
 
 
    armarx::PropertyDefinitionsPtr SegmentRansacShapeExtractor::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new SegmentRansacShapeExtractorPropertyDefinitions(getConfigIdentifier()));
    }
 
}