SegmentSpatialRelations.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::SegmentSpatialRelations
 * @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 "SegmentSpatialRelations.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
{
 
    SegmentSpatialRelationsPropertyDefinitions::SegmentSpatialRelationsPropertyDefinitions(std::string prefix) :
        visionx::PointCloudProcessorPropertyDefinitions(prefix)
    {
        defineOptionalProperty<std::string>("ice.DebugObserverName", "DebugObserver",
                                            "Name of the topic the DebugObserver listens to.");
 
        defineOptionalProperty<std::string>("ice.SemanticGraphTopicName", "SemanticGraphTopic",
                                            "Name of the topic on which attributed graphs are reported.");
 
        defineOptionalProperty<std::string>("ice.GraphName", "Spatial",
                                            "Name to use when reporting the extracted graph.");
 
        defineOptionalProperty<float>("aabb.OutlierRate", 0.01f, "Allowed outliers for AABBs.");
    }
 
 
    std::string SegmentSpatialRelations::getDefaultName() const
    {
        return "SegmentSpatialRelations";
    }
 
 
    void SegmentSpatialRelations::onInitPointCloudProcessor()
    {
        offeringTopicFromProperty("ice.DebugObserverName");
        offeringTopicFromProperty("ice.SemanticGraphTopicName");
        debugDrawer.offeringTopic(*this);
    }
 
 
    void SegmentSpatialRelations::onConnectPointCloudProcessor()
    {
        getTopicFromProperty(debugObserver, "ice.DebugObserverName");
        getTopicFromProperty(graphStorage, "ice.SemanticGraphTopicName");
        debugDrawer.getTopic(*this);
 
        enableResultPointClouds<PointT>();
    }
 
 
    void SegmentSpatialRelations::onDisconnectPointCloudProcessor()
    {
    }
 
    void SegmentSpatialRelations::onExitPointCloudProcessor()
    {
    }
 
 
    void SegmentSpatialRelations::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 graphName = getProperty<std::string>("ice.GraphName");
        graphStorage->reportGraph(graphName, armarx::semantic::toIce(graph, shapeMap));
    }
 
 
    armarx::PropertyDefinitionsPtr SegmentSpatialRelations::createPropertyDefinitions()
    {
        return armarx::PropertyDefinitionsPtr(new SegmentSpatialRelationsPropertyDefinitions(getConfigIdentifier()));
    }
 
}