Main MRPT website > C++ reference for MRPT 1.9.9
COctoMap.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "maps-precomp.h" // Precomp header
11 
12 #include <octomap/octomap.h>
13 #include <octomap/OcTree.h>
14 #include <mrpt/maps/COctoMap.h>
15 #include <mrpt/maps/CPointsMap.h>
19 #include <mrpt/system/filesystem.h>
20 
21 #include "COctoMapBase_impl.h"
22 
23 // Explicit instantiation:
25 
26 using namespace std;
27 using namespace mrpt;
28 using namespace mrpt::maps;
29 using namespace mrpt::img;
30 using namespace mrpt::obs;
31 using namespace mrpt::poses;
32 using namespace mrpt::math;
33 using namespace mrpt::opengl;
34 
35 // =========== Begin of Map definition ============
37 
38 COctoMap::TMapDefinition::TMapDefinition() : resolution(0.10) {}
39 void COctoMap::TMapDefinition::loadFromConfigFile_map_specific(
41  const std::string& sectionNamePrefix)
42 {
43  // [<sectionNamePrefix>+"_creationOpts"]
44  const std::string sSectCreation =
45  sectionNamePrefix + string("_creationOpts");
46  MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation);
47 
48  insertionOpts.loadFromConfigFile(
49  source, sectionNamePrefix + string("_insertOpts"));
50  likelihoodOpts.loadFromConfigFile(
51  source, sectionNamePrefix + string("_likelihoodOpts"));
52 }
53 
54 void COctoMap::TMapDefinition::dumpToTextStream_map_specific(
55  std::ostream& out) const
56 {
57  LOADABLEOPTS_DUMP_VAR(resolution, double);
58 
59  this->insertionOpts.dumpToTextStream(out);
60  this->likelihoodOpts.dumpToTextStream(out);
61 }
62 
63 mrpt::maps::CMetricMap* COctoMap::internal_CreateFromMapDefinition(
65 {
66  const COctoMap::TMapDefinition& def =
67  *dynamic_cast<const COctoMap::TMapDefinition*>(&_def);
68  COctoMap* obj = new COctoMap(def.resolution);
69  obj->insertionOptions = def.insertionOpts;
70  obj->likelihoodOptions = def.likelihoodOpts;
71  return obj;
72 }
73 // =========== End of Map definition Block =========
74 
76 
77 /*---------------------------------------------------------------
78  Constructor
79  ---------------------------------------------------------------*/
80 COctoMap::COctoMap(const double resolution) :
81  COctoMapBase<octomap::OcTree, octomap::OcTreeNode>(resolution)
82 {
83 }
84 
85 COctoMap::~COctoMap() {}
86 uint8_t COctoMap::serializeGetVersion() const { return 3; }
87 void COctoMap::serializeTo(mrpt::serialization::CArchive& out) const
88 {
89  this->likelihoodOptions.writeToStream(out);
90  this->renderingOptions.writeToStream(out); // Added in v1
91  out << genericMapParams;
92  // v2->v3: remove CMemoryChunk
93  std::stringstream ss;
94  const_cast<octomap::OcTree*>(&m_impl->m_octomap)
95  ->writeBinary(ss);
96  const std::string& buf = ss.str();
97  out << buf;
98 }
99 
100 void COctoMap::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
101 {
102  switch (version)
103  {
104  case 0:
105  case 1:
106  case 2:
107  {
109  "Deserialization of old versions of this class was "
110  "discontinued in MRPT 1.9.9 [no CMemoryChunk]");
111  }
112  break;
113  case 3:
114  {
115  this->likelihoodOptions.readFromStream(in);
116  if (version >= 1) this->renderingOptions.readFromStream(in);
117  if (version >= 2) in >> genericMapParams;
118 
119  this->clear();
120 
121  std::string buf;
122  in >> buf;
123 
124  if (!buf.empty())
125  {
126  std::stringstream ss;
127  ss.str(buf);
128  ss.seekg(0);
129  m_impl->m_octomap.readBinary(ss);
130  }
131  }
132  break;
133  default:
135  };
136 }
137 
138 bool COctoMap::internal_insertObservation(
139  const mrpt::obs::CObservation* obs, const CPose3D* robotPose)
140 {
141  octomap::point3d sensorPt;
142  octomap::Pointcloud scan;
143  if (!internal_build_PointCloud_for_observation(
144  obs, robotPose, sensorPt, scan))
145  return false; // Nothing to do.
146  // Insert rays:
147  m_impl->m_octomap
148  .insertPointCloud(
149  scan, sensorPt, insertionOptions.maxrange,
150  insertionOptions.pruning);
151  return true;
152 }
153 
154 /** Builds a renderizable representation of the octomap as a
155  * mrpt::opengl::COctoMapVoxels object. */
156 void COctoMap::getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels& gl_obj) const
157 {
158  // Go thru all voxels:
159  // OcTreeVolume voxel; // current voxel, possibly transformed
160  octomap::OcTree::tree_iterator it_end =
161  m_impl->m_octomap.end_tree();
162 
163  const unsigned char max_depth = 0; // all
164  const TColorf general_color = gl_obj.getColor();
165  const TColor general_color_u(
166  general_color.R * 255, general_color.G * 255, general_color.B * 255,
167  general_color.A * 255);
168 
169  gl_obj.clear();
170  gl_obj.reserveGridCubes(this->calcNumNodes());
171 
172  gl_obj.resizeVoxelSets(2); // 2 sets of voxels: occupied & free
173 
174  gl_obj.showVoxels(
175  VOXEL_SET_OCCUPIED, renderingOptions.visibleOccupiedVoxels);
176  gl_obj.showVoxels(VOXEL_SET_FREESPACE, renderingOptions.visibleFreeVoxels);
177 
178  const size_t nLeafs = this->getNumLeafNodes();
179  gl_obj.reserveVoxels(VOXEL_SET_OCCUPIED, nLeafs);
180  gl_obj.reserveVoxels(VOXEL_SET_FREESPACE, nLeafs);
181 
182  double xmin, xmax, ymin, ymax, zmin, zmax, inv_dz;
183  this->getMetricMin(xmin, ymin, zmin);
184  this->getMetricMax(xmax, ymax, zmax);
185  inv_dz = 1 / (zmax - zmin + 0.01);
186 
187  for (octomap::OcTree::tree_iterator it =
188  m_impl->m_octomap.begin_tree(max_depth);
189  it != it_end; ++it)
190  {
191  const octomap::point3d vx_center = it.getCoordinate();
192  const double vx_length = it.getSize();
193  const double L = 0.5 * vx_length;
194 
195  if (it.isLeaf())
196  {
197  // voxels for leaf nodes
198  const double occ = it->getOccupancy();
199  if ((occ >= 0.5 && renderingOptions.generateOccupiedVoxels) ||
200  (occ < 0.5 && renderingOptions.generateFreeVoxels))
201  {
202  mrpt::img::TColor vx_color;
203  double coefc, coeft;
204  switch (gl_obj.getVisualizationMode())
205  {
206  case COctoMapVoxels::FIXED:
207  vx_color = general_color_u;
208  break;
209  case COctoMapVoxels::COLOR_FROM_HEIGHT:
210  coefc = 255 * inv_dz * (vx_center.z() - zmin);
211  vx_color = TColor(
212  coefc * general_color.R, coefc * general_color.G,
213  coefc * general_color.B, 255.0 * general_color.A);
214  break;
215 
216  case COctoMapVoxels::COLOR_FROM_OCCUPANCY:
217  coefc = 240 * (1 - occ) + 15;
218  vx_color = TColor(
219  coefc * general_color.R, coefc * general_color.G,
220  coefc * general_color.B, 255.0 * general_color.A);
221  break;
222 
223  case COctoMapVoxels::TRANSPARENCY_FROM_OCCUPANCY:
224  coeft = 255 - 510 * (1 - occ);
225  if (coeft < 0)
226  {
227  coeft = 0;
228  }
229  vx_color = TColor(
230  255 * general_color.R, 255 * general_color.G,
231  255 * general_color.B, coeft);
232  break;
233 
234  case COctoMapVoxels::TRANS_AND_COLOR_FROM_OCCUPANCY:
235  coefc = 240 * (1 - occ) + 15;
236  vx_color = TColor(
237  coefc * general_color.R, coefc * general_color.G,
238  coefc * general_color.B, 50);
239  break;
240 
241  case COctoMapVoxels::MIXED:
242  coefc = 255 * inv_dz * (vx_center.z() - zmin);
243  coeft = 255 - 510 * (1 - occ);
244  if (coeft < 0)
245  {
246  coeft = 0;
247  }
248  vx_color = TColor(
249  coefc * general_color.R, coefc * general_color.G,
250  coefc * general_color.B, coeft);
251  break;
252 
253  default:
254  THROW_EXCEPTION("Unknown coloring scheme!");
255  }
256 
257  const size_t vx_set =
258  (m_impl->m_octomap.isNodeOccupied(*it))
261 
262  gl_obj.push_back_Voxel(
263  vx_set,
265  TPoint3D(vx_center.x(), vx_center.y(), vx_center.z()),
266  vx_length, vx_color));
267  }
268  }
269  else if (renderingOptions.generateGridLines)
270  {
271  // Not leaf-nodes:
272  const mrpt::math::TPoint3D pt_min(
273  vx_center.x() - L, vx_center.y() - L, vx_center.z() - L);
274  const mrpt::math::TPoint3D pt_max(
275  vx_center.x() + L, vx_center.y() + L, vx_center.z() + L);
276  gl_obj.push_back_GridCube(
277  COctoMapVoxels::TGridCube(pt_min, pt_max));
278  }
279  } // end for each voxel
280 
281  // if we use transparency, sort cubes by "Z" as an approximation to
282  // far-to-near render ordering:
283  if (gl_obj.isCubeTransparencyEnabled()) gl_obj.sort_voxels_by_z();
284 
285  // Set bounding box:
286  {
287  mrpt::math::TPoint3D bbmin, bbmax;
288  this->getMetricMin(bbmin.x, bbmin.y, bbmin.z);
289  this->getMetricMax(bbmax.x, bbmax.y, bbmax.z);
290  gl_obj.setBoundingBox(bbmin, bbmax);
291  }
292 }
293 
294 void COctoMap::insertRay(
295  const float end_x, const float end_y, const float end_z,
296  const float sensor_x, const float sensor_y, const float sensor_z)
297 {
298  m_impl->m_octomap
299  .insertRay(
300  octomap::point3d(sensor_x, sensor_y, sensor_z),
301  octomap::point3d(end_x, end_y, end_z), insertionOptions.maxrange,
302  insertionOptions.pruning);
303 }
304 void COctoMap::updateVoxel(
305  const double x, const double y, const double z, bool occupied)
306 {
307  m_impl->m_octomap.updateNode(x, y, z, occupied);
308 }
309 bool COctoMap::isPointWithinOctoMap(
310  const float x, const float y, const float z) const
311 {
312  octomap::OcTreeKey key;
313  return m_impl->m_octomap
314  .coordToKeyChecked(octomap::point3d(x, y, z), key);
315 }
316 
317 double COctoMap::getResolution() const
318 {
319  return m_impl->m_octomap.getResolution();
320 }
321 unsigned int COctoMap::getTreeDepth() const
322 {
323  return m_impl->m_octomap.getTreeDepth();
324 }
325 size_t COctoMap::size() const
326 {
327  return m_impl->m_octomap.size();
328 }
329 size_t COctoMap::memoryUsage() const
330 {
331  return m_impl->m_octomap.memoryUsage();
332 }
333 size_t COctoMap::memoryUsageNode() const
334 {
335  return m_impl->m_octomap.memoryUsageNode();
336 }
337 size_t COctoMap::memoryFullGrid() const
338 {
339  return m_impl->m_octomap.memoryFullGrid();
340 }
341 double COctoMap::volume() { return m_impl->m_octomap.volume(); }
342 void COctoMap::getMetricSize(double& x, double& y, double& z)
343 {
344  return m_impl->m_octomap.getMetricSize(x, y, z);
345 }
346 void COctoMap::getMetricSize(double& x, double& y, double& z) const
347 {
348  return m_impl->m_octomap.getMetricSize(x, y, z);
349 }
350 void COctoMap::getMetricMin(double& x, double& y, double& z)
351 {
352  return m_impl->m_octomap.getMetricMin(x, y, z);
353 }
354 void COctoMap::getMetricMin(double& x, double& y, double& z) const
355 {
356  return m_impl->m_octomap.getMetricMin(x, y, z);
357 }
358 void COctoMap::getMetricMax(double& x, double& y, double& z)
359 {
360  return m_impl->m_octomap.getMetricMax(x, y, z);
361 }
362 void COctoMap::getMetricMax(double& x, double& y, double& z) const
363 {
364  return m_impl->m_octomap.getMetricMax(x, y, z);
365 }
366 size_t COctoMap::calcNumNodes() const
367 {
368  return m_impl->m_octomap.calcNumNodes();
369 }
370 size_t COctoMap::getNumLeafNodes() const
371 {
372  return m_impl->m_octomap.getNumLeafNodes();
373 }
374 void COctoMap::setOccupancyThres(double prob)
375 {
376  m_impl->m_octomap.setOccupancyThres(prob);
377 }
378 void COctoMap::setProbHit(double prob)
379 {
380  m_impl->m_octomap.setProbHit(prob);
381 }
382 void COctoMap::setProbMiss(double prob)
383 {
384  m_impl->m_octomap.setProbMiss(prob);
385 }
386 void COctoMap::setClampingThresMin(double thresProb)
387 {
388  m_impl->m_octomap.setClampingThresMin(thresProb);
389 }
390 void COctoMap::setClampingThresMax(double thresProb)
391 {
392  m_impl->m_octomap.setClampingThresMax(thresProb);
393 }
394 double COctoMap::getOccupancyThres() const
395 {
396  return m_impl->m_octomap.getOccupancyThres();
397 }
398 float COctoMap::getOccupancyThresLog() const
399 {
400  return m_impl->m_octomap.getOccupancyThresLog();
401 }
402 double COctoMap::getProbHit() const
403 {
404  return m_impl->m_octomap.getProbHit();
405 }
406 float COctoMap::getProbHitLog() const
407 {
408  return m_impl->m_octomap.getProbHitLog();
409 }
410 double COctoMap::getProbMiss() const
411 {
412  return m_impl->m_octomap.getProbMiss();
413 }
414 float COctoMap::getProbMissLog() const
415 {
416  return m_impl->m_octomap.getProbMissLog();
417 }
418 double COctoMap::getClampingThresMin() const
419 {
420  return m_impl->m_octomap.getClampingThresMin();
421 }
422 float COctoMap::getClampingThresMinLog() const
423 {
424  return m_impl->m_octomap.getClampingThresMinLog();
425 }
426 double COctoMap::getClampingThresMax() const
427 {
428  return m_impl->m_octomap.getClampingThresMax();
429 }
430 float COctoMap::getClampingThresMaxLog() const
431 {
432  return m_impl->m_octomap.getClampingThresMaxLog();
433 }
434 void COctoMap::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)
GLdouble GLdouble z
Definition: glext.h:3872
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
double resolution
The finest resolution of the octomap (default: 0.10.
Definition: COctoMap.h:48
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)
STL namespace.
void clear()
Clears everything.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
void push_back_GridCube(const TGridCube &c)
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::COctoMap::TInsertionOptions insertionOpts
meters)
Definition: COctoMap.h:51
mrpt::maps::COctoMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
Definition: COctoMap.h:53
This namespace contains representation of robot actions and observations.
double x
X,Y,Z coordinates.
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
Definition: COctoMap.h:35
GLsizei const GLchar ** string
Definition: glext.h:4101
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)
GLclampd zmax
Definition: glext.h:7918
#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.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:56
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
void resizeVoxelSets(const size_t nVoxelSets)
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:79
visualization_mode_t getVisualizationMode() const
GLuint in
Definition: glext.h:7274
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
GLenum GLint GLint y
Definition: glext.h:3538
GLsizeiptr size
Definition: glext.h:3923
A RGB color - 8bit.
Definition: TColor.h:22
GLenum GLint x
Definition: glext.h:3538
Lightweight 3D point.
bool isCubeTransparencyEnabled() const
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:188



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019