SegmentSpatialRelations.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package VisionX::ArmarXObjects::SegmentSpatialRelations
17  * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
18  * @date 2019
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include <VirtualRobot/VirtualRobot.h>
24 
26 
27 #include <SemanticObjectRelations/Shapes.h>
28 #include <SemanticObjectRelations/Spatial.h>
29 #include <SemanticObjectRelations/RelationGraph/json.h>
30 
32 
35 
36 
37 namespace visionx
38 {
39 
42  {
43  defineOptionalProperty<std::string>("ice.DebugObserverName", "DebugObserver",
44  "Name of the topic the DebugObserver listens to.");
45 
46  defineOptionalProperty<std::string>("ice.SemanticGraphTopicName", "SemanticGraphTopic",
47  "Name of the topic on which attributed graphs are reported.");
48 
49  defineOptionalProperty<std::string>("ice.GraphName", "Spatial",
50  "Name to use when reporting the extracted graph.");
51 
52  defineOptionalProperty<float>("aabb.OutlierRate", 0.01f, "Allowed outliers for AABBs.");
53  }
54 
55 
57  {
58  return "SegmentSpatialRelations";
59  }
60 
61 
63  {
64  offeringTopicFromProperty("ice.DebugObserverName");
65  offeringTopicFromProperty("ice.SemanticGraphTopicName");
66  debugDrawer.offeringTopic(*this);
67  }
68 
69 
71  {
72  getTopicFromProperty(debugObserver, "ice.DebugObserverName");
73  getTopicFromProperty(graphStorage, "ice.SemanticGraphTopicName");
74  debugDrawer.getTopic(*this);
75 
76  enableResultPointClouds<PointT>();
77  }
78 
79 
81  {
82  }
83 
85  {
86  }
87 
88 
90  {
91  // Fetch input point cloud.
92  pcl::PointCloud<PointT>::Ptr inputCloud(new pcl::PointCloud<PointT>());
93  if (waitForPointClouds())
94  {
95  getPointClouds(inputCloud);
96  }
97  else
98  {
99  ARMARX_VERBOSE << "Timeout or error while waiting for point cloud data";
100  return;
101  }
102 
103  // Do processing.
104 
105  const float outlierRate = getProperty<float>("aabb.OutlierRate");
106 
107  semrel::ShapeList shapes;
108  if (outlierRate > 0 && outlierRate < 1)
109  {
110  shapes = armarx::semantic::getShapesFromSoftAABBs(*inputCloud, outlierRate);
111  }
112  else
113  {
115  }
116 
117  const semrel::ShapeMap shapeMap = semrel::toShapeMap(shapes);
118  const semrel::SpatialGraph graph = semrel::spatial::evaluateStaticRelations(shapeMap);
119 
120  debugObserver->setDebugChannel(getName(),
121  {
122  { "Point Cloud Size", new armarx::Variant(static_cast<int>(inputCloud->size())) },
123  { "Point Cloud Time", new armarx::Variant(static_cast<int>(inputCloud->header.stamp)) },
124  { "Num Objects", new armarx::Variant(static_cast<int>(shapes.size())) },
125  });
126 
127  const std::string graphName = getProperty<std::string>("ice.GraphName");
128  graphStorage->reportGraph(graphName, armarx::semantic::toIce(graph, shapeMap));
129  }
130 
131 
133  {
135  }
136 
137 }
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::DebugDrawerTopic::offeringTopic
void offeringTopic(ManagedIceObject &component, const std::string &topicNameOverride="") const
Call offeringTopic([topicName]) on the given component.
Definition: DebugDrawerTopic.cpp:73
armarx::semantic::getShapesFromSoftAABBs
semrel::ShapeList getShapesFromSoftAABBs(const pcl::PointCloud< pcl::PointXYZL > &pointCloud, float outlierRatio)
Return the AABBs of each point cloud segment in a pointCloud.
Definition: shapes_from_aabbs.cpp:63
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
visionx::SegmentSpatialRelationsPropertyDefinitions
Property definitions of SegmentSpatialRelations.
Definition: SegmentSpatialRelations.h:44
visionx::SegmentSpatialRelations::onDisconnectPointCloudProcessor
void onDisconnectPointCloudProcessor() override
Definition: SegmentSpatialRelations.cpp:80
Pose.h
shapes_from_aabbs.h
visionx::SegmentSpatialRelations::onExitPointCloudProcessor
void onExitPointCloudProcessor() override
Definition: SegmentSpatialRelations.cpp:84
armarx::Component::offeringTopicFromProperty
void offeringTopicFromProperty(const std::string &propertyName)
Offer a topic whose name is specified by the given property.
Definition: Component.cpp:145
armarx::semantic::getShapesFromAABBs
semrel::ShapeList getShapesFromAABBs(const pcl::PointCloud< pcl::PointXYZL > &pointCloud)
Get the AABBs of each point cloud segment in pointCloud.
Definition: shapes_from_aabbs.cpp:25
armarx::semantic::toIce
data::Graph toIce(const semrel::AttributedGraph &input)
Definition: graph.cpp:17
visionx::SegmentSpatialRelations::getDefaultName
std::string getDefaultName() const override
Definition: SegmentSpatialRelations.cpp:56
armarx::Component::getTopicFromProperty
TopicProxyType getTopicFromProperty(const std::string &propertyName)
Get a topic proxy whose name is specified by the given property.
Definition: Component.h:218
SegmentSpatialRelations.h
visionx::SegmentSpatialRelations::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: SegmentSpatialRelations.cpp:132
visionx::PointCloudProcessor::waitForPointClouds
bool waitForPointClouds(int milliseconds=1000)
Wait for new PointClouds.
Definition: PointCloudProcessor.cpp:433
visionx::SegmentSpatialRelations::onConnectPointCloudProcessor
void onConnectPointCloudProcessor() override
Definition: SegmentSpatialRelations.cpp:70
visionx::PointCloudProcessorPropertyDefinitions
Properties of PointCloudProcessor.
Definition: PointCloudProcessor.h:173
visionx::PointCloudProcessor::getPointClouds
int getPointClouds(const PointCloudPtrT &pointCloudPtr)
Poll PointClouds from provider.
Definition: PointCloudProcessor.h:373
visionx::SegmentSpatialRelationsPropertyDefinitions::SegmentSpatialRelationsPropertyDefinitions
SegmentSpatialRelationsPropertyDefinitions(std::string prefix)
Definition: SegmentSpatialRelations.cpp:40
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:70
visionx::SegmentSpatialRelations::process
void process() override
Definition: SegmentSpatialRelations.cpp:89
IceUtil::Handle< class PropertyDefinitionContainer >
visionx::SegmentSpatialRelations::onInitPointCloudProcessor
void onInitPointCloudProcessor() override
Definition: SegmentSpatialRelations.cpp:62
armarx::ManagedIceObject::getName
std::string getName() const
Retrieve name of object.
Definition: ManagedIceObject.cpp:107
ice_serialization.h
armarx::DebugDrawerTopic::getTopic
DebugDrawerInterfacePrx getTopic() const
Get the topic.
Definition: DebugDrawerTopic.cpp:58
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
shapes
MiscLib::Vector< std::pair< MiscLib::RefCountPtr< PrimitiveShape >, size_t > > shapes
Definition: ReadMe.txt:92