PrimitiveExtractor.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package VisionX
19  * @author Eren Aksoy ( eren dot aksoy at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #pragma once
26 
27 #include <tuple>
28 #include <limits>
29 #include <cmath>
30 #include <map>
31 #include <stdarg.h>
32 #include <functional>
33 
34 #include <IceUtil/IceUtil.h>
35 
36 #include <Eigen/Core>
37 
39 
40 #include <RobotAPI/interface/core/PoseBase.h>
41 #include <RobotAPI/interface/core/RobotState.h>
43 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
44 
46 #include <MemoryX/interface/components/WorkingMemoryInterface.h>
49 
52 #include <VisionX/interface/components/PointCloudSegmenter.h>
53 #include <VisionX/interface/components/PrimitiveExtractor.h>
54 #include <VisionX/interface/components/PrimitiveMapper.h>
55 
56 #include <pcl/features/moment_of_inertia_estimation.h>
57 #include <pcl/common/angles.h>
58 #include <pcl/kdtree/kdtree.h>
59 #include <pcl/kdtree/kdtree_flann.h>
60 #include <pcl/common/intersections.h>
61 #include <pcl/common/geometry.h>
62 
63 #include "PrimitiveFilter.h"
64 #include "PrimitiveFusion.h"
65 
66 #include <SimoxUtility/algorithm/string/string_tools.h>
67 
68 #include <mutex>
69 
70 namespace pcl::console
71 {
72  // Replace PCL's print function in order to pass the messages to the ArmarX logging system.
73  // TODO: This should be done automatically if PCL is linked into a component
74  void print(pcl::console::VERBOSITY_LEVEL level, const char* format, ...)
75  {
76  std::string message;
77 
78  va_list args, args_copy;
79  va_start(args, format);
80  va_copy(args_copy, args);
81 
82  int size = std::vsnprintf(nullptr, 0, format, args) + 1;
83  message = std::string(size, ' ');
84  std::vsnprintf(&message.front(), size, format, args_copy);
85 
86  va_end(args_copy);
87  va_end(args);
88 
89  simox::alg::trim(message);
90 
91  switch (level)
92  {
93  case L_ALWAYS:
94  case L_ERROR:
95  case L_WARN:
97  break;
98 
99  case L_INFO:
100  case L_DEBUG:
101  case L_VERBOSE:
102  // Ignore
103  break;
104  }
105  }
106 }
107 
108 
109 namespace AffordanceKit
110 {
111  class PrimitiveExtractor;
112  using PrimitiveExtractorPtr = std::shared_ptr<PrimitiveExtractor>;
113 }
114 
115 namespace armarx
116 {
117  /**
118  * @class PrimitiveExtractorPropertyDefinitions
119  * @brief
120  */
123  {
124  public:
127  {
128  defineOptionalProperty<std::string>("segmenterName", "PointCloudSegmenterResult", "name of the segmented point cloud data processor");
129 
130  // required primitive parameters
131  defineOptionalProperty<double>("minimumSegmentSize", 100, "Minimum segment size to be considered in the primitive extraction");
132  defineOptionalProperty<double>("maximumSegmentSize", 75000, "Maximum segment size to be considered in the primitive extraction");
133  defineOptionalProperty<double>("planeMaximumIteration", 100, "Maximum ransac iteration for plane estimation");
134  defineOptionalProperty<float>("planeDistanceThreshold", 10.0, "Distance threshold for plane estimation");
135  defineOptionalProperty<float>("planeNormalDistance", 0.10, "Normal distance for plane estimation");
136  defineOptionalProperty<double>("cylinderMaximumIteration", 100, "Maximum ransac iteration for cylinder estimation");
137  defineOptionalProperty<float>("cylinderDistanceThreshold", 0.1, "Distance threshold for cylinder estimation");
138  defineOptionalProperty<float>("cylinderRadiousLimit", 0.1, "Radious limit for cylinder estimation");
139  defineOptionalProperty<double>("sphereMaximumIteration", 100, "Maximum ransac iteration for sphere estimation");
140  defineOptionalProperty<float>("sphereDistanceThreshold", 0.9, "Distance threshold for sphere estimation");
141  defineOptionalProperty<float>("sphereNormalDistance", 0.1, "Normal distance for sphere estimation");
142  defineOptionalProperty<bool>("vebose", false, "Required for debugging the primitive engine");
143  defineOptionalProperty<float>("euclideanClusteringTolerance", 50.0, "Euclidean distance threshold for fine clustering");
144  defineOptionalProperty<float>("outlierThreshold", 50.0, "Outlier threshold for computing model outliers");
145  defineOptionalProperty<float>("circularDistanceThreshold", 0.01, "Distance threshold for circle estimation");
146  defineOptionalProperty<bool>("enablePrimitiveFusion", false, "Fuse new primitives with existing ones");
147  defineOptionalProperty<bool>("UseAffordanceExtractionLibExport", false, "Use export functionality of affordance extraction lib");
148  defineOptionalProperty<float>("SpatialSamplingDistance", 50, "Distance between two spatial samplings in mm");
149  defineOptionalProperty<int>("NumOrientationalSamplings", 4, "Number of orientational samplings");
150  defineOptionalProperty<bool>("EnableBoxPrimitives", false, "Toggle detection of box primitives");
151 
152  defineOptionalProperty<std::string>("sourceFrameName", "DepthCamera", "the robot node to use as source coordinate system for the captured point clouds");
153  defineOptionalProperty<std::string>("SegmentedPointCloudTopicName", "SegmentedPointCloud", "name of SegmentedPointCloud topic");
154  defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the robot state component that should be used");
155 
156  defineOptionalProperty<std::string>("PriorKnowledgeProxyName", "PriorKnowledge", "name of prior memory proxy");
157  defineOptionalProperty<std::string>("WorkingMemoryName", "WorkingMemory", "name of WorkingMemory component");
158  defineOptionalProperty<std::string>("DebugDrawerTopicName", "DebugDrawerUpdates", "name of DebugDrawer topic");
159  defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver", "Name of the topic the DebugObserver listens on");
160  }
161  };
162 
163  /**
164  * @class PrimitiveExtractor
165  *
166  * @ingroup VisionX-Components
167  * @brief A brief description
168  *
169  *
170  * Detailed Description
171  */
173  virtual public visionx::PrimitiveMapperInterface,
174  virtual public visionx::PointCloudProcessor
175  {
176  public:
177  /**
178  * @see armarx::ManagedIceObject::getDefaultName()
179  */
180  std::string getDefaultName() const override
181  {
182  return "PrimitiveExtractor";
183  }
184 
185  protected:
186  /**
187  * @see visionx::PointCloudProcessor::onInitPointCloudProcessor()
188  */
189  void onInitPointCloudProcessor() override;
190 
191  /**
192  * @see visionx::PointCloudProcessor::onConnectPointCloudProcessor()
193  */
194  void onConnectPointCloudProcessor() override;
195 
196  /**
197  * @see visionx::PointCloudProcessor::onDisconnectPointCloudProcessor()
198  */
199  void onDisconnectPointCloudProcessor() override;
200 
201  /**
202  * @see visionx::PointCloudProcessor::onExitPointCloudProcessor()
203  */
204  void onExitPointCloudProcessor() override;
205 
206  /**
207  * @see visionx::PointCloudProcessor::process()
208  */
209  void process() override;
210 
211  void enablePipelineStep(const Ice::Current& c = Ice::emptyCurrent) override;
212  void disablePipelineStep(const Ice::Current& c = Ice::emptyCurrent) override;
213  bool isPipelineStepEnabled(const Ice::Current& c = Ice::emptyCurrent) override;
214  armarx::TimestampBasePtr getLastProcessedTimestamp(const Ice::Current& c = Ice::emptyCurrent) override;
215 
216  void setParameters(const visionx::PrimitiveExtractorParameters& parameters, const Ice::Current& c = Ice::emptyCurrent) override;
217  void loadParametersFromFile(const std::string& filename, const Ice::Current& c = Ice::emptyCurrent) override;
218 
219  visionx::PrimitiveExtractorParameters getParameters(const Ice::Current& c = Ice::emptyCurrent) override;
220 
221  /**
222  * @see PropertyUser::createPropertyDefinitions()
223  */
225 
226  private:
227 
228  void fusePrimitives(std::vector<memoryx::EntityBasePtr>& newPrimitives, long timestamp);
229 
230  void extractPrimitives();
231  float getGraspPointInlierRatio(memoryx::EnvironmentalPrimitiveBasePtr leftPrimitive, memoryx::EnvironmentalPrimitiveBasePtr rightPrimitive);
232 
233  void pushPrimitivesToMemory(IceUtil::Int64 originalTimestamp);
234  void pushPrimitivesToMemoryAffordanceKit(IceUtil::Int64 originalTimestamp);
235 
236  private:
237  AffordanceKit::PrimitiveExtractorPtr primitiveExtractor;
238 
239  bool isConnectedtoExtractor;
240  double minSegmentSize;
241  double maxSegmentSize;
242  double pMaxIter;
243  float pDistThres;
244  float pNorDist;
245  double cMaxIter;
246  float cDistThres;
247  float cRadLim;
248  double sMaxIter;
249  float sDistThres;
250  float sNorDist;
251  bool verbose;
252  float euclideanClusteringTolerance;
253  float outlierThreshold;
254  float circleDistThres;
255 
256  // init point clouds
257  pcl::PointCloud<pcl::PointXYZL>::Ptr labeledCloudPtr;
258  pcl::PointCloud<pcl::PointXYZL>::Ptr inliersCloudPtr;
259  pcl::PointCloud<pcl::PointXYZL>::Ptr graspCloudPtr;
260 
261  std::mutex mutexPushing;
262  std::mutex pointCloudMutex;
263  std::mutex enableMutex;
264 
265  std::mutex timestampMutex;
266  long lastProcessedTimestamp;
267 
268  bool isConnectedtoSegmenter;
269 
270  std::string providerName;
271 
272  memoryx::WorkingMemoryInterfacePrx workingMemory;
273  memoryx::EnvironmentalPrimitiveSegmentBasePrx environmentalPrimitiveSegment;
274  visionx::PointCloudSegmentationListenerPrx pointCloudSegmentationPrx;
275 
276  std::string sourceFrameName;
277  std::string agentName;
278 
279  DebugDrawerInterfacePrx debugDrawerTopicPrx;
280 
281  int lastUsedLabel;
282 
283  bool mappingEnabled;
284  bool enablePrimitiveFusion;
285 
286  armarx::RobotStateComponentInterfacePrx robotStateComponent;
287 
288  PrimitiveFusion fusionStrategy;
289  PrimitiveFilter primitiveFilter;
290 
291  size_t numViews;
292 
293  float spatialSamplingDistance;
294  int numOrientationalSamplings;
295 
296  DebugObserverInterfacePrx debugObserver;
297  };
298 }
299 
300 
301 
armarx::PrimitiveExtractor::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: PrimitiveExtractor.cpp:909
armarx::PrimitiveFilter
A brief description.
Definition: PrimitiveFilter.h:48
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:333
armarx::PrimitiveExtractor::onConnectPointCloudProcessor
void onConnectPointCloudProcessor() override
Definition: PrimitiveExtractor.cpp:106
PointCloudProvider.h
armarx::PrimitiveExtractor::getParameters
visionx::PrimitiveExtractorParameters getParameters(const Ice::Current &c=Ice::emptyCurrent) override
Definition: PrimitiveExtractor.cpp:966
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
message
message(STATUS "Boost-Library-Dir: " "${Boost_LIBRARY_DIRS}") message(STATUS "Boost-LIBRARIES
Definition: CMakeLists.txt:8
EnvironmentalPrimitiveSegment.h
armarx::PrimitiveExtractor::isPipelineStepEnabled
bool isPipelineStepEnabled(const Ice::Current &c=Ice::emptyCurrent) override
Definition: PrimitiveExtractor.cpp:914
pcl::console
Definition: PrimitiveExtractor.h:70
armarx::PrimitiveExtractorPropertyDefinitions::PrimitiveExtractorPropertyDefinitions
PrimitiveExtractorPropertyDefinitions(std::string prefix)
Definition: PrimitiveExtractor.h:125
PrimitiveFilter.h
pcl::console::print
void print(pcl::console::VERBOSITY_LEVEL level, const char *format,...)
Definition: PrimitiveExtractor.h:74
armarx::PrimitiveExtractor
A brief description.
Definition: PrimitiveExtractor.h:172
PrimitiveFusion.h
armarx::PrimitiveExtractor::enablePipelineStep
void enablePipelineStep(const Ice::Current &c=Ice::emptyCurrent) override
Definition: PrimitiveExtractor.cpp:167
armarx::PrimitiveExtractor::process
void process() override
Definition: PrimitiveExtractor.cpp:179
visionx::PointCloudProcessor
The PointCloudProcessor class provides an interface for access to PointCloudProviders via Ice and sha...
Definition: PointCloudProcessor.h:186
armarx::PrimitiveExtractor::getDefaultName
std::string getDefaultName() const override
Definition: PrimitiveExtractor.h:180
FramedPose.h
MemoryXCoreObjectFactories.h
filename
std::string filename
Definition: VisualizationRobot.cpp:83
armarx::PrimitiveFusion
A brief description.
Definition: PrimitiveFusion.h:42
armarx::PrimitiveExtractor::disablePipelineStep
void disablePipelineStep(const Ice::Current &c=Ice::emptyCurrent) override
Definition: PrimitiveExtractor.cpp:173
PointCloudProcessor.h
Component.h
armarx::PrimitiveExtractor::onInitPointCloudProcessor
void onInitPointCloudProcessor() override
Definition: PrimitiveExtractor.cpp:52
armarx::PrimitiveExtractor::onDisconnectPointCloudProcessor
void onDisconnectPointCloudProcessor() override
Definition: PrimitiveExtractor.cpp:158
armarx::PrimitiveExtractor::loadParametersFromFile
void loadParametersFromFile(const std::string &filename, const Ice::Current &c=Ice::emptyCurrent) override
Definition: PrimitiveExtractor.cpp:936
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
AffordanceKit
Definition: PrimitiveExtractor.h:109
armarx::PrimitiveExtractorPropertyDefinitions
Definition: PrimitiveExtractor.h:121
IceUtil::Handle< class PropertyDefinitionContainer >
IceInternal::ProxyHandle<::IceProxy::armarx::DebugDrawerInterface >
MemoryXTypesObjectFactories.h
armarx::PrimitiveExtractor::getLastProcessedTimestamp
armarx::TimestampBasePtr getLastProcessedTimestamp(const Ice::Current &c=Ice::emptyCurrent) override
Definition: PrimitiveExtractor.cpp:920
ARMARX_INFO_S
#define ARMARX_INFO_S
Definition: Logging.h:195
armarx::PrimitiveExtractor::setParameters
void setParameters(const visionx::PrimitiveExtractorParameters &parameters, const Ice::Current &c=Ice::emptyCurrent) override
Definition: PrimitiveExtractor.cpp:926
armarx::PrimitiveExtractor::onExitPointCloudProcessor
void onExitPointCloudProcessor() override
Definition: PrimitiveExtractor.cpp:163
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
AffordanceKit::PrimitiveExtractorPtr
std::shared_ptr< PrimitiveExtractor > PrimitiveExtractorPtr
Definition: PrimitiveExtractor.h:112