PrimitiveExtractor.cpp
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 #include "PrimitiveExtractor.h"
26 
27 #include <boost/lexical_cast.hpp>
28 
32 
34 
35 #include <AffordanceKit/primitives/Plane.h>
36 #include <AffordanceKit/primitives/Cylinder.h>
37 #include <AffordanceKit/primitives/Sphere.h>
38 #include <AffordanceKit/primitives/Box.h>
39 #include <AffordanceKit/PrimitiveExtractor.h>
40 
41 #include <pcl/common/colors.h>
42 
43 #include <QSettings>
44 
45 #include <set>
46 
47 using namespace memoryx;
48 using namespace armarx;
49 using namespace pcl;
50 
51 
52 void PrimitiveExtractor::onInitPointCloudProcessor()
53 {
54  lastProcessedTimestamp = 0;
55 
56  providerName = getProperty<std::string>("segmenterName").getValue();
57 
58  // get required primitive parameters
59  minSegmentSize = getProperty<double>("minimumSegmentSize").getValue();
60  maxSegmentSize = getProperty<double>("maximumSegmentSize").getValue();
61  pMaxIter = getProperty<double>("planeMaximumIteration").getValue();
62  pDistThres = getProperty<float>("planeDistanceThreshold").getValue();
63  pNorDist = getProperty<float>("planeNormalDistance").getValue();
64  cMaxIter = getProperty<double>("cylinderMaximumIteration").getValue();
65  cDistThres = getProperty<float>("cylinderDistanceThreshold").getValue();
66  cRadLim = getProperty<float>("cylinderRadiousLimit").getValue();
67  sMaxIter = getProperty<double>("sphereMaximumIteration").getValue();
68  sDistThres = getProperty<float>("sphereDistanceThreshold").getValue();
69  sNorDist = getProperty<float>("sphereNormalDistance").getValue();
70  verbose = getProperty<bool>("vebose").getValue();
71  euclideanClusteringTolerance = getProperty<float>("euclideanClusteringTolerance").getValue();
72  outlierThreshold = getProperty<float>("outlierThreshold").getValue();
73  circleDistThres = getProperty<float>("circularDistanceThreshold").getValue();
74 
75  spatialSamplingDistance = getProperty<float>("SpatialSamplingDistance").getValue();
76  numOrientationalSamplings = getProperty<int>("NumOrientationalSamplings").getValue();
77 
78  // reset all point cloud pointers
79  labeledCloudPtr.reset(new PointCloud<PointXYZL>);
80  inliersCloudPtr.reset(new PointCloud<PointXYZL>);
81  graspCloudPtr.reset(new PointCloud<PointXYZL>);
82 
83  // init the primitive class
84  primitiveExtractor.reset(new AffordanceKit::PrimitiveExtractor());
85  primitiveExtractor->updateParameters(verbose, minSegmentSize, maxSegmentSize, pMaxIter, pDistThres, pNorDist, cMaxIter, cDistThres, cRadLim, sMaxIter, sDistThres, sNorDist, euclideanClusteringTolerance, outlierThreshold, circleDistThres);
86  usingPointCloudProvider(providerName);
87 
88  if (getProperty<std::string>("RobotStateComponentName").getValue() != "")
89  {
90  usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
91  sourceFrameName = getProperty<std::string>("sourceFrameName").getValue();
92  }
93  usingProxy(getProperty<std::string>("WorkingMemoryName").getValue());
94 
95  mappingEnabled = true;
96 
97  enablePrimitiveFusion = getProperty<bool>("enablePrimitiveFusion").getValue();
98 
99  offeringTopic(getProperty<std::string>("DebugDrawerTopicName").getValue());
100  offeringTopic(getProperty<std::string>("SegmentedPointCloudTopicName").getValue());
101 
102  offeringTopic(getProperty<std::string>("DebugObserverName").getValue());
103 }
104 
105 
106 void PrimitiveExtractor::onConnectPointCloudProcessor()
107 {
108  enableResultPointClouds<PointXYZL>("GraspPointCloudResult");
109  enableResultPointClouds<PointXYZL>("PrimitiveExtractorResult");
110  enableResultPointClouds<PointXYZL>("Inliers");
111 
112  workingMemory = getProxy<memoryx::WorkingMemoryInterfacePrx>(getProperty<std::string>("WorkingMemoryName").getValue());
113  if (!workingMemory)
114  {
115  ARMARX_ERROR << "Failed to obtain working memory proxy";
116  }
117 
118  environmentalPrimitiveSegment = workingMemory->getEnvironmentalPrimitiveSegment();
119  if (!environmentalPrimitiveSegment)
120  {
121  ARMARX_ERROR << "Failed to obtain environmental primitive segment pointer";
122  }
123 
124  pointCloudSegmentationPrx = getTopic<visionx::PointCloudSegmentationListenerPrx>(getProperty<std::string>("SegmentedPointCloudTopicName").getValue());
125 
126  debugDrawerTopicPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopicName").getValue());
127 
128  if (getProperty<std::string>("RobotStateComponentName").getValue() != "")
129  {
130  robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
131  if (!robotStateComponent->getSynchronizedRobot()->hasRobotNode(sourceFrameName))
132  {
133  ARMARX_ERROR << "source node does not exist!";
134  robotStateComponent = nullptr;
135  }
136  }
137 
138  lastUsedLabel = 0;
139 
140  debugDrawerTopicPrx->clearLayer("primitiveMapper");
141 
142  debugObserver = getTopic<DebugObserverInterfacePrx>(getProperty<std::string>("DebugObserverName").getValue());
143  numViews = 0;
144 
145 
146  StringVariantBaseMap debugValues;
147  debugValues["processingTime"] = new Variant(0.0);
148  debugValues["oldPrimitives"] = new Variant(0.0);
149  debugValues["visiblePrimitives"] = new Variant(0.0);
150  debugValues["newPrimitives"] = new Variant(0.0);
151  debugValues["modifiedPrimitives"] = new Variant(0.0);
152  debugValues["unmodifiedPrimitives"] = new Variant(0.0);
153  debugValues["removedPrimitives"] = new Variant(0.0);
154  debugValues["numViews"] = new Variant(0.0);
155  debugObserver->setDebugChannel(getName(), debugValues);
156 }
157 
158 void PrimitiveExtractor::onDisconnectPointCloudProcessor()
159 {
160 
161 }
162 
163 void PrimitiveExtractor::onExitPointCloudProcessor()
164 {
165 }
166 
167 void PrimitiveExtractor::enablePipelineStep(const Ice::Current& c)
168 {
169  std::unique_lock lock(enableMutex);
170  mappingEnabled = true;
171 }
172 
173 void PrimitiveExtractor::disablePipelineStep(const Ice::Current& c)
174 {
175  std::unique_lock lock(enableMutex);
176  mappingEnabled = false;
177 }
178 
179 void PrimitiveExtractor::process()
180 {
181  if (!waitForPointClouds(providerName, 10000))
182  {
183  return;
184  }
185 
186  {
187  std::unique_lock lock(enableMutex);
188 
189  if (!mappingEnabled)
190  {
191  return;
192  }
193  }
194 
195  // start timer
196  IceUtil::Time startTime = IceUtil::Time::now();
197 
198  pcl::PointCloud<pcl::PointXYZRGBL>::Ptr temp(new pcl::PointCloud<pcl::PointXYZRGBL>());
199  if (!getPointClouds(providerName, temp))
200  {
201  ARMARX_WARNING << "Unable to get point cloud data.";
202  return;
203  }
204 
205  pcl::copyPointCloud(*temp, *labeledCloudPtr);
206  if (labeledCloudPtr->size() == 0)
207  {
208  ARMARX_WARNING << "input point cloud is empty";
209 
210  provideResultPointClouds(labeledCloudPtr, "GraspPointCloudResult");
211  provideResultPointClouds(labeledCloudPtr, "PrimitiveExtractorResult");
212 
213  return;
214  }
215 
216 
217  inliersCloudPtr = primitiveExtractor->computeScenePrimitives(labeledCloudPtr);
218  graspCloudPtr = primitiveExtractor->getDetectedGraspPoints();
219 
220 
221  const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr >& planeInliers = primitiveExtractor->getPlaneInliers();
222  const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr >& cylinderInliers = primitiveExtractor->getCylinderInliers();
223  const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr >& sphereInliers = primitiveExtractor->getSphereInliers();
224  const std::vector<float>& circularityProbabilities = primitiveExtractor->getCircularityProbabilities();
225 
226  ARMARX_INFO << " ********* Detected Plane number: " << planeInliers.size();
227  ARMARX_INFO << " ********* Detected Cylinder number: " << cylinderInliers.size();
228  ARMARX_INFO << " ********* Detected Sphere number: " << sphereInliers.size();
229  ARMARX_INFO << " ********* Detected Circle Probabilities: " << circularityProbabilities.size();
230 
231  // publish the extracted inliers
232  provideResultPointClouds(graspCloudPtr, "GraspPointCloudResult");
233  provideResultPointClouds(inliersCloudPtr, "PrimitiveExtractorResult");
234 
235  ARMARX_INFO << "Primitive Extraction took " << (IceUtil::Time::now() - startTime).toSecondsDouble() << " secs";
236 
237  // Push each extracted primitive to the memory
238  startTime = IceUtil::Time::now();
239 
240  bool enabled;
241  {
242  std::unique_lock lock(enableMutex);
243  enabled = mappingEnabled;
244  }
245 
246  if (enabled)
247  {
248  if (getProperty<bool>("UseAffordanceExtractionLibExport").getValue())
249  {
250  pushPrimitivesToMemoryAffordanceKit(temp->header.stamp);
251  }
252  else
253  {
254  pushPrimitivesToMemory(temp->header.stamp);
255  }
256 
257  {
258  std::unique_lock lock(timestampMutex);
259  lastProcessedTimestamp = temp->header.stamp;
260  }
261  }
262 
263  // Signal the availability of a new segmentation
264  pointCloudSegmentationPrx->reportNewPointCloudSegmentation();
265 
266 
267  ARMARX_INFO << "Mapping took " << (IceUtil::Time::now() - startTime).toSecondsDouble() << " secs";
268 }
269 
270 float PrimitiveExtractor::getGraspPointInlierRatio(memoryx::EnvironmentalPrimitiveBasePtr leftPrimitive, memoryx::EnvironmentalPrimitiveBasePtr rightPrimitive)
271 {
272  FramedPosePtr pose = FramedPosePtr::dynamicCast(rightPrimitive->getPose());
273  Eigen::Matrix4f worldToPrimitive = pose->toEigen().inverse();
274 
275  Eigen::Vector3f oBBDimensions = Vector3Ptr::dynamicCast(rightPrimitive->getOBBDimensions())->toEigen();
276  Eigen::Vector3f extent = 0.5 * oBBDimensions + (Eigen::Vector3f::Ones() * 50);
277 
278  float numPoints = 0.0;
279  memoryx::PointList graspPoints = leftPrimitive->getGraspPoints();
280 
281  for (Vector3BasePtr& p : graspPoints)
282  {
283  Eigen::Vector4f temp = Eigen::Vector4f(p->x, p->y, p->z, 1.0);
284  Eigen::Vector3f point = (worldToPrimitive * temp).head<3>();
285 
286  if ((point.array().abs() < extent.array()).all())
287  {
288  numPoints++;
289  }
290  }
291 
292  return numPoints / graspPoints.size();
293 }
294 
295 
296 
297 void PrimitiveExtractor::fusePrimitives(std::vector<memoryx::EntityBasePtr>& newPrimitives, long timestamp)
298 {
299  // memoryx::SegmentLockBasePtr segmentLock = environmentalPrimitiveSegment->lockSegment();
300  std::vector<EnvironmentalPrimitiveBasePtr> primitives = environmentalPrimitiveSegment->getEnvironmentalPrimitives();
301  // environmentalPrimitiveSegment->unlockSegment(segmentLock);
302 
303  std::vector<memoryx::EnvironmentalPrimitiveBasePtr> nonVisiblePrimitives;
304  std::vector<memoryx::EnvironmentalPrimitiveBasePtr> visiblePrimitives;
305 
306  std::vector<memoryx::EnvironmentalPrimitiveBasePtr> modifiedPrimitives;
307  std::set<std::string> processedPrimitives;
308  std::set<std::string> primitivesToRemove;
309 
310  if (robotStateComponent)
311  {
312  Eigen::Matrix4f cameraToWorld = Eigen::Matrix4f::Identity();
313  if (sourceFrameName != armarx::GlobalFrame)
314  {
315  auto snapshot = robotStateComponent->getRobotSnapshotAtTimestamp(timestamp);
316  snapshot->ref();
317  FramedPoseBasePtr pose = snapshot->getRobotNode(sourceFrameName)->getGlobalPose();
318  snapshot->unref();
319  cameraToWorld = armarx::FramedPosePtr::dynamicCast(pose)->toEigen();
320  }
321 
322  primitiveFilter.updateCameraPosition(cameraToWorld);
323  // if (verbose)
324  // {
325  primitiveFilter.visualizeFrustum(debugDrawerTopicPrx);
326  // }
327  primitiveFilter.getPrimitivesInFieldOfView(primitives, visiblePrimitives, nonVisiblePrimitives);
328  }
329  else
330  {
331 
332  ARMARX_ERROR << "Frustum culling not possible without RobotStateComponent assuming that all primitives are visible";
333  visiblePrimitives = primitives;
334  }
335 
336 
337  if (getProperty<bool>("EnableBoxPrimitives").getValue())
338  {
339 
340  // fusionStrategy.findBoxPrimitives(primitives, newPrimitives, environmentalPrimitiveSegment);
341  for (const EnvironmentalPrimitiveBasePtr& p : primitives)
342  {
343  if (p->ice_isA(memoryx::BoxPrimitiveBase::ice_staticId()))
344  {
345  primitivesToRemove.insert(p->getId());
346  }
347  }
348  }
349 
350  if (nonVisiblePrimitives.size())
351  {
352  ARMARX_INFO << "found " << nonVisiblePrimitives.size() << " non visible primitives in field of view.";
353  for (memoryx::EnvironmentalPrimitiveBasePtr& p : nonVisiblePrimitives)
354  {
355  p->setProbability(std::max(0.0, p->getProbability() - 0.1));
356  if (p->getProbability() <= 0.4)
357  {
358  primitivesToRemove.insert(p->getId());
359  }
360  else
361  {
362  modifiedPrimitives.push_back(p);
363  }
364  }
365  }
366 
367 
368  ARMARX_DEBUG << "total primitives " << primitives.size() << " visible primitives " << visiblePrimitives.size() << "new primitives" << newPrimitives.size();
369 
370  std::vector<memoryx::EntityBasePtr> uniqueEntities;
371  for (const memoryx::EntityBasePtr& newEntity : newPrimitives)
372  {
373  memoryx::EnvironmentalPrimitiveBasePtr newPrimitive = memoryx::EnvironmentalPrimitiveBasePtr::dynamicCast(newEntity);
374 
375  std::vector<EnvironmentalPrimitiveBasePtr> intersectingPrimitives;
376  fusionStrategy.getIntersectingSimilarPrimitives(primitives, newPrimitive, intersectingPrimitives, 50.0);
377  // fusionStrategy.getIntersectingSimilarPrimitives(visiblePrimitives, newPrimitive, intersectingPrimitives, 50.0);
378 
379 
380  bool addPrimitive = true;
381  for (EnvironmentalPrimitiveBasePtr& intersectingPrimitive : intersectingPrimitives)
382  {
383  if (primitivesToRemove.count(intersectingPrimitive->getId()))
384  {
385  continue;
386  }
387 
388  int currentLabel = 0;
389  float inlierRatioLeft = getGraspPointInlierRatio(newPrimitive, intersectingPrimitive);
390  float inlierRatioRight = getGraspPointInlierRatio(intersectingPrimitive, newPrimitive);
391 
392  if (inlierRatioLeft <= 0.1 && inlierRatioRight <= 0.1)
393  {
394  processedPrimitives.insert(intersectingPrimitive->getId());
395  }
396 
397  if (inlierRatioLeft >= 0.9 && inlierRatioRight >= 0.9)
398  {
399  intersectingPrimitive->setTime(newPrimitive->getTime());
400  float newProbability = std::fmax(newPrimitive->getProbability(), intersectingPrimitive->getProbability() + 0.15);
401  intersectingPrimitive->setProbability(std::fmin(1.0, newProbability));
402  modifiedPrimitives.push_back(intersectingPrimitive);
403 
404  addPrimitive = false;
405  break;
406  }
407  else if (inlierRatioLeft <= 0.3 && inlierRatioRight >= 0.7)
408  {
409 
410  primitivesToRemove.insert(intersectingPrimitive->getId());
411  }
412  else if (inlierRatioLeft >= 0.7 && inlierRatioRight <= 0.3)
413  {
414 
415  addPrimitive = false;
416  break;
417  }
418  else if ((inlierRatioLeft + inlierRatioRight) >= 1.4)
419  {
420  if ((newPrimitive->getOBBDimensions()->x * newPrimitive->getOBBDimensions()->y) > (intersectingPrimitive->getOBBDimensions()->x * intersectingPrimitive->getOBBDimensions()->y))
421  {
422  primitivesToRemove.insert(intersectingPrimitive->getId());
423  }
424  else
425  {
426  addPrimitive = false;
427  break;
428  }
429  }
430 
431  if (currentLabel)
432  {
433  }
434  else if (intersectingPrimitive->getLabel())
435  {
436  currentLabel = intersectingPrimitive->getLabel();
437  }
438 
439  if (currentLabel == 0 && intersectingPrimitives.size())
440  {
441  currentLabel = ++lastUsedLabel;
442  }
443  newPrimitive->setLabel(currentLabel);
444 
445  for (EnvironmentalPrimitiveBasePtr& x : intersectingPrimitives)
446  {
447  x->setLabel(currentLabel);
448  modifiedPrimitives.push_back(x);
449  }
450  }
451  if (addPrimitive)
452  {
453  uniqueEntities.push_back(newPrimitive);
454  }
455  }
456 
457  if (robotStateComponent)
458  {
459 
460  for (memoryx::EnvironmentalPrimitiveBasePtr& p : visiblePrimitives)
461  {
462 
463  if (processedPrimitives.count(p->getId()) || primitivesToRemove.count(p->getId()))
464  {
465  continue;
466  }
467 
468  float probability = p->getProbability();
469  p->setProbability(std::max(0.0, p->getProbability() - 0.5));
470  ARMARX_LOG << "reducing probability for primitive in field of view: " << p->getId() << " from " << probability << " to " << p->getProbability();
471 
472  if (p->getProbability() <= 0.4)
473  {
474  ARMARX_LOG << "scheduling primitive " << p->getId() << " for removal";
475  primitivesToRemove.insert(p->getId());
476  }
477  else
478  {
479  modifiedPrimitives.push_back(p);
480  }
481  }
482  }
483 
484 
485 
486 
487  ARMARX_INFO << "removing " << primitivesToRemove.size() << " primitives";
488 
489  for (const std::string& x : primitivesToRemove)
490  {
491  environmentalPrimitiveSegment->removeEntity(x);
492  }
493 
494  int modified = 0;
495  processedPrimitives.clear();
496  for (EnvironmentalPrimitiveBasePtr& x : modifiedPrimitives)
497  {
498  if (!primitivesToRemove.count(x->getId()) && !processedPrimitives.count(x->getId()))
499  {
500  modified++;
501  environmentalPrimitiveSegment->updateEntity(x->getId(), x);
502  processedPrimitives.insert(x->getId());
503  }
504  }
505 
506  float unmodified = primitives.size() - modified - primitivesToRemove.size();
507 
508  ARMARX_INFO << "Writing segmentation (" << uniqueEntities.size() << " primitives) to memory";
509 
510  environmentalPrimitiveSegment->addEntityList(uniqueEntities);
511 
512  StringVariantBaseMap debugValues;
513  debugValues["processingTime"] = new Variant((TimeUtil::GetTime().toMicroSeconds() - timestamp) / (1000.0 * 1000.0));
514  debugValues["oldPrimitives"] = new Variant((float) primitives.size());
515  debugValues["visiblePrimitives"] = new Variant((float) visiblePrimitives.size());
516  debugValues["newPrimitives"] = new Variant((float) uniqueEntities.size());
517  debugValues["modifiedPrimitives"] = new Variant(modified);
518  debugValues["unmodifiedPrimitives"] = new Variant(unmodified);
519  debugValues["removedPrimitives"] = new Variant((float) primitivesToRemove.size());
520  numViews++;
521  debugValues["numViews"] = new Variant((int) numViews);
522  debugObserver->setDebugChannel(getName(), debugValues);
523 
524  pcl::PointCloud<pcl::PointXYZL>::Ptr inliers(new pcl::PointCloud<pcl::PointXYZL>());
525  for (const EnvironmentalPrimitiveBasePtr& x : primitives)
526  {
527  if (primitivesToRemove.count(x->getId()))
528  {
529  continue;
530  }
531 
532  for (const armarx::Vector3BasePtr& v : x->getInliers())
533  {
534  pcl::PointXYZL p;
535  p.label = x->getLabel();
536  p.x = v->x;
537  p.y = v->y;
538  p.z = v->z;
539  inliers->points.push_back(p);
540  }
541  }
542 
543  for (const memoryx::EntityBasePtr& e : uniqueEntities)
544  {
545  EnvironmentalPrimitivePtr x = EnvironmentalPrimitivePtr::dynamicCast(e);
546 
547  for (const armarx::Vector3BasePtr& v : x->getInliers())
548  {
549  pcl::PointXYZL p;
550  p.label = x->getLabel();
551  p.x = v->x;
552  p.y = v->y;
553  p.z = v->z;
554  inliers->points.push_back(p);
555  }
556  }
557 
558 
559  inliers->height = 1;
560  inliers->width = inliers->points.size();
561 
562 
563  provideResultPointClouds(inliers, "Inliers");
564 
565 
566  //primitiveExtractorPrx->reportNewPrimitiveSet(newPrimitives);
567 }
568 
569 void PrimitiveExtractor::pushPrimitivesToMemory(IceUtil::Int64 originalTimestamp)
570 {
571  std::vector<memoryx::EntityBasePtr> newPrimitives;
572 
573  const std::vector<pcl::ModelCoefficients::Ptr>& planeCoefficients = primitiveExtractor->getPlaneCoefficients();
574  const std::vector<pcl::ModelCoefficients::Ptr>& cylinderCoefficients = primitiveExtractor->getCylinderCoefficients();
575  const std::vector<pcl::ModelCoefficients::Ptr>& sphereCoefficients = primitiveExtractor->getSphereCoefficients();
576  const std::vector<float>& circularityProbabilities = primitiveExtractor->getCircularityProbabilities();
577 
578  const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr>& planeInliers = primitiveExtractor->getPlaneInliers();
579  const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr>& cylinderInliers = primitiveExtractor->getCylinderInliers();
580  const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr>& sphereInliers = primitiveExtractor->getSphereInliers();
581 
582  const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr>& cylinderGraspPoints = primitiveExtractor->getCylinderGraspPoints();
583  const std::vector<pcl::PointCloud<pcl::PointXYZL>::Ptr>& sphereGraspPoints = primitiveExtractor->getSphereGraspPoints();
584 
585  int label = 0;
586  // for each detected plane primitive
587  //#pragma omp parallel for
588  for (size_t i = 0 ; i < planeCoefficients.size(); i++)
589  {
590  // coefficients
591  pcl::ModelCoefficients::Ptr coefficients = planeCoefficients[i];
592  pcl::PointCloud<pcl::PointXYZL>::Ptr inliers = planeInliers[i];
593  // pcl::PointCloud<pcl::PointXYZL>::Ptr graspPoints = planeGraspPoints[i];
594  pcl::PointCloud<pcl::PointXYZL>::Ptr graspPoints(new pcl::PointCloud<pcl::PointXYZL>());
595 
596  pcl::PointCloud<pcl::PointXYZL>::Ptr temp(new pcl::PointCloud<pcl::PointXYZL>());
597  pcl::ProjectInliers<PointL> proj;
598  proj.setModelType(pcl::SACMODEL_PLANE);
599  proj.setInputCloud(inliers);
600  proj.setModelCoefficients(coefficients);
601  proj.filter(*temp);
602 
603  ARMARX_LOG << __LINE__ << "computing hull";
604 
605  pcl::ConcaveHull<PointL> concaveHull;
606  concaveHull.setInputCloud(temp);
607  concaveHull.setDimension(2);
608  concaveHull.setAlpha(69.0);
609  // concaveHull.setKeepInformation(true);
610  concaveHull.reconstruct(*graspPoints);
611 
612 
613  ARMARX_LOG << __LINE__ << " done computing hull";
614 
615  // bounding boxes
616  pcl::MomentOfInertiaEstimation <pcl::PointXYZL> bb;
617  pcl::PointXYZL min_point_OBB, max_point_OBB, position_OBB;
618  Eigen::Matrix3f rotational_matrix_OBB;
619 
620  bb.setInputCloud(graspPoints);
621  bb.compute();
622  bb.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
623 
624  Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
625  Vector3Ptr dimension = new Vector3(max_point_OBB.x * 2, max_point_OBB.y * 2, max_point_OBB.z * 2);
626  armarx::FramedPosePtr pose = new FramedPose(rotational_matrix_OBB, position, armarx::GlobalFrame, "");
627 
628  // computing the length
629  pcl::PointXYZL pointMin, pointMax;
630  pcl::getMinMax3D<pcl::PointXYZL>(*inliers, pointMin, pointMax);
631  float length = (pointMax.getVector3fMap() - pointMin.getVector3fMap()).norm();
632 
633  // setting primitives
634  EnvironmentalPrimitiveBasePtr primitive;
635  primitive = new PlanePrimitive(coefficients->values);
636  primitive->setOBBDimensions(dimension);
637  primitive->setPose(pose);
638  primitive->setProbability(0.75); // dummy value
639  primitive->setLabel(0);
640  primitive->setTime(new TimestampVariant(originalTimestamp));
641  primitive->setLength(length);
642  primitive->setCircularityProbability(circularityProbabilities[i]);
643  primitive->setSampling(new armarx::MatrixFloat(4, 0));
644  primitive->setName("environmentalPrimitive_" + boost::lexical_cast<std::string>(label++));
645 
646  // convert pcl grasp points into PointList
647  PointList graspPointList(graspPoints->points.size());
648  for (size_t p = 0; p < graspPoints->points.size(); p++)
649  {
650  Vector3Ptr currPoint = new Vector3(graspPoints->points[p].x, graspPoints->points[p].y, graspPoints->points[p].z);
651  graspPointList[p] = currPoint;
652  }
653  primitive->setGraspPoints(graspPointList);
654 
655  PointList inlierList(inliers->points.size());
656  for (size_t p = 0; p < inliers->points.size(); p++)
657  {
658  const Eigen::Vector3f& f = inliers->points[p].getVector3fMap();
659  inlierList[p] = new Vector3(f);
660  }
661  primitive->setInliers(inlierList);
662 
663 
664  //#pragma omp critical
665 
666  // Generate sampling
667  std::vector<Eigen::Vector3f> convexHull(graspPointList.size());
668  for (unsigned int i = 0; i < graspPointList.size(); i++)
669  {
670  convexHull[i] = armarx::Vector3Ptr::dynamicCast(graspPointList[i])->toEigen();
671  }
672  AffordanceKit::PlanePtr plane(new AffordanceKit::Plane(convexHull));
673  plane->sample(spatialSamplingDistance, numOrientationalSamplings);
674  armarx::MatrixFloatBasePtr s(new armarx::MatrixFloat(plane->getSampling()));
675  primitive->setSampling(s);
676 
677  //#pragma omp critical
678  {
679  newPrimitives.push_back(primitive);
680  }
681  }
682 
683  // for each detected cylinder primitive
684  //#pragma omp parallel for
685  for (size_t i = 0 ; i < cylinderCoefficients.size(); i++)
686  {
687  // coefficients
688  pcl::ModelCoefficients::Ptr coefficients = cylinderCoefficients[i];
689  pcl::PointCloud<pcl::PointXYZL>::Ptr inliers = cylinderInliers[i];
690  pcl::PointCloud<pcl::PointXYZL>::Ptr graspPoints = cylinderGraspPoints[i];
691 
692  pcl::PointCloud<pcl::PointXYZL>::Ptr temp(new pcl::PointCloud<pcl::PointXYZL>());
693  pcl::ProjectInliers<PointL> proj;
694  proj.setModelType(pcl::SACMODEL_PLANE);
695  proj.setInputCloud(inliers);
696  proj.setModelCoefficients(coefficients);
697  proj.filter(*temp);
698 
699  pcl::ConcaveHull<PointL> concaveHull;
700  concaveHull.setInputCloud(temp);
701  concaveHull.setDimension(2);
702  concaveHull.setAlpha(69.0);
703  concaveHull.setKeepInformation(true);
704  concaveHull.reconstruct(*graspPoints);
705 
706  // bounding boxes
707  pcl::MomentOfInertiaEstimation <pcl::PointXYZL> bb;
708  pcl::PointXYZL min_point_OBB, max_point_OBB, position_OBB;
709  Eigen::Matrix3f rotational_matrix_OBB;
710 
711  bb.setInputCloud(inliers);
712  bb.compute();
713  bb.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
714 
715  Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
716  Vector3Ptr dimension = new Vector3(max_point_OBB.x * 2, max_point_OBB.y * 2, max_point_OBB.z * 2);
717  armarx::FramedPosePtr pose = new FramedPose(rotational_matrix_OBB, position, armarx::GlobalFrame, "");
718 
719  // Projected inliers to determine cylinder base point and length
720  // (These are not properly encoded in the RANSAC coefficients)
721  float min_t = std::numeric_limits<float>::infinity();
722  float max_t = -std::numeric_limits<float>::infinity();
723 
724  Eigen::Vector3f b(coefficients->values[0], coefficients->values[1], coefficients->values[2]);
725  Eigen::Vector3f d(coefficients->values[3], coefficients->values[4], coefficients->values[5]);
726  d.normalize();
727 
728  for (unsigned int i = 0; i < inliers->points.size(); i++)
729  {
730  Eigen::Vector3f v(inliers->points[i].x, inliers->points[i].y, inliers->points[i].z);
731 
732  float t = (v - b).dot(d) / d.dot(d);
733 
734  min_t = std::min(min_t, t);
735  max_t = std::max(max_t, t);
736  }
737 
738  float length = max_t - min_t;
739  Eigen::Vector3f base = b + min_t * d + 0.5 * length * d;
740  Eigen::Vector3f direction = d;
741  float radius = coefficients->values[6];
742 
743  // Create MemoryX cylinder primitive instance
744  CylinderPrimitiveBasePtr primitive;
745  primitive = new CylinderPrimitive;
746  primitive->setCylinderPoint(new Vector3(base));
747  primitive->setCylinderAxisDirection(new Vector3(direction));
748  primitive->setCylinderRadius(radius);
749  primitive->setLength(length);
750  primitive->setOBBDimensions(dimension);
751  primitive->setPose(pose);
752  primitive->setProbability(0.75); // dummy value
753  primitive->setLabel(0);
754  primitive->setTime(new TimestampVariant(originalTimestamp));
755  primitive->setCircularityProbability(0.0); // it is always zero for non-planar surfaces
756  primitive->setSampling(new armarx::MatrixFloat(4, 0));
757 
758  // convert pcl grasp points into PointList
759  PointList graspPointList(graspPoints->points.size());
760 
761  for (size_t p = 0; p < graspPoints->points.size(); p++)
762  {
763  Vector3Ptr currPoint = new Vector3(graspPoints->points[p].x, graspPoints->points[p].y, graspPoints->points[p].z);
764  graspPointList[p] = currPoint;
765  }
766 
767  primitive->setName("environmentalPrimitive_" + boost::lexical_cast<std::string>(planeCoefficients.size() + i));
768  primitive->setGraspPoints(graspPointList);
769 
770  // Generate sampling
771  AffordanceKit::CylinderPtr cylinder(new AffordanceKit::Cylinder(base, direction, length, radius));
772  cylinder->sample(spatialSamplingDistance, numOrientationalSamplings);
773  armarx::MatrixFloatBasePtr s(new armarx::MatrixFloat(cylinder->getSampling()));
774  primitive->setSampling(s);
775 
776  //#pragma omp critical
777  {
778  newPrimitives.push_back(primitive);
779  }
780  }
781 
782  // for each detected sphere primitive
783  //#pragma omp parallel for
784  for (size_t i = 0 ; i < sphereCoefficients.size(); i++)
785  {
786  // coefficients
787  pcl::ModelCoefficients::Ptr coefficients = sphereCoefficients[i];
788  pcl::PointCloud<pcl::PointXYZL>::Ptr inliers = sphereInliers[i];
789  pcl::PointCloud<pcl::PointXYZL>::Ptr graspPoints = sphereGraspPoints[i];
790 
791  // apply transformation to each inlier and grasp point
792  //pcl::transformPointCloud(*inliers, *inliers, transform);
793  //pcl::transformPointCloud(*graspPoints, *graspPoints, transform);
794 
795  // bounding boxes
796  pcl::MomentOfInertiaEstimation <pcl::PointXYZL> bb;
797  pcl::PointXYZL min_point_OBB, max_point_OBB, position_OBB;
798  Eigen::Matrix3f rotational_matrix_OBB;
799 
800  bb.setInputCloud(inliers);
801  bb.compute();
802  bb.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
803 
804  Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
805  Vector3Ptr dimension = new Vector3(max_point_OBB.x * 2, max_point_OBB.y * 2, max_point_OBB.z * 2);
806  armarx::FramedPosePtr pose = new FramedPose(rotational_matrix_OBB, position, armarx::GlobalFrame, "");
807 
808 
809  //computing the length
810  pcl::PointXYZL pointMin, pointMax;
811  pcl::getMinMax3D<pcl::PointXYZL>(*inliers, pointMin, pointMax);
812  float length = (pointMax.getVector3fMap() - pointMin.getVector3fMap()).norm();
813 
814  // setting primitives
815  EnvironmentalPrimitiveBasePtr primitive = {};
816  primitive = new SpherePrimitive(coefficients->values);
817  primitive->setOBBDimensions(dimension);
818  primitive->setPose(pose);
819  primitive->setLabel(0);
820  primitive->setProbability(0.75); // dummy value
821  primitive->setTime(new TimestampVariant(originalTimestamp));
822  primitive->setLength(length);
823  primitive->setCircularityProbability(0.0); // it is always zero for non-planar surfaces
824  primitive->setSampling(new armarx::MatrixFloat(4, 0));
825 
826  // convert pcl grasp points into PointList
827  PointList graspPointList(graspPoints->points.size());
828 
829  for (size_t p = 0; p < graspPoints->points.size(); p++)
830  {
831  Vector3Ptr currPoint = new Vector3(graspPoints->points[p].x, graspPoints->points[p].y, graspPoints->points[p].z);
832  graspPointList[p] = currPoint;
833  }
834 
835  primitive->setName("environmentalPrimitive_" + boost::lexical_cast<std::string>(cylinderCoefficients.size() + planeCoefficients.size() + i));
836  primitive->setGraspPoints(graspPointList);
837 
838  // Generate sampling
839  Eigen::Vector3f center(coefficients->values[0], coefficients->values[1], coefficients->values[2]);
840  float radius = coefficients->values[3];
841  AffordanceKit::SpherePtr sphere(new AffordanceKit::Sphere(center, radius));
842  sphere->sample(spatialSamplingDistance, numOrientationalSamplings);
843  armarx::MatrixFloatBasePtr s(new armarx::MatrixFloat(sphere->getSampling()));
844  primitive->setSampling(s);
845 
846  //#pragma omp critical
847  {
848  newPrimitives.push_back(primitive);
849  }
850  }
851 
852  ARMARX_LOG << __LINE__ << " writing primitives to memory.";
853 
854  if (enablePrimitiveFusion)
855  {
856  fusePrimitives(newPrimitives, originalTimestamp);
857  }
858  else
859  {
860  ARMARX_LOG << __LINE__ << " removing previous primitives from memory.";
861  environmentalPrimitiveSegment->removeAllEntities();
862  ARMARX_LOG << __LINE__ << " adding " << VAROUT(newPrimitives.size());
863  environmentalPrimitiveSegment->addEntityList(newPrimitives);
864  }
865 
866 
867  ARMARX_LOG << __LINE__ << "done writing primitives to memory.";
868 
869  if (getProperty<bool>("EnableBoxPrimitives").getValue())
870  {
871  newPrimitives.clear();
872 
873  std::vector<EnvironmentalPrimitiveBasePtr> primitives = environmentalPrimitiveSegment->getEnvironmentalPrimitives();
874 
875 
876  unsigned int sz = newPrimitives.size();
877  fusionStrategy.findBoxPrimitives(primitives, newPrimitives, environmentalPrimitiveSegment);
878 
879  // Generate sampling
880  for (auto& primitive : newPrimitives)
881  {
882  memoryx::BoxPrimitivePtr box = memoryx::BoxPrimitivePtr::dynamicCast(primitive);
883  if (!box)
884  {
885  continue;
886  }
887 
888  Eigen::Matrix4f pose = PosePtr::dynamicCast(box->getPose())->toEigen();
889  Eigen::Vector3f dimensions = Vector3Ptr::dynamicCast(box->getOBBDimensions())->toEigen();
890  AffordanceKit::BoxPtr b(new AffordanceKit::Box(pose, dimensions));
891  b->sample(getProperty<float>("SpatialSamplingDistance").getValue(), getProperty<int>("NumOrientationalSamplings").getValue());
892  armarx::MatrixFloatBasePtr s(new armarx::MatrixFloat(b->getSampling()));
893  box->setSampling(s);
894  }
895 
896  ARMARX_INFO << "Found " << (newPrimitives.size() - sz) << " Box primitives";
897 
898  environmentalPrimitiveSegment->addEntityList(newPrimitives);
899  }
900 }
901 
902 void PrimitiveExtractor::pushPrimitivesToMemoryAffordanceKit(IceUtil::Int64 originalTimestamp)
903 {
904  AffordanceKit::PrimitiveSetPtr ps = primitiveExtractor->getPrimitiveSet(getProperty<float>("SpatialSamplingDistance").getValue(), getProperty<int>("NumOrientationalSamplings").getValue(), originalTimestamp);
906  primitiveSet.writeToMemory(environmentalPrimitiveSegment);
907 }
908 
909 PropertyDefinitionsPtr PrimitiveExtractor::createPropertyDefinitions()
910 {
911  return PropertyDefinitionsPtr(new PrimitiveExtractorPropertyDefinitions(getConfigIdentifier()));
912 }
913 
915 {
916  std::unique_lock lock(enableMutex);
917  return mappingEnabled;
918 }
919 
920 armarx::TimestampBasePtr armarx::PrimitiveExtractor::getLastProcessedTimestamp(const Ice::Current& c)
921 {
922  std::unique_lock lock(timestampMutex);
923  return new TimestampVariant(lastProcessedTimestamp);
924 }
925 
926 void armarx::PrimitiveExtractor::setParameters(const visionx::PrimitiveExtractorParameters& parameters, const Ice::Current& c)
927 {
928  ARMARX_INFO << "Updating parameter setup";
929  primitiveExtractor->updateParameters(verbose, parameters.minSegmentSize, parameters.maxSegmentSize,
930  parameters.planeMaxIterations, parameters.planeDistanceThreshold, parameters.planeNormalDistance,
931  parameters.cylinderMaxIterations, parameters.cylinderDistanceThreshold, parameters.cylinderRadiusLimit,
932  parameters.sphereMaxIterations, parameters.sphereDistanceThreshold, parameters.sphereNormalDistance,
933  parameters.euclideanClusteringTolerance, parameters.outlierThreshold, parameters.circularDistanceThreshold);
934 }
935 
936 void PrimitiveExtractor::loadParametersFromFile(const std::string& filename, const Ice::Current& c)
937 {
938  std::string absolute_filename;
939  ArmarXDataPath::getAbsolutePath(filename, absolute_filename);
940 
941  ARMARX_INFO << "Loading parameter setup from " << absolute_filename;
942  QSettings config(QString::fromStdString(absolute_filename), QSettings::IniFormat);
943 
944  config.beginGroup("PrimitiveExtraction");
945 
946  primitiveExtractor->updateParameters(verbose,
947  config.value("MinPrimitiveSize", getProperty<double>("minimumSegmentSize").getValue()).toDouble(),
948  config.value("MaxPrimitiveSize", getProperty<double>("maximumSegmentSize").getValue()).toDouble(),
949  config.value("PlaneMaxIterations", getProperty<double>("planeMaximumIteration").getValue()).toDouble(),
950  config.value("PlaneDistanceThreshold", getProperty<float>("planeDistanceThreshold").getValue()).toFloat(),
951  config.value("PlaneNormalDistance", getProperty<float>("planeNormalDistance").getValue()).toFloat(),
952  config.value("CylinderMaxIterations", getProperty<double>("cylinderMaximumIteration").getValue()).toDouble(),
953  config.value("CylinderDistanceThreshold", getProperty<float>("cylinderDistanceThreshold").getValue()).toFloat(),
954  config.value("CylinderRadiusLimit", getProperty<float>("cylinderRadiousLimit").getValue()).toFloat(),
955  config.value("SphereMaxIterations", getProperty<double>("sphereMaximumIteration").getValue()).toDouble(),
956  config.value("SphereDistanceThreshold", getProperty<float>("sphereDistanceThreshold").getValue()).toFloat(),
957  config.value("SphereNormalDistance", getProperty<float>("sphereNormalDistance").getValue()).toFloat(),
958  config.value("EuclideanClusteringTolerance", getProperty<float>("euclideanClusteringTolerance").getValue()).toFloat(),
959  config.value("OutlierDistanceThreshold", getProperty<float>("outlierThreshold").getValue()).toFloat(),
960  config.value("PlaneCircularDistanceThreshold", getProperty<float>("circularDistanceThreshold").getValue()).toFloat()
961  );
962 
963  config.endGroup();
964 }
965 
966 visionx::PrimitiveExtractorParameters armarx::PrimitiveExtractor::getParameters(const Ice::Current&)
967 {
968  visionx::PrimitiveExtractorParameters parameters;
969  parameters.minSegmentSize = primitiveExtractor->minSegmentSize;
970  parameters.maxSegmentSize = primitiveExtractor->maxSegmentSize;
971  parameters.planeMaxIterations = primitiveExtractor->planeMaxIteration;
972  parameters.planeDistanceThreshold = primitiveExtractor->planeMaxIteration;
973  parameters.planeNormalDistance = primitiveExtractor->planeNormalDistanceWeight;
974  parameters.cylinderMaxIterations = primitiveExtractor->cylinderMaxIteration;
975  parameters.cylinderDistanceThreshold = primitiveExtractor->cylinderDistanceThreshold;
976  parameters.cylinderRadiusLimit = primitiveExtractor->cylinderRadiusLimit;
977 
978  parameters.sphereMaxIterations = primitiveExtractor->sphereMaxIteration;
979  parameters.sphereDistanceThreshold = primitiveExtractor->sphereDistanceThreshold;
980  parameters.sphereNormalDistance = primitiveExtractor->sphereNormalDistanceWeight;
981  parameters.euclideanClusteringTolerance = primitiveExtractor->euclideanClusteringTolerance;
982  parameters.outlierThreshold = primitiveExtractor->outlierThreshold;
983  parameters.circularDistanceThreshold = primitiveExtractor->circleDistanceThreshold;
984 
985  return parameters;
986 }
987 
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
pcl
Definition: pcl_point_operators.cpp:4
memoryx::VariantType::PlanePrimitive
const armarx::VariantTypeId PlanePrimitive
Definition: EnvironmentalPrimitive.h:38
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:111
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:37
MatrixVariant.h
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
armarx::TimestampVariant
Definition: TimestampVariant.h:54
memoryx::VariantType::SpherePrimitive
const armarx::VariantTypeId SpherePrimitive
Definition: EnvironmentalPrimitive.h:39
Convex::convexHull
ConvexHull< Point >::type convexHull(const std::vector< Point > &points)
Definition: convexHull.hpp:487
armarx::PrimitiveExtractor::getParameters
visionx::PrimitiveExtractorParameters getParameters(const Ice::Current &c=Ice::emptyCurrent) override
Definition: PrimitiveExtractor.cpp:966
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::PrimitiveExtractor::isPipelineStepEnabled
bool isPipelineStepEnabled(const Ice::Current &c=Ice::emptyCurrent) override
Definition: PrimitiveExtractor.cpp:914
IceInternal::Handle< FramedPose >
memoryx::VariantType::CylinderPrimitive
const armarx::VariantTypeId CylinderPrimitive
Definition: EnvironmentalPrimitive.h:40
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
TimestampVariant.h
armarx::MatrixFloat
The MatrixFloat class.
Definition: MatrixVariant.h:48
AffordanceKitArmarX::PrimitiveSetArmarX
Definition: PrimitiveSetArmarX.h:32
enabled
std::atomic< bool > * enabled
Definition: RemoteGuiWidgetController.cpp:75
filename
std::string filename
Definition: VisualizationRobot.cpp:84
max
T max(T t1, T t2)
Definition: gdiam.h:48
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
ARMARX_LOG
#define ARMARX_LOG
Definition: Logging.h:163
armarx::control::common::getValue
T getValue(nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName)
Definition: utils.h:55
PointCloud
Definition: PointCloud.h:69
armarx::ctrlutil::v
double v(double t, double v0, double a0, double j)
Definition: CtrlUtil.h:39
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::PrimitiveExtractorPropertyDefinitions
Definition: PrimitiveExtractor.h:121
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
IceUtil::Handle< class PropertyDefinitionContainer >
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:41
dot
double dot(const Point &x, const Point &y)
Definition: point.hpp:53
armarx::PrimitiveExtractor::getLastProcessedTimestamp
armarx::TimestampBasePtr getLastProcessedTimestamp(const Ice::Current &c=Ice::emptyCurrent) override
Definition: PrimitiveExtractor.cpp:920
armarx::PrimitiveExtractor::setParameters
void setParameters(const visionx::PrimitiveExtractorParameters &parameters, const Ice::Current &c=Ice::emptyCurrent) override
Definition: PrimitiveExtractor.cpp:926
PrimitiveSetArmarX.h
min
T min(T t1, T t2)
Definition: gdiam.h:42
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
PrimitiveExtractor.h
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
ArmarXDataPath.h
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
norm
double norm(const Point &a)
Definition: point.hpp:94