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 "SegmentAABBShapesProvider.h"
 
#include <VirtualRobot/VirtualRobot.h>
 
#include <RobotAPI/libraries/core/Pose.h>
 
#include <VisionX/libraries/SemanticObjectRelations/ice_serialization.h>
#include <VisionX/libraries/SemanticObjectRelations/shapes_from_aabbs.h>
 
#include <SemanticObjectRelations/RelationGraph/json.h>
#include <SemanticObjectRelations/Shapes.h>
#include <SemanticObjectRelations/Spatial.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()));
    }
 
} // namespace visionx