MRPT  1.9.9
CColouredOctoMap.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "maps-precomp.h" // Precomp header
11 
12 #include <octomap/ColorOcTree.h>
13 #include <octomap/octomap.h>
14 
16 #include <mrpt/maps/CPointsMap.h>
19 
23 
25 #include <mrpt/system/filesystem.h>
26 #include <sstream>
27 
28 #include "COctoMapBase_impl.h"
29 
30 // Explicit instantiation:
31 template class mrpt::maps::COctoMapBase<
32  octomap::ColorOcTree, octomap::ColorOcTreeNode>;
33 
34 using namespace std;
35 using namespace mrpt;
36 using namespace mrpt::maps;
37 using namespace mrpt::obs;
38 using namespace mrpt::poses;
39 using namespace mrpt::img;
40 using namespace mrpt::opengl;
41 using namespace mrpt::math;
42 
43 // =========== Begin of Map definition ============
45  "mrpt::maps::CColouredOctoMap,colourOctoMap,colorOctoMap",
47 
48 CColouredOctoMap::TMapDefinition::TMapDefinition() = default;
49 void CColouredOctoMap::TMapDefinition::loadFromConfigFile_map_specific(
50  const mrpt::config::CConfigFileBase& source,
51  const std::string& sectionNamePrefix)
52 {
53  // [<sectionNamePrefix>+"_creationOpts"]
54  const std::string sSectCreation =
55  sectionNamePrefix + string("_creationOpts");
56  MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation);
57 
58  insertionOpts.loadFromConfigFile(
59  source, sectionNamePrefix + string("_insertOpts"));
60  likelihoodOpts.loadFromConfigFile(
61  source, sectionNamePrefix + string("_likelihoodOpts"));
62 }
63 
64 void CColouredOctoMap::TMapDefinition::dumpToTextStream_map_specific(
65  std::ostream& out) const
66 {
67  LOADABLEOPTS_DUMP_VAR(resolution, double);
68 
69  this->insertionOpts.dumpToTextStream(out);
70  this->likelihoodOpts.dumpToTextStream(out);
71 }
72 
73 mrpt::maps::CMetricMap* CColouredOctoMap::internal_CreateFromMapDefinition(
75 {
77  *dynamic_cast<const CColouredOctoMap::TMapDefinition*>(&_def);
78  auto* obj = new CColouredOctoMap(def.resolution);
79  obj->insertionOptions = def.insertionOpts;
80  obj->likelihoodOptions = def.likelihoodOpts;
81  return obj;
82 }
83 // =========== End of Map definition Block =========
84 
86 
87 CColouredOctoMap::CColouredOctoMap(const double resolution)
88  : COctoMapBase<octomap::ColorOcTree, octomap::ColorOcTreeNode>(resolution)
89 
90 {
91 }
92 
93 CColouredOctoMap::~CColouredOctoMap() = default;
94 uint8_t CColouredOctoMap::serializeGetVersion() const { return 3; }
95 void CColouredOctoMap::serializeTo(mrpt::serialization::CArchive& out) const
96 {
97  this->likelihoodOptions.writeToStream(out);
98  this->renderingOptions.writeToStream(out); // Added in v1
99  out << genericMapParams; // v2
100 
101  // v2->v3: remove CMemoryChunk
102  std::stringstream ss;
103  m_impl->m_octomap.writeBinaryConst(ss);
104  const std::string& buf = ss.str();
105  out << buf;
106 }
107 
108 void CColouredOctoMap::serializeFrom(
109  mrpt::serialization::CArchive& in, uint8_t version)
110 {
111  switch (version)
112  {
113  case 0:
114  case 1:
115  case 2:
116  {
118  "Deserialization of old versions of this class was "
119  "discontinued in MRPT 1.9.9 [no CMemoryChunk]");
120  }
121  break;
122  case 3:
123  {
124  this->likelihoodOptions.readFromStream(in);
125  if (version >= 1) this->renderingOptions.readFromStream(in);
126  if (version >= 2) in >> genericMapParams;
127 
128  this->clear();
129 
130  std::string buf;
131  in >> buf;
132 
133  if (!buf.empty())
134  {
135  std::stringstream ss;
136  ss.str(buf);
137  ss.seekg(0);
138  m_impl->m_octomap.readBinary(ss);
139  }
140  }
141  break;
142  default:
144  };
145 }
146 
147 /*---------------------------------------------------------------
148  insertObservation
149  ---------------------------------------------------------------*/
150 bool CColouredOctoMap::internal_insertObservation(
151  const mrpt::obs::CObservation& obs, const CPose3D* robotPose)
152 {
153  octomap::point3d sensorPt;
154  octomap::Pointcloud scan;
155 
156  CPose3D robotPose3D;
157  if (robotPose) // Default values are (0,0,0)
158  robotPose3D = (*robotPose);
159 
161  {
162  /********************************************************************
163  OBSERVATION TYPE: CObservation2DRangeScan
164  ********************************************************************/
165  const auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);
166 
167  // Build a points-map representation of the points from the scan
168  // (coordinates are wrt the robot base)
169 
170  // Sensor_pose = robot_pose (+) sensor_pose_on_robot
171  CPose3D sensorPose(UNINITIALIZED_POSE);
172  sensorPose.composeFrom(robotPose3D, o.sensorPose);
173  sensorPt =
174  octomap::point3d(sensorPose.x(), sensorPose.y(), sensorPose.z());
175 
176  const auto* scanPts = o.buildAuxPointsMap<mrpt::maps::CPointsMap>();
177  const size_t nPts = scanPts->size();
178 
179  // Transform 3D point cloud:
180  scan.reserve(nPts);
181 
183  for (size_t i = 0; i < nPts; i++)
184  {
185  // Load the next point:
186  scanPts->getPointFast(i, pt.x, pt.y, pt.z);
187 
188  // Translation:
189  double gx, gy, gz;
190  robotPose3D.composePoint(pt.x, pt.y, pt.z, gx, gy, gz);
191 
192  // Add to this map:
193  scan.push_back(gx, gy, gz);
194  }
195 
196  // Insert rays:
197  m_impl->m_octomap.insertPointCloud(
198  scan, sensorPt, insertionOptions.maxrange,
199  insertionOptions.pruning);
200  return true;
201  }
202  else if (IS_CLASS(obs, CObservation3DRangeScan))
203  {
204  /********************************************************************
205  OBSERVATION TYPE: CObservation3DRangeScan
206  ********************************************************************/
207  const auto& o = dynamic_cast<const CObservation3DRangeScan&>(obs);
208 
209  o.load(); // Just to make sure the points are loaded from an external
210  // source, if that's the case...
211 
212  // Project 3D points & color:
215  T3DPointsProjectionParams proj_params;
216  proj_params.PROJ3D_USE_LUT = true;
217  proj_params.robotPoseInTheWorld = robotPose;
218  const_cast<CObservation3DRangeScan&>(o)
219  .project3DPointsFromDepthImageInto(*pts, proj_params);
220 
221  // Sensor_pose = robot_pose (+) sensor_pose_on_robot
222  CPose3D sensorPose(UNINITIALIZED_POSE);
223  sensorPose.composeFrom(robotPose3D, o.sensorPose);
224  sensorPt =
225  octomap::point3d(sensorPose.x(), sensorPose.y(), sensorPose.z());
226 
227  const size_t sizeRangeScan = pts->size();
228  scan.reserve(sizeRangeScan);
229 
230  for (size_t i = 0; i < sizeRangeScan; i++)
231  {
233  pts->getPoint(i);
234 
235  // Add to this map:
236  if (pt.x != 0 || pt.y != 0 || pt.z != 0)
237  scan.push_back(pt.x, pt.y, pt.z);
238  }
239 
240  // Insert rays:
241  octomap::KeySet free_cells, occupied_cells;
242  m_impl->m_octomap.computeUpdate(
243  scan, sensorPt, free_cells, occupied_cells,
244  insertionOptions.maxrange);
245 
246  // insert data into tree -----------------------
247  for (const auto& free_cell : free_cells)
248  {
249  m_impl->m_octomap.updateNode(free_cell, false, false);
250  }
251  for (const auto& occupied_cell : occupied_cells)
252  {
253  m_impl->m_octomap.updateNode(occupied_cell, true, false);
254  }
255 
256  // Update color -----------------------
257  const float colF2B = 255.0f;
258  for (size_t i = 0; i < sizeRangeScan; i++)
259  {
261  pts->getPoint(i);
262 
263  // Add to this map:
264  if (pt.x != 0 || pt.y != 0 || pt.z != 0)
265  this->updateVoxelColour(
266  pt.x, pt.y, pt.z, uint8_t(pt.R * colF2B),
267  uint8_t(pt.G * colF2B), uint8_t(pt.B * colF2B));
268  }
269 
270  // TODO: does pruning make sense if we used "lazy_eval"?
271  if (insertionOptions.pruning) m_impl->m_octomap.prune();
272 
273  return true;
274  }
275 
276  return false;
277 }
278 
279 /** Get the RGB colour of a point
280  * \return false if the point is not mapped, in which case the returned colour
281  * is undefined. */
282 bool CColouredOctoMap::getPointColour(
283  const float x, const float y, const float z, uint8_t& r, uint8_t& g,
284  uint8_t& b) const
285 {
286  octomap::OcTreeKey key;
287  if (m_impl->m_octomap.coordToKeyChecked(octomap::point3d(x, y, z), key))
288  {
289  octomap::ColorOcTreeNode* node =
290  m_impl->m_octomap.search(key, 0 /*depth*/);
291  if (!node) return false;
292 
293  const octomap::ColorOcTreeNode::Color& col = node->getColor();
294  r = col.r;
295  g = col.g;
296  b = col.b;
297  return true;
298  }
299  else
300  return false;
301 }
302 
303 /** Manually update the colour of the voxel at (x,y,z) */
304 void CColouredOctoMap::updateVoxelColour(
305  const double x, const double y, const double z, const uint8_t r,
306  const uint8_t g, const uint8_t b)
307 {
308  switch (m_colour_method)
309  {
310  case INTEGRATE:
311  m_impl->m_octomap.integrateNodeColor(x, y, z, r, g, b);
312  break;
313  case SET:
314  m_impl->m_octomap.setNodeColor(x, y, z, r, g, b);
315  break;
316  case AVERAGE:
317  m_impl->m_octomap.averageNodeColor(x, y, z, r, g, b);
318  break;
319  default:
320  THROW_EXCEPTION("Invalid value found for 'm_colour_method'");
321  }
322 }
323 
324 /** Builds a renderizable representation of the octomap as a
325  * mrpt::opengl::COctoMapVoxels object. */
326 void CColouredOctoMap::getAsOctoMapVoxels(
327  mrpt::opengl::COctoMapVoxels& gl_obj) const
328 {
329  // Go thru all voxels:
330 
331  // OcTreeVolume voxel; // current voxel, possibly transformed
332  octomap::ColorOcTree::tree_iterator it_end = m_impl->m_octomap.end_tree();
333 
334  const unsigned char max_depth = 0; // all
335  const TColorf general_color = gl_obj.getColor();
336  const TColor general_color_u(
337  general_color.R * 255, general_color.G * 255, general_color.B * 255,
338  general_color.A * 255);
339 
340  gl_obj.clear();
341  gl_obj.reserveGridCubes(this->calcNumNodes());
342 
343  gl_obj.resizeVoxelSets(2); // 2 sets of voxels: occupied & free
344 
345  gl_obj.showVoxels(
346  VOXEL_SET_OCCUPIED, renderingOptions.visibleOccupiedVoxels);
347  gl_obj.showVoxels(VOXEL_SET_FREESPACE, renderingOptions.visibleFreeVoxels);
348 
349  const size_t nLeafs = this->getNumLeafNodes();
350  gl_obj.reserveVoxels(VOXEL_SET_OCCUPIED, nLeafs);
351  gl_obj.reserveVoxels(VOXEL_SET_FREESPACE, nLeafs);
352 
353  double xmin, xmax, ymin, ymax, zmin, zmax;
354  this->getMetricMin(xmin, ymin, zmin);
355  this->getMetricMax(xmax, ymax, zmax);
356 
357  for (octomap::ColorOcTree::tree_iterator it =
358  m_impl->m_octomap.begin_tree(max_depth);
359  it != it_end; ++it)
360  {
361  const octomap::point3d vx_center = it.getCoordinate();
362  const double vx_length = it.getSize();
363  const double L = 0.5 * vx_length;
364 
365  if (it.isLeaf())
366  {
367  // voxels for leaf nodes
368  const double occ = it->getOccupancy();
369  if ((occ >= 0.5 && renderingOptions.generateOccupiedVoxels) ||
370  (occ < 0.5 && renderingOptions.generateFreeVoxels))
371  {
372  mrpt::img::TColor vx_color;
373  octomap::ColorOcTreeNode::Color node_color = it->getColor();
374  vx_color = TColor(node_color.r, node_color.g, node_color.b);
375 
376  const size_t vx_set = (m_impl->m_octomap.isNodeOccupied(*it))
379 
380  gl_obj.push_back_Voxel(
381  vx_set,
383  TPoint3D(vx_center.x(), vx_center.y(), vx_center.z()),
384  vx_length, vx_color));
385  }
386  }
387  else if (renderingOptions.generateGridLines)
388  {
389  // Not leaf-nodes:
390  const mrpt::math::TPoint3D pt_min(
391  vx_center.x() - L, vx_center.y() - L, vx_center.z() - L);
392  const mrpt::math::TPoint3D pt_max(
393  vx_center.x() + L, vx_center.y() + L, vx_center.z() + L);
394  gl_obj.push_back_GridCube(
395  COctoMapVoxels::TGridCube(pt_min, pt_max));
396  }
397  } // end for each voxel
398 
399  // if we use transparency, sort cubes by "Z" as an approximation to
400  // far-to-near render ordering:
401  if (gl_obj.isCubeTransparencyEnabled()) gl_obj.sort_voxels_by_z();
402 
403  // Set bounding box:
404  {
405  mrpt::math::TPoint3D bbmin, bbmax;
406  this->getMetricMin(bbmin.x, bbmin.y, bbmin.z);
407  this->getMetricMax(bbmax.x, bbmax.y, bbmax.z);
408  gl_obj.setBoundingBox(bbmin, bbmax);
409  }
410 }
411 
412 void CColouredOctoMap::insertRay(
413  const float end_x, const float end_y, const float end_z,
414  const float sensor_x, const float sensor_y, const float sensor_z)
415 {
416  m_impl->m_octomap.insertRay(
417  octomap::point3d(sensor_x, sensor_y, sensor_z),
418  octomap::point3d(end_x, end_y, end_z), insertionOptions.maxrange,
419  insertionOptions.pruning);
420 }
421 void CColouredOctoMap::updateVoxel(
422  const double x, const double y, const double z, bool occupied)
423 {
424  m_impl->m_octomap.updateNode(x, y, z, occupied);
425 }
426 bool CColouredOctoMap::isPointWithinOctoMap(
427  const float x, const float y, const float z) const
428 {
429  octomap::OcTreeKey key;
430  return m_impl->m_octomap.coordToKeyChecked(octomap::point3d(x, y, z), key);
431 }
432 
433 double CColouredOctoMap::getResolution() const
434 {
435  return m_impl->m_octomap.getResolution();
436 }
437 unsigned int CColouredOctoMap::getTreeDepth() const
438 {
439  return m_impl->m_octomap.getTreeDepth();
440 }
441 size_t CColouredOctoMap::size() const { return m_impl->m_octomap.size(); }
442 size_t CColouredOctoMap::memoryUsage() const
443 {
444  return m_impl->m_octomap.memoryUsage();
445 }
446 size_t CColouredOctoMap::memoryUsageNode() const
447 {
448  return m_impl->m_octomap.memoryUsageNode();
449 }
450 size_t CColouredOctoMap::memoryFullGrid() const
451 {
452  return m_impl->m_octomap.memoryFullGrid();
453 }
454 double CColouredOctoMap::volume() { return m_impl->m_octomap.volume(); }
455 void CColouredOctoMap::getMetricSize(double& x, double& y, double& z)
456 {
457  return m_impl->m_octomap.getMetricSize(x, y, z);
458 }
459 void CColouredOctoMap::getMetricSize(double& x, double& y, double& z) const
460 {
461  return m_impl->m_octomap.getMetricSize(x, y, z);
462 }
463 void CColouredOctoMap::getMetricMin(double& x, double& y, double& z)
464 {
465  return m_impl->m_octomap.getMetricMin(x, y, z);
466 }
467 void CColouredOctoMap::getMetricMin(double& x, double& y, double& z) const
468 {
469  return m_impl->m_octomap.getMetricMin(x, y, z);
470 }
471 void CColouredOctoMap::getMetricMax(double& x, double& y, double& z)
472 {
473  return m_impl->m_octomap.getMetricMax(x, y, z);
474 }
475 void CColouredOctoMap::getMetricMax(double& x, double& y, double& z) const
476 {
477  return m_impl->m_octomap.getMetricMax(x, y, z);
478 }
479 size_t CColouredOctoMap::calcNumNodes() const
480 {
481  return m_impl->m_octomap.calcNumNodes();
482 }
483 size_t CColouredOctoMap::getNumLeafNodes() const
484 {
485  return m_impl->m_octomap.getNumLeafNodes();
486 }
487 void CColouredOctoMap::setOccupancyThres(double prob)
488 {
489  m_impl->m_octomap.setOccupancyThres(prob);
490 }
491 void CColouredOctoMap::setProbHit(double prob)
492 {
493  m_impl->m_octomap.setProbHit(prob);
494 }
495 void CColouredOctoMap::setProbMiss(double prob)
496 {
497  m_impl->m_octomap.setProbMiss(prob);
498 }
499 void CColouredOctoMap::setClampingThresMin(double thresProb)
500 {
501  m_impl->m_octomap.setClampingThresMin(thresProb);
502 }
503 void CColouredOctoMap::setClampingThresMax(double thresProb)
504 {
505  m_impl->m_octomap.setClampingThresMax(thresProb);
506 }
507 double CColouredOctoMap::getOccupancyThres() const
508 {
509  return m_impl->m_octomap.getOccupancyThres();
510 }
511 float CColouredOctoMap::getOccupancyThresLog() const
512 {
513  return m_impl->m_octomap.getOccupancyThresLog();
514 }
515 double CColouredOctoMap::getProbHit() const
516 {
517  return m_impl->m_octomap.getProbHit();
518 }
519 float CColouredOctoMap::getProbHitLog() const
520 {
521  return m_impl->m_octomap.getProbHitLog();
522 }
523 double CColouredOctoMap::getProbMiss() const
524 {
525  return m_impl->m_octomap.getProbMiss();
526 }
527 float CColouredOctoMap::getProbMissLog() const
528 {
529  return m_impl->m_octomap.getProbMissLog();
530 }
531 double CColouredOctoMap::getClampingThresMin() const
532 {
533  return m_impl->m_octomap.getClampingThresMin();
534 }
535 float CColouredOctoMap::getClampingThresMinLog() const
536 {
537  return m_impl->m_octomap.getClampingThresMinLog();
538 }
539 double CColouredOctoMap::getClampingThresMax() const
540 {
541  return m_impl->m_octomap.getClampingThresMax();
542 }
543 float CColouredOctoMap::getClampingThresMaxLog() const
544 {
545  return m_impl->m_octomap.getClampingThresMaxLog();
546 }
547 void CColouredOctoMap::internal_clear() { m_impl->m_octomap.clear(); }
void showVoxels(unsigned int voxel_set, bool show)
Shows/hides the voxels (voxel_set is a 0-based index for the set of voxels to modify, e.g.
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
void reserveVoxels(const size_t set_index, const size_t nVoxels)
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
static Ptr Create(Args &&... args)
const mrpt::poses::CPose3D * robotPoseInTheWorld
(Default: nullptr) Read takeIntoAccountSensorPoseOnRobot
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
size_t size(const MATRIXLIKE &m, const int dim)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void setBoundingBox(const mrpt::math::TPoint3D &bb_min, const mrpt::math::TPoint3D &bb_max)
Manually changes the bounding box (normally the user doesn&#39;t need to call this)
void reserveGridCubes(const size_t nCubes)
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
mrpt::maps::CColouredOctoMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
STL namespace.
void clear()
Clears everything.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
Definition: COctoMapBase.h:45
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
void push_back_GridCube(const TGridCube &c)
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:65
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
mrpt::maps::CColouredOctoMap::TInsertionOptions insertionOpts
meters)
Lightweight 3D point (float version).
Definition: TPoint3D.h:21
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:572
void project3DPointsFromDepthImageInto(mrpt::obs::CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams &projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams)
This namespace contains representation of robot actions and observations.
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
The info of each grid block.
void push_back_Voxel(const size_t set_index, const TVoxel &v)
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
The info of each of the voxels.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
void resizeVoxelSets(const size_t nVoxelSets)
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
mrpt::img::TColorf getColor() const
Returns the object color property as a TColorf.
A RGB color - floats in the range [0,1].
Definition: TColor.h:78
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
bool PROJ3D_USE_LUT
(Default:true) [Only used when range_is_depth=true] Whether to use a Look-up-table (LUT) to speed up ...
void composePoint(double lx, double ly, double lz, double &gx, double &gy, double &gz, mrpt::optional_ref< mrpt::math::CMatrixDouble33 > out_jacobian_df_dpoint=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dpose=std::nullopt, mrpt::optional_ref< mrpt::math::CMatrixDouble36 > out_jacobian_df_dse3=std::nullopt, bool use_small_rot_approx=false) const
An alternative, slightly more efficient way of doing with G and L being 3D points and P this 6D pose...
A RGB color - 8bit.
Definition: TColor.h:20
Lightweight 3D point.
Definition: TPoint3D.h:90
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
double resolution
The finest resolution of the octomap (default: 0.10.
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:441
bool isCubeTransparencyEnabled() const
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
void load() const override
Makes sure all images and other fields which may be externally stored are loaded in memory...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7e629e01a Sat Dec 14 00:05:55 2019 +0100 at sáb dic 14 00:15:10 CET 2019