MRPT  2.0.1
COctoMap.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-2020, 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 <mrpt/maps/COctoMap.h>
13 #include <mrpt/maps/CPointsMap.h>
17 #include <mrpt/system/filesystem.h>
18 #include <octomap/OcTree.h>
19 #include <octomap/octomap.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 ============
36 MAP_DEFINITION_REGISTER("mrpt::maps::COctoMap,octoMap", mrpt::maps::COctoMap)
37 
38 COctoMap::TMapDefinition::TMapDefinition() = default;
39 void COctoMap::TMapDefinition::loadFromConfigFile_map_specific(
40  const mrpt::config::CConfigFileBase& source,
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  auto* 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() = default;
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)->writeBinary(ss);
95  const std::string& buf = ss.str();
96  out << buf;
97 }
98 
99 void COctoMap::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
100 {
101  switch (version)
102  {
103  case 0:
104  case 1:
105  case 2:
106  {
108  "Deserialization of old versions of this class was "
109  "discontinued in MRPT 1.9.9 [no CMemoryChunk]");
110  }
111  break;
112  case 3:
113  {
114  this->likelihoodOptions.readFromStream(in);
115  if (version >= 1) this->renderingOptions.readFromStream(in);
116  if (version >= 2) in >> genericMapParams;
117 
118  this->clear();
119 
120  std::string buf;
121  in >> buf;
122 
123  if (!buf.empty())
124  {
125  std::stringstream ss;
126  ss.str(buf);
127  ss.seekg(0);
128  m_impl->m_octomap.readBinary(ss);
129  }
130  }
131  break;
132  default:
134  };
135 }
136 
137 bool COctoMap::internal_insertObservation(
138  const mrpt::obs::CObservation& obs, const CPose3D* robotPose)
139 {
140  octomap::point3d sensorPt;
141  octomap::Pointcloud scan;
142  if (!internal_build_PointCloud_for_observation(
143  obs, robotPose, sensorPt, scan))
144  return false; // Nothing to do.
145  // Insert rays:
146  m_impl->m_octomap.insertPointCloud(
147  scan, sensorPt, insertionOptions.maxrange, insertionOptions.pruning);
148  return true;
149 }
150 
151 /** Builds a renderizable representation of the octomap as a
152  * mrpt::opengl::COctoMapVoxels object. */
153 void COctoMap::getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels& gl_obj) const
154 {
155  // Go thru all voxels:
156  // OcTreeVolume voxel; // current voxel, possibly transformed
157  octomap::OcTree::tree_iterator it_end = m_impl->m_octomap.end_tree();
158 
159  const unsigned char max_depth = 0; // all
160  const TColorf general_color = gl_obj.getColor();
161  const TColor general_color_u(
162  general_color.R * 255, general_color.G * 255, general_color.B * 255,
163  general_color.A * 255);
164 
165  gl_obj.clear();
166  gl_obj.reserveGridCubes(this->calcNumNodes());
167 
168  gl_obj.resizeVoxelSets(2); // 2 sets of voxels: occupied & free
169 
170  gl_obj.showVoxels(
171  VOXEL_SET_OCCUPIED, renderingOptions.visibleOccupiedVoxels);
172  gl_obj.showVoxels(VOXEL_SET_FREESPACE, renderingOptions.visibleFreeVoxels);
173 
174  const size_t nLeafs = this->getNumLeafNodes();
175  gl_obj.reserveVoxels(VOXEL_SET_OCCUPIED, nLeafs);
176  gl_obj.reserveVoxels(VOXEL_SET_FREESPACE, nLeafs);
177 
178  double xmin, xmax, ymin, ymax, zmin, zmax, inv_dz;
179  this->getMetricMin(xmin, ymin, zmin);
180  this->getMetricMax(xmax, ymax, zmax);
181  inv_dz = 1 / (zmax - zmin + 0.01);
182 
183  for (octomap::OcTree::tree_iterator it =
184  m_impl->m_octomap.begin_tree(max_depth);
185  it != it_end; ++it)
186  {
187  const octomap::point3d vx_center = it.getCoordinate();
188  const double vx_length = it.getSize();
189  const double L = 0.5 * vx_length;
190 
191  if (it.isLeaf())
192  {
193  // voxels for leaf nodes
194  const double occ = it->getOccupancy();
195  if ((occ >= 0.5 && renderingOptions.generateOccupiedVoxels) ||
196  (occ < 0.5 && renderingOptions.generateFreeVoxels))
197  {
198  mrpt::img::TColor vx_color;
199  double coefc, coeft;
200  switch (gl_obj.getVisualizationMode())
201  {
202  case COctoMapVoxels::FIXED:
203  vx_color = general_color_u;
204  break;
205  case COctoMapVoxels::COLOR_FROM_HEIGHT:
206  coefc = 255 * inv_dz * (vx_center.z() - zmin);
207  vx_color = TColor(
208  coefc * general_color.R, coefc * general_color.G,
209  coefc * general_color.B, 255.0 * general_color.A);
210  break;
211 
212  case COctoMapVoxels::COLOR_FROM_OCCUPANCY:
213  coefc = 240 * (1 - occ) + 15;
214  vx_color = TColor(
215  coefc * general_color.R, coefc * general_color.G,
216  coefc * general_color.B, 255.0 * general_color.A);
217  break;
218 
219  case COctoMapVoxels::TRANSPARENCY_FROM_OCCUPANCY:
220  coeft = 255 - 510 * (1 - occ);
221  if (coeft < 0)
222  {
223  coeft = 0;
224  }
225  vx_color = TColor(
226  255 * general_color.R, 255 * general_color.G,
227  255 * general_color.B, coeft);
228  break;
229 
230  case COctoMapVoxels::TRANS_AND_COLOR_FROM_OCCUPANCY:
231  coefc = 240 * (1 - occ) + 15;
232  vx_color = TColor(
233  coefc * general_color.R, coefc * general_color.G,
234  coefc * general_color.B, 50);
235  break;
236 
237  case COctoMapVoxels::MIXED:
238  coefc = 255 * inv_dz * (vx_center.z() - zmin);
239  coeft = 255 - 510 * (1 - occ);
240  if (coeft < 0)
241  {
242  coeft = 0;
243  }
244  vx_color = TColor(
245  coefc * general_color.R, coefc * general_color.G,
246  coefc * general_color.B, coeft);
247  break;
248 
249  default:
250  THROW_EXCEPTION("Unknown coloring scheme!");
251  }
252 
253  const size_t vx_set = (m_impl->m_octomap.isNodeOccupied(*it))
256 
257  gl_obj.push_back_Voxel(
258  vx_set,
260  TPoint3D(vx_center.x(), vx_center.y(), vx_center.z()),
261  vx_length, vx_color));
262  }
263  }
264  else if (renderingOptions.generateGridLines)
265  {
266  // Not leaf-nodes:
267  const mrpt::math::TPoint3D pt_min(
268  vx_center.x() - L, vx_center.y() - L, vx_center.z() - L);
269  const mrpt::math::TPoint3D pt_max(
270  vx_center.x() + L, vx_center.y() + L, vx_center.z() + L);
271  gl_obj.push_back_GridCube(
272  COctoMapVoxels::TGridCube(pt_min, pt_max));
273  }
274  } // end for each voxel
275 
276  // if we use transparency, sort cubes by "Z" as an approximation to
277  // far-to-near render ordering:
278  if (gl_obj.isCubeTransparencyEnabled()) gl_obj.sort_voxels_by_z();
279 
280  // Set bounding box:
281  {
282  mrpt::math::TPoint3D bbmin, bbmax;
283  this->getMetricMin(bbmin.x, bbmin.y, bbmin.z);
284  this->getMetricMax(bbmax.x, bbmax.y, bbmax.z);
285  gl_obj.setBoundingBox(bbmin, bbmax);
286  }
287 }
288 
289 void COctoMap::insertRay(
290  const float end_x, const float end_y, const float end_z,
291  const float sensor_x, const float sensor_y, const float sensor_z)
292 {
293  m_impl->m_octomap.insertRay(
294  octomap::point3d(sensor_x, sensor_y, sensor_z),
295  octomap::point3d(end_x, end_y, end_z), insertionOptions.maxrange,
296  insertionOptions.pruning);
297 }
298 void COctoMap::updateVoxel(
299  const double x, const double y, const double z, bool occupied)
300 {
301  m_impl->m_octomap.updateNode(x, y, z, occupied);
302 }
303 bool COctoMap::isPointWithinOctoMap(
304  const float x, const float y, const float z) const
305 {
306  octomap::OcTreeKey key;
307  return m_impl->m_octomap.coordToKeyChecked(octomap::point3d(x, y, z), key);
308 }
309 
310 double COctoMap::getResolution() const
311 {
312  return m_impl->m_octomap.getResolution();
313 }
314 unsigned int COctoMap::getTreeDepth() const
315 {
316  return m_impl->m_octomap.getTreeDepth();
317 }
318 size_t COctoMap::size() const { return m_impl->m_octomap.size(); }
319 size_t COctoMap::memoryUsage() const { return m_impl->m_octomap.memoryUsage(); }
320 size_t COctoMap::memoryUsageNode() const
321 {
322  return m_impl->m_octomap.memoryUsageNode();
323 }
324 size_t COctoMap::memoryFullGrid() const
325 {
326  return m_impl->m_octomap.memoryFullGrid();
327 }
328 double COctoMap::volume() { return m_impl->m_octomap.volume(); }
329 void COctoMap::getMetricSize(double& x, double& y, double& z)
330 {
331  return m_impl->m_octomap.getMetricSize(x, y, z);
332 }
333 void COctoMap::getMetricSize(double& x, double& y, double& z) const
334 {
335  return m_impl->m_octomap.getMetricSize(x, y, z);
336 }
337 void COctoMap::getMetricMin(double& x, double& y, double& z)
338 {
339  return m_impl->m_octomap.getMetricMin(x, y, z);
340 }
341 void COctoMap::getMetricMin(double& x, double& y, double& z) const
342 {
343  return m_impl->m_octomap.getMetricMin(x, y, z);
344 }
345 void COctoMap::getMetricMax(double& x, double& y, double& z)
346 {
347  return m_impl->m_octomap.getMetricMax(x, y, z);
348 }
349 void COctoMap::getMetricMax(double& x, double& y, double& z) const
350 {
351  return m_impl->m_octomap.getMetricMax(x, y, z);
352 }
353 size_t COctoMap::calcNumNodes() const
354 {
355  return m_impl->m_octomap.calcNumNodes();
356 }
357 size_t COctoMap::getNumLeafNodes() const
358 {
359  return m_impl->m_octomap.getNumLeafNodes();
360 }
361 void COctoMap::setOccupancyThres(double prob)
362 {
363  m_impl->m_octomap.setOccupancyThres(prob);
364 }
365 void COctoMap::setProbHit(double prob) { m_impl->m_octomap.setProbHit(prob); }
366 void COctoMap::setProbMiss(double prob) { m_impl->m_octomap.setProbMiss(prob); }
367 void COctoMap::setClampingThresMin(double thresProb)
368 {
369  m_impl->m_octomap.setClampingThresMin(thresProb);
370 }
371 void COctoMap::setClampingThresMax(double thresProb)
372 {
373  m_impl->m_octomap.setClampingThresMax(thresProb);
374 }
375 double COctoMap::getOccupancyThres() const
376 {
377  return m_impl->m_octomap.getOccupancyThres();
378 }
379 float COctoMap::getOccupancyThresLog() const
380 {
381  return m_impl->m_octomap.getOccupancyThresLog();
382 }
383 double COctoMap::getProbHit() const { return m_impl->m_octomap.getProbHit(); }
384 float COctoMap::getProbHitLog() const
385 {
386  return m_impl->m_octomap.getProbHitLog();
387 }
388 double COctoMap::getProbMiss() const { return m_impl->m_octomap.getProbMiss(); }
389 float COctoMap::getProbMissLog() const
390 {
391  return m_impl->m_octomap.getProbMissLog();
392 }
393 double COctoMap::getClampingThresMin() const
394 {
395  return m_impl->m_octomap.getClampingThresMin();
396 }
397 float COctoMap::getClampingThresMinLog() const
398 {
399  return m_impl->m_octomap.getClampingThresMinLog();
400 }
401 double COctoMap::getClampingThresMax() const
402 {
403  return m_impl->m_octomap.getClampingThresMax();
404 }
405 float COctoMap::getClampingThresMaxLog() const
406 {
407  return m_impl->m_octomap.getClampingThresMaxLog();
408 }
409 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)
#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.
double resolution
The finest resolution of the octomap (default: 0.10.
Definition: COctoMap.h:53
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.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
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:57
mrpt::maps::COctoMap::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
Definition: COctoMap.h:59
This namespace contains representation of robot actions and observations.
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
A three-dimensional probabilistic occupancy grid, implemented as an octo-tree with the "octomap" C++ ...
Definition: COctoMap.h:40
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...
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
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: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)
mrpt::img::TColorf getColor() const
Returns the object color property as a TColorf.
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
visualization_mode_t getVisualizationMode() const
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
A RGB color - 8bit.
Definition: TColor.h:25
bool isCubeTransparencyEnabled() const
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020