SegmentAABBShapesProvider.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::SegmentAABBShapesProvider
 * @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 "SegmentAABBShapesProvider.h"
 
#include <SemanticObjectRelations/Shapes.h>
#include <SemanticObjectRelations/Spatial.h>
#include <SemanticObjectRelations/RelationGraph/json.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
#include <VisionX/libraries/SemanticObjectRelations/shapes_from_aabbs.h>
#include <VisionX/libraries/SemanticObjectRelations/ice_serialization.h>
 
 
namespace visionx
{
 
    SegmentAABBShapesProviderPropertyDefinitions::SegmentAABBShapesProviderPropertyDefinitions(std::string prefix) :
        visionx::PointCloudProcessorPropertyDefinitions(prefix)
    {
        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", "SegmentAABBs",
                                            "Name to use when reporting the extracted shapes.");
 
        defineOptionalProperty<float>("aabb.OutlierRate", 0.01f, "Allowed outliers for soft AABBs.")
        .setMin(0).setMax(0.5);
 
        defineOptionalProperty<bool>("visu.Enabled", false, "Enable visualization of result.");
    }
 
 
    std::string SegmentAABBShapesProvider::getDefaultName() const
    {
        return "SegmentAABBShapesProvider";
    }
 
 
    void SegmentAABBShapesProvider::onInitPointCloudProcessor()
    {
        offeringTopicFromProperty("ice.ShapesTopicName");
 
        offeringTopicFromProperty("ice.DebugObserverName");
        debugDrawer.offeringTopic(*this);
    }
 
 
    void SegmentAABBShapesProvider::onConnectPointCloudProcessor()
    {
        getTopicFromProperty(shapesTopic, "ice.ShapesTopicName");
 
        getTopicFromProperty(debugObserver, "ice.DebugObserverName");
        debugDrawer.getTopic(*this);
    }
 
 
    void SegmentAABBShapesProvider::onDisconnectPointCloudProcessor()
    {
    }
 
    void SegmentAABBShapesProvider::onExitPointCloudProcessor()
    {
    }
 
 
    void SegmentAABBShapesProvider::process()
    {
        // Fetch input point cloud.
        pcl::PointCloud<PointT>::Ptr inputCloud(new pcl::PointCloud<PointT>());
        if (waitForPointClouds())
        {
            getPointClouds(inputCloud);
        }
        else
        {
            ARMARX_VERBOSE << "Timeout or error while waiting for point cloud data";
            return;
        }
 
        // Do processing.
 
        const float outlierRate = getProperty<float>("aabb.OutlierRate");
 
        semrel::ShapeList shapes;
        if (outlierRate > 0 && outlierRate < 1)
        {
            shapes = armarx::semantic::getShapesFromSoftAABBs(*inputCloud, outlierRate);
        }
        else
        {
            shapes = armarx::semantic::getShapesFromAABBs(*inputCloud);
        }
 
        const semrel::ShapeMap shapeMap = semrel::toShapeMap(shapes);
        const semrel::SpatialGraph graph = semrel::spatial::evaluateStaticRelations(shapeMap);
 
        debugObserver->setDebugChannel(getName(),
        {
            { "Point Cloud Size", new armarx::Variant(static_cast<int>(inputCloud->size())) },
            { "Point Cloud Time", new armarx::Variant(static_cast<int>(inputCloud->header.stamp)) },
            { "Num Objects", new armarx::Variant(static_cast<int>(shapes.size())) },
        });
 
        const std::string shapesName = getProperty<std::string>("ice.ShapesName");
        shapesTopic->reportShapes(shapesName, armarx::semantic::toIce(shapes));
 
        if (getProperty<bool>("visu.Enabled"))
        {
            for (const auto& [id, box] : shapeMap)
            {
                std::stringstream ss;
                ss << id;
                debugDrawer.drawBox({ getName(), ss.str() },
                                    box->getPosition(), box->getOrientation(), box->getAABB().extents(),
                                    debugDrawer.getGlasbeyLUTColor(int(id), 0.5));
            }
        }
    }
 
 
    armarx::PropertyDefinitionsPtr SegmentAABBShapesProvider::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new SegmentAABBShapesProviderPropertyDefinitions(getConfigIdentifier()));
    }
 
}