VoxelGridMappingProvider.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 RobotComponents::ArmarXObjects::VoxelGridMappingProvider
17  * @author Mirko Waechter ( mirko dot waechter at kit dot edu )
18  * @date 2018
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "VoxelGridMappingProvider.h"
24 #include <ArmarXCore/core/util/algorithm.h>
25 #include <ArmarXCore/core/util/IceReportSkipper.h>
26 #include <ArmarXCore/core/util/StringHelperTemplates.h>
27 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
28 #include <RobotAPI/components/DebugDrawer/DebugDrawerUtils.h>
29 
30 
31 #include <pcl/point_types.h>
32 #include <pcl/common/transforms.h>
33 #include <pcl/filters/approximate_voxel_grid.h>
34 #include <pcl/filters/voxel_grid.h>
35 #include <pcl/filters/uniform_sampling.h>
36 #include <pcl/filters/passthrough.h>
37 #include <pcl/filters/frustum_culling.h>
38 #include <pcl/io/pcd_io.h>
39 
40 //#include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
41 #include <VirtualRobot/CollisionDetection/CDManager.h>
42 #include <MotionPlanning/CSpace/CSpaceSampled.h>
43 #include <MotionPlanning/Planner/BiRrt.h>
44 #include <MotionPlanning/PostProcessing/ShortcutProcessor.h>
45 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
46 #include <VisionX/components/pointcloud_core/PointCloudConversions.h>
47 #include <VisionX/interface/components/Calibration.h>
48 
49 
50 #include <VirtualRobot/Visualization/TriMeshUtils.h>
51 
52 
53 using namespace armarx;
54 namespace visionx
55 {
56  VoxelGridMappingProvider::VoxelGridMappingProvider()
57  {
58 
59  }
60 
61  std::string VoxelGridMappingProvider::getDefaultName() const
62  {
63  return "VoxelGridMappingProvider";
64  }
65 
66  PropertyDefinitionsPtr VoxelGridMappingProvider::createPropertyDefinitions()
67  {
68  return PropertyDefinitionsPtr(new VoxelGridMappingProviderPropertyDefinitions(
69  getConfigIdentifier()));
70  }
71 
72 
73 
74 
75 
76  void VoxelGridMappingProvider::onInitPointCloudProcessor()
77  {
78  skipper.reset(new IceReportSkipper(getProperty<float>("PointCloudStorageFrequency").getValue()));
79  cycleKeeper.reset(new CycleUtil(1000.0f / getProperty<float>("UpdateRate").getValue()));
80  providerName = getProperty<std::string>("providerName").getValue();
81  sourceFrameName = getProperty<std::string>("sourceFrameName").getValue();
82  updateBoundaries();
83  gridLeafSize = getProperty<float>("GridSize");
84 
85  usingPointCloudProvider(providerName);
86  accumulatedPointCloud.reset(new pcl::PointCloud<PointType>());
87  if (!getProperty<std::string>("PointCloudLoadFilepath").getValue().empty())
88  {
89  ARMARX_INFO << "Loading pointcloud from " << getProperty<std::string>("PointCloudLoadFilepath").getValue();
90  pcl::io::loadPCDFile(getProperty<std::string>("PointCloudLoadFilepath"), *accumulatedPointCloud);
91  }
92  }
93 
94  void VoxelGridMappingProvider::onConnectPointCloudProcessor()
95  {
96  if (getProperty<std::string>("RobotStateComponentName").getValue() != "")
97  {
98  robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
99  auto localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eCollisionModel);
100  robotPool.reset(new RobotPool(localRobot, 3));
101  }
102  dd = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
103  collectingPointClouds = getProperty<bool>("ActivateOnStartUp");
104  octtree.reset(new pcl::octree::OctreePointCloud<PointType>(gridLeafSize));
105 
106  if (sourceFrameName.empty())
107  {
108 
109  visionx::ReferenceFrameInterfacePrx proxy = getProxy<visionx::ReferenceFrameInterfacePrx>(providerName);
110  if (proxy)
111  {
112  sourceFrameName = proxy->getReferenceFrame();
113  ARMARX_INFO << "source frame name is " << sourceFrameName;
114  }
115  }
116 
117  enableResultPointClouds<PointType>();
118  }
119 
120  void VoxelGridMappingProvider::onExitPointCloudProcessor()
121  {
122  }
123 
124 
125 
126 
127  void VoxelGridMappingProvider::updateBoundaries()
128  {
129  auto limits = Split(getProperty<std::string>("BoundingBox").getValue(), ";", true);
130  int i = 0;
131  ARMARX_CHECK_EQUAL(limits.size(), 3);
132  for (auto& pos : limits)
133  {
134  auto posVec = Split<float>(pos, ",", true);
135  ARMARX_CHECK_EQUAL(posVec.size(), 2);
136  croppingMin(i) = posVec.at(0);
137  croppingMax(i) = posVec.at(1);
138  i++;
139  }
140  }
141 
142  void VoxelGridMappingProvider::process()
143  {
144 
145  typename pcl::PointCloud<PointType>::Ptr inputCloudPtr(new pcl::PointCloud<PointType>());
146  typename pcl::PointCloud<PointType>::Ptr downsampledTransformedInputCloudPtr(new pcl::PointCloud<PointType>());
147  typename pcl::PointCloud<PointType>::Ptr outputCloudPtr(new pcl::PointCloud<PointType>());
148 
149  if (!collectingPointClouds)
150  {
151  ARMARX_DEBUG << deactivateSpam(10) << "Pointcloud processing is disabled";
152  usleep(10000);
153  return;
154  }
155  if (!waitForPointClouds(providerName, 10000))
156  {
157  ARMARX_INFO << "Timeout or error while waiting for point cloud data" << flush;
158  return;
159  }
160  else
161  {
162  getPointClouds(providerName, inputCloudPtr);
163  }
164  TIMING_START(FullCalc);
165 
166  ARMARX_INFO << "Got cloud of size: " << inputCloudPtr->size();
167  auto localRobot = robotPool->getRobot();
168  visionx::MetaPointCloudFormatPtr format = getPointCloudFormat(providerName);
169  ARMARX_INFO << deactivateSpam(1) << "pc timestamp: " << IceUtil::Time::microSeconds(format->timeProvided).toDateTime();
170  if (robotStateComponent)
171  {
172  if (!RemoteRobot::synchronizeLocalCloneToTimestamp(localRobot, robotStateComponent, format->timeProvided))
173  {
174  ARMARX_WARNING << deactivateSpam(5) << "failed to sync robot to timestamp! conversion to globalpose will fail! aborting";
175  return;
176  }
177  }
178 
179  Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
180  // Eigen::Matrix4f sourceFrameGlobalPose = Eigen::Matrix4f::Identity();
181  if (robotStateComponent && sourceFrameName != "Global")
182  {
183  transform = localRobot->getRobotNode(sourceFrameName)->getGlobalPose();
184  }
185 
186  Eigen::Matrix4f cam2robot;
187  cam2robot << 0, 0, 1, 0,
188  0, -1, 0, 0,
189  1, 0, 0, 0,
190  0, 0, 0, 1;
191  // .. read or fill the source cloud
192  pcl::FrustumCulling<PointType> fc;
193  fc.setInputCloud(inputCloudPtr);
194  fc.setNegative(false);
195  fc.setVerticalFOV(35);
196  fc.setHorizontalFOV(45);
197  fc.setNearPlaneDistance(0.0);
198  fc.setFarPlaneDistance(3500);
199 
200  fc.setCameraPose(cam2robot);
201 
202  fc.filter(*outputCloudPtr);
203  ARMARX_INFO << "size before frustrum culling: " << inputCloudPtr->size() << " size after: " << outputCloudPtr->size();
204  inputCloudPtr.swap(outputCloudPtr);
205 
206  ARMARX_INFO << "Robot camera transform is: " << transform;
207  // float N = 0.001f;
208  // transform (0,0) = transform (0,0) * N;
209  // transform (1,1) = transform (1,1) * N;
210  // transform (2,2) = transform (2,2) * N;
211  TIMING_START(PCl_TRANSFORM);
212  // Transform to intermediate cropping frame
213  pcl::transformPointCloud(*inputCloudPtr, *outputCloudPtr, transform);
214  outputCloudPtr.swap(inputCloudPtr);
215  TIMING_END(PCl_TRANSFORM);
216 
217  TIMING_START(PCL_FILTER);
218  // // assumes that z axis goes up in robot's root frame
219  pcl::PassThrough<PointType> pass;
220 
221 
222  pass.setInputCloud(inputCloudPtr);
223  pass.setFilterFieldName("z");
224  pass.setFilterLimits(croppingMin(2), croppingMax(2));
225  pass.filter(*outputCloudPtr);
226  outputCloudPtr.swap(inputCloudPtr);
227 
228  pass.setInputCloud(inputCloudPtr);
229  pass.setFilterFieldName("x");
230  pass.setFilterLimits(croppingMin(0), croppingMax(0));
231  pass.filter(*outputCloudPtr);
232  outputCloudPtr.swap(inputCloudPtr);
233 
234  pass.setInputCloud(inputCloudPtr);
235  pass.setFilterFieldName("y");
236  pass.setFilterLimits(croppingMin(1), croppingMax(1));
237  pass.filter(*outputCloudPtr);
238  outputCloudPtr.swap(inputCloudPtr);
239  TIMING_END(PCL_FILTER);
240 
241 
242  TIMING_START(PCl_DOWNSAMPLING);
243  gridLeafSize = getProperty<float>("GridSize");
244  pcl::VoxelGrid<PointType> grid;
245  grid.setMinimumPointsNumberPerVoxel(getProperty<int>("MinimumPointNumberPerVoxel").getValue());
246  // grid.setDownsampleAllData(false);
247  grid.setLeafSize(gridLeafSize, gridLeafSize, gridLeafSize);
248  grid.setInputCloud(inputCloudPtr);
249  grid.filter(*outputCloudPtr);
250  ARMARX_VERBOSE << "Got cloud of size: " << inputCloudPtr->size() << " after downsampling: " << outputCloudPtr->size();
251  outputCloudPtr.swap(inputCloudPtr);
252 
253 
254  if (!lastPointCloud)
255  {
256  lastPointCloud.reset(new Cloud());
257  }
258  // {
259  // *inputCloudPtr += *lastPointCloud;
260 
261  // pcl::VoxelGrid<PointType> grid;
262  // grid.setMinimumPointsNumberPerVoxel(2);
263  // // grid.setDownsampleAllData(false);
264  // grid.setLeafSize(gridLeafSize, gridLeafSize, gridLeafSize);
265  // grid.setInputCloud(inputCloudPtr);
266  // grid.filter(*outputCloudPtr);
267  // ARMARX_VERBOSE << "Cloud size after removing of temporal points (e.g. flickering): " << outputCloudPtr->size();
268  // outputCloudPtr.swap(inputCloudPtr);
269  // lastPointCloud = downsampledTransformedInputCloudPtr;
270  // }
271 
272 
273  TIMING_END(PCl_DOWNSAMPLING);
274 
275  // Eigen::Matrix4f transformScaleUp = Eigen::Matrix4f::Identity();
276  // float N2 = 1000.f;
277  // transformScaleUp (0,0) = transformScaleUp (0,0) * N2;
278  // transformScaleUp (1,1) = transformScaleUp (1,1) * N2;
279  // transformScaleUp (2,2) = transformScaleUp (2,2) * N2;
280 
281  // pcl::transformPointCloud(*inputCloudPtr, *outputCloudPtr, transformScaleUp);
282  // outputCloudPtr.swap(inputCloudPtr);
283 
284 
285 
286 
287  typename pcl::PointCloud<PointType>::Ptr occludedPoints(new pcl::PointCloud<PointType>());
288  typename pcl::PointCloud<PointType>::Ptr oldPointsInView(new pcl::PointCloud<PointType>());
289  {
290  TIMING_START(PCL_FrustumCulling);
291  std::unique_lock lock(dataMutex);
292  Eigen::Matrix4f cam2robot;
293  cam2robot << 0, 0, 1, 0,
294  0, -1, 0, 0,
295  1, 0, 0, 0,
296  0, 0, 0, 1;
297  // .. read or fill the source cloud
298  pcl::FrustumCulling<PointType> fc;
299  fc.setInputCloud(accumulatedPointCloud);
300  fc.setNegative(false);
301  fc.setVerticalFOV(40);
302  fc.setHorizontalFOV(50);
303  fc.setNearPlaneDistance(0.0);
304  fc.setFarPlaneDistance(2500);
305 
306  fc.setCameraPose(transform * cam2robot);
307 
308  fc.filter(*oldPointsInView);
309 
310 
311  fc.setNegative(true);
312  fc.filter(*outputCloudPtr);
313  outputCloudPtr.swap(accumulatedPointCloud);
314  TIMING_END(PCL_FrustumCulling);
315 
316 
317 
318 
319  TIMING_START(OCCLUSION);
320  Eigen::Vector3f cameraPos = transform.block<3, 1>(0, 3);
321  for (auto& p : *oldPointsInView)
322  {
323  Eigen::Vector3f currentPoint(p._PointXYZRGBA::x,
324  p._PointXYZRGBA::y,
325  p._PointXYZRGBA::z);
326  VirtualRobot::MathTools::BaseLine<Eigen::Vector3f> line(transform.block<3, 1>(0, 3), currentPoint - cameraPos);
327  float distP1 = (currentPoint - cameraPos).norm();
328  for (auto& p2pcl : *inputCloudPtr)
329  {
330  Eigen::Vector3f p2(p2pcl._PointXYZRGBA::x,
331  p2pcl._PointXYZRGBA::y,
332  p2pcl._PointXYZRGBA::z);
333  float distP2 = (cameraPos - p2).norm();
334  if (distP1 > distP2)
335  {
336  auto distance = VirtualRobot::MathTools::distPointLine(line, p2);
337  ARMARX_CHECK_GREATER_EQUAL(distance, 0.0f);
338  if (distance < gridLeafSize)
339  {
340  auto newP = p;
341  // newP._PointXYZRGBA::r = 255;
342  // newP._PointXYZRGBA::g = 0;
343  // newP._PointXYZRGBA::b = 0;
344  occludedPoints->push_back(newP);
345  break;
346  }
347  }
348  }
349  }
350  TIMING_END(OCCLUSION);
351 
352  TIMING_START(RemoveRobotVoxels);
353  inputCloudPtr = removeRobotVoxels(inputCloudPtr, localRobot, gridLeafSize);
354  ARMARX_VERBOSE << "Size after removal of robot voxels: " << inputCloudPtr->size();
355  TIMING_END(RemoveRobotVoxels);
356 
357  TIMING_START(Accumulation);
358  // octtree->setInputCloud(inputCloudPtr);
359  // octtree->addPointsFromInputCloud();
360  // octtree->setInputCloud(occludedPoints);
361  // octtree->addPointsFromInputCloud();
362 
363  *accumulatedPointCloud += *inputCloudPtr;
364  *accumulatedPointCloud += *occludedPoints;
365 
366 
367  *accumulatedPointCloud += *occludedPoints;
368  ARMARX_VERBOSE << "Accumulated pc size: " << accumulatedPointCloud->size() << " new point count: " << inputCloudPtr->size() << " number of occluded points: " << occludedPoints->size();
369  TIMING_END(Accumulation);
370  TIMING_START(Downsampling2);
371  // filter again to remove duplicated points
372 
373  pcl::VoxelGrid<PointType> grid2;
374  grid2.setLeafSize(gridLeafSize, gridLeafSize, gridLeafSize);
375  grid2.setInputCloud(accumulatedPointCloud);
376  grid2.filter(*outputCloudPtr);
377  outputCloudPtr.swap(accumulatedPointCloud);
378  ARMARX_VERBOSE << "Accumulated pc size after grid filtering: " << accumulatedPointCloud->size() << " " << VAROUT(gridLeafSize);
379  TIMING_END(Downsampling2);
380 
381 
382  provideResultPointClouds(accumulatedPointCloud);
383 
384  TIMING_START(PREPARATION);
385 
386  // std::map<Eigen::Vector3i, int> grid;
387  std::vector<VirtualRobot::VisualizationFactory::Color> gridColors;
388  gridPositions.reserve(accumulatedPointCloud->size());
389  gridColors.reserve(accumulatedPointCloud->size());
390 
391  DebugDrawerTriMesh ddTriMesh;
392 
393  this->gridPositions.clear();
394  this->gridPositions.reserve(accumulatedPointCloud->size());
395  // pcl::octree::OctreePointCloud<PointType>::AlignedPointTVector points;
396  // octtree->getOccupiedVoxelCenters(points);
397  for (auto& p : *accumulatedPointCloud)
398  {
399  if (!std::isnan(p._PointXYZRGBA::x) && !std::isnan(p._PointXYZRGBA::y) && !std::isnan(p._PointXYZRGBA::z))
400  {
401 
402  this->gridPositions.push_back({p._PointXYZRGBA::x,
403  p._PointXYZRGBA::y,
404  p._PointXYZRGBA::z
405  });
406  gridColors.push_back(VirtualRobot::VisualizationFactory::Color(p._PointXYZRGBA::r / 255.f,
407  p._PointXYZRGBA::g / 255.f,
408  p._PointXYZRGBA::b / 255.f,
409  0.8));
410  }
411  }
412  TIMING_END(PREPARATION);
413 
414  std::vector<Eigen::Vector3f> gridPositionsEigen = armarx::transform(this->gridPositions, +[](const Vector3f& p) -> Eigen::Vector3f
415  {
416  return {p.e0, p.e1, p.e2};
417  });
418 
419 
420  ARMARX_INFO << VAROUT(gridPositionsEigen.size());
421 
422  TIMING_START(MESHING);
423 
424  gridMesh = VirtualRobot::TriMeshUtils::CreateSparseBoxGrid(Eigen::Matrix4f::Identity(), gridPositionsEigen,
425  gridLeafSize, gridLeafSize, gridLeafSize,
426  VirtualRobot::VisualizationFactory::Color::Blue(),
427  gridColors);
428  ddTriMesh = DebugDrawerUtils::convertTriMesh(*gridMesh);
429  TIMING_END(MESHING);
430  TIMING_START(VISU);
431  // DebugDrawer24BitColoredPointCloud ddPC;
432  // visionx::tools::convertFromPCLToDebugDrawer(accumulatedPointCloud, ddPC, visionx::tools::eConvertAsColors);
433  // dd->set24BitColoredPointCloudVisu("VoxelGrid", "PointCloud", ddPC);
434  // ARMARX_INFO << "Sending point cloud to debug drawer - size: " << ddTriMesh.faces.size() << " first: " << ddTriMesh.vertices.at(0).x << ", " << ddTriMesh.vertices.at(0).y << ", " << ddTriMesh.vertices.at(0).z;
435  dd->setTriMeshVisu("VoxelGrid", "PointCloudCollisionGrid", ddTriMesh);
436  TIMING_END(VISU);
437  }
438 
439 
440 
441 
442  if (accumulatedPointCloud->size() > 0 && skipper->checkFrequency("SavingPCD")
443  && !getProperty<std::string>("PointCloudStoreFilepath").getValue().empty())
444  {
445  std::unique_lock lock(dataMutex);
446  ARMARX_INFO << "Saving point cloud to " << getProperty<std::string>("PointCloudStoreFilepath").getValue();
447  pcl::io::savePCDFile(getProperty<std::string>("PointCloudStoreFilepath").getValue(), *accumulatedPointCloud);
448  }
449  TIMING_END(FullCalc);
450 
451 
452  cycleKeeper->waitForCycleDuration();
453 
454 
455  }
456 
457 
458 
459  VoxelGridMappingProvider::CloudPtr VoxelGridMappingProvider::removeRobotVoxels(const VoxelGridMappingProvider::CloudPtr& cloud, const VirtualRobot::RobotPtr& robot, float distanceThreshold)
460  {
461  CloudPtr result(new Cloud());
462  VirtualRobot::CDManager cdm;
463  auto colModels = robot->getCollisionModels();
464  auto collisionChecker = robot->getCollisionChecker();
465  for (auto& p : *cloud)
466  {
467  bool collisionFound = false;
468  for (auto& colModel : colModels)
469  {
470  Eigen::Vector3f pEigen(p._PointXYZRGBA::x, p._PointXYZRGBA::y, p._PointXYZRGBA::z);
471  if (collisionChecker->checkCollision(colModel, pEigen, distanceThreshold))
472  {
473  collisionFound = true;
474  break;
475  }
476  }
477  if (!collisionFound)
478  {
479  result->push_back(p);
480  }
481  }
482  return result;
483  }
484 
485 
486  Vector3fSeq VoxelGridMappingProvider::getFilledGridPositions(const Ice::Current&) const
487  {
488  std::unique_lock lock(dataMutex);
489  return gridPositions;
490  }
491 
492  Ice::Float VoxelGridMappingProvider::getGridSize(const Ice::Current&) const
493  {
494  return gridLeafSize;
495  }
496 
497 
498  void VoxelGridMappingProvider::startCollectingPointClouds(const Ice::Current&)
499  {
500  collectingPointClouds = true;
501  }
502 
503  void VoxelGridMappingProvider::stopCollectingPointClouds(const Ice::Current&)
504  {
505  collectingPointClouds = false;
506  }
507 
508  void VoxelGridMappingProvider::reset(const Ice::Current&)
509  {
510  std::unique_lock lock(dataMutex);
511  ARMARX_INFO << "Clearing voxel grid";
512  accumulatedPointCloud->clear();
513  }
514 
515 
516 
517  void VoxelGridMappingProvider::componentPropertiesUpdated(const std::set<std::string>& changedProperties)
518  {
519  if (changedProperties.count("BoundingBox"))
520  {
521  updateBoundaries();
522  }
523  }
524 }
525 
RemoteRobot.h
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
TIMING_START
#define TIMING_START(name)
Definition: TimeUtil.h:280
algorithm.h
visionx::VoxelGridMappingProviderPropertyDefinitions
Definition: VoxelGridMappingProvider.h:51
armarx::VariantType::Float
const VariantTypeId Float
Definition: Variant.h:918
TIMING_END
#define TIMING_END(name)
Definition: TimeUtil.h:296
visionx
ArmarX headers.
Definition: OpenPoseStressTest.h:38
armarx::RemoteRobot::createLocalCloneFromFile
static VirtualRobot::RobotPtr createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx, VirtualRobot::RobotIO::RobotDescription loadMode=VirtualRobot::RobotIO::eFull)
This is a convenience function for createLocalClone, which automatically gets the filename from the R...
Definition: RemoteRobot.cpp:441
armarx::Split
std::vector< std::string > Split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelperTemplates.h:35
armarx::CycleUtil
This util class helps with keeping a cycle time during a control cycle.
Definition: CycleUtil.h:40
visionx::VoxelGridMappingProvider::Cloud
pcl::PointCloud< PointType > Cloud
Definition: VoxelGridMappingProvider.h:92
cxxopts::empty
bool empty(const std::string &s)
Definition: cxxopts.hpp:255
deactivateSpam
SpamFilterDataPtr deactivateSpam(SpamFilterDataPtr const &spamFilter, float deactivationDurationSec, const std::string &identifier, bool deactivate)
Definition: Logging.cpp:72
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
Color
uint32_t Color
RGBA color.
Definition: color.h:8
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::RobotPool
This class holds a pool of local VirtualRobots for multi threaded applications that can be requested ...
Definition: RobotPool.h:39
IceReportSkipper.h
ARMARX_CHECK_GREATER_EQUAL
#define ARMARX_CHECK_GREATER_EQUAL(lhs, rhs)
This macro evaluates whether lhs is greater or equal (>=) rhs and if it turns out to be false it will...
Definition: ExpressionException.h:123
PointCloudConversions.h
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT > >
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:315
armarx::RemoteRobot::synchronizeLocalCloneToTimestamp
static bool synchronizeLocalCloneToTimestamp(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp)
Synchronizes a local robot to a robot state at timestamp.
Definition: RemoteRobot.cpp:487
StringHelperTemplates.h
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:88
visionx::VoxelGridMappingProvider::CloudPtr
Cloud::Ptr CloudPtr
Definition: VoxelGridMappingProvider.h:93
DebugDrawerUtils.h
armarx::IceReportSkipper
Definition: IceReportSkipper.h:36
VoxelGridMappingProvider.h
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::DebugDrawerUtils::convertTriMesh
static DebugDrawerTriMesh convertTriMesh(const VirtualRobot::TriMeshModel &trimesh)
Definition: DebugDrawerUtils.h:34
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
norm
double norm(const Point &a)
Definition: point.hpp:94