12 #include <octomap/ColorOcTree.h>
13 #include <octomap/octomap.h>
32 octomap::ColorOcTree, octomap::ColorOcTreeNode>;
45 "mrpt::maps::CColouredOctoMap,colourOctoMap,colorOctoMap",
48 CColouredOctoMap::TMapDefinition::TMapDefinition() =
default;
49 void CColouredOctoMap::TMapDefinition::loadFromConfigFile_map_specific(
51 const std::string& sectionNamePrefix)
54 const std::string sSectCreation =
55 sectionNamePrefix + string(
"_creationOpts");
58 insertionOpts.loadFromConfigFile(
59 source, sectionNamePrefix +
string(
"_insertOpts"));
60 likelihoodOpts.loadFromConfigFile(
61 source, sectionNamePrefix +
string(
"_likelihoodOpts"));
64 void CColouredOctoMap::TMapDefinition::dumpToTextStream_map_specific(
65 std::ostream&
out)
const
69 this->insertionOpts.dumpToTextStream(
out);
70 this->likelihoodOpts.dumpToTextStream(
out);
93 CColouredOctoMap::~CColouredOctoMap() =
default;
94 uint8_t CColouredOctoMap::serializeGetVersion()
const {
return 3; }
97 this->likelihoodOptions.writeToStream(
out);
98 this->renderingOptions.writeToStream(
out);
99 out << genericMapParams;
102 std::stringstream ss;
103 m_impl->m_octomap.writeBinaryConst(ss);
104 const std::string& buf = ss.str();
108 void CColouredOctoMap::serializeFrom(
118 "Deserialization of old versions of this class was "
119 "discontinued in MRPT 1.9.9 [no CMemoryChunk]");
124 this->likelihoodOptions.readFromStream(in);
125 if (version >= 1) this->renderingOptions.readFromStream(in);
126 if (version >= 2) in >> genericMapParams;
135 std::stringstream ss;
138 m_impl->m_octomap.readBinary(ss);
150 bool CColouredOctoMap::internal_insertObservation(
153 octomap::point3d sensorPt;
154 octomap::Pointcloud scan;
158 robotPose3D = (*robotPose);
174 octomap::point3d(sensorPose.
x(), sensorPose.
y(), sensorPose.z());
177 const size_t nPts = scanPts->
size();
183 for (
size_t i = 0; i < nPts; i++)
186 scanPts->getPointFast(i, pt.
x, pt.
y, pt.
z);
193 scan.push_back(gx, gy, gz);
197 m_impl->m_octomap.insertPointCloud(
198 scan, sensorPt, insertionOptions.maxrange,
199 insertionOptions.pruning);
224 octomap::point3d(sensorPose.
x(), sensorPose.
y(), sensorPose.z());
226 const size_t sizeRangeScan = pts->size();
227 scan.reserve(sizeRangeScan);
229 for (
size_t i = 0; i < sizeRangeScan; i++)
234 if (pt.
x != 0 || pt.
y != 0 || pt.
z != 0)
235 scan.push_back(pt.
x, pt.
y, pt.
z);
239 octomap::KeySet free_cells, occupied_cells;
240 m_impl->m_octomap.computeUpdate(
241 scan, sensorPt, free_cells, occupied_cells,
242 insertionOptions.maxrange);
245 for (
const auto& free_cell : free_cells)
247 m_impl->m_octomap.updateNode(free_cell,
false,
false);
249 for (
const auto& occupied_cell : occupied_cells)
251 m_impl->m_octomap.updateNode(occupied_cell,
true,
false);
255 for (
size_t i = 0; i < sizeRangeScan; i++)
257 const auto& pt = pts->getPoint3Df(i);
258 const auto pt_col = pts->getPointColor(i);
261 if (pt.x != 0 || pt.y != 0 || pt.z != 0)
262 this->updateVoxelColour(
263 pt.x, pt.y, pt.z, pt_col.R, pt_col.G, pt_col.B);
267 if (insertionOptions.pruning) m_impl->m_octomap.prune();
278 bool CColouredOctoMap::getPointColour(
279 const float x,
const float y,
const float z, uint8_t& r, uint8_t& g,
282 octomap::OcTreeKey key;
283 if (m_impl->m_octomap.coordToKeyChecked(octomap::point3d(x, y, z), key))
285 octomap::ColorOcTreeNode* node =
286 m_impl->m_octomap.search(key, 0 );
287 if (!node)
return false;
289 const octomap::ColorOcTreeNode::Color& col = node->getColor();
300 void CColouredOctoMap::updateVoxelColour(
301 const double x,
const double y,
const double z,
const uint8_t r,
302 const uint8_t g,
const uint8_t b)
304 switch (m_colour_method)
307 m_impl->m_octomap.integrateNodeColor(x, y, z, r, g, b);
310 m_impl->m_octomap.setNodeColor(x, y, z, r, g, b);
313 m_impl->m_octomap.averageNodeColor(x, y, z, r, g, b);
322 void CColouredOctoMap::getAsOctoMapVoxels(
328 octomap::ColorOcTree::tree_iterator it_end = m_impl->m_octomap.end_tree();
330 const unsigned char max_depth = 0;
332 const TColor general_color_u(
333 general_color.
R * 255, general_color.
G * 255, general_color.
B * 255,
334 general_color.
A * 255);
345 const size_t nLeafs = this->getNumLeafNodes();
349 double xmin, xmax, ymin, ymax, zmin, zmax;
350 this->getMetricMin(xmin, ymin, zmin);
351 this->getMetricMax(xmax, ymax, zmax);
353 for (octomap::ColorOcTree::tree_iterator it =
354 m_impl->m_octomap.begin_tree(max_depth);
357 const octomap::point3d vx_center = it.getCoordinate();
358 const double vx_length = it.getSize();
359 const double L = 0.5 * vx_length;
364 const double occ = it->getOccupancy();
365 if ((occ >= 0.5 && renderingOptions.generateOccupiedVoxels) ||
366 (occ < 0.5 && renderingOptions.generateFreeVoxels))
369 octomap::ColorOcTreeNode::Color node_color = it->getColor();
370 vx_color =
TColor(node_color.r, node_color.g, node_color.b);
372 const size_t vx_set = (m_impl->m_octomap.isNodeOccupied(*it))
379 TPoint3D(vx_center.x(), vx_center.y(), vx_center.z()),
380 vx_length, vx_color));
383 else if (renderingOptions.generateGridLines)
387 vx_center.x() - L, vx_center.y() - L, vx_center.z() - L);
389 vx_center.x() + L, vx_center.y() + L, vx_center.z() + L);
402 this->getMetricMin(bbmin.
x, bbmin.
y, bbmin.
z);
403 this->getMetricMax(bbmax.
x, bbmax.
y, bbmax.
z);
408 void CColouredOctoMap::insertRay(
409 const float end_x,
const float end_y,
const float end_z,
410 const float sensor_x,
const float sensor_y,
const float sensor_z)
412 m_impl->m_octomap.insertRay(
413 octomap::point3d(sensor_x, sensor_y, sensor_z),
414 octomap::point3d(end_x, end_y, end_z), insertionOptions.maxrange,
415 insertionOptions.pruning);
417 void CColouredOctoMap::updateVoxel(
418 const double x,
const double y,
const double z,
bool occupied)
420 m_impl->m_octomap.updateNode(x, y, z, occupied);
422 bool CColouredOctoMap::isPointWithinOctoMap(
423 const float x,
const float y,
const float z)
const
425 octomap::OcTreeKey key;
426 return m_impl->m_octomap.coordToKeyChecked(octomap::point3d(x, y, z), key);
429 double CColouredOctoMap::getResolution()
const
431 return m_impl->m_octomap.getResolution();
433 unsigned int CColouredOctoMap::getTreeDepth()
const
435 return m_impl->m_octomap.getTreeDepth();
438 size_t CColouredOctoMap::memoryUsage()
const
440 return m_impl->m_octomap.memoryUsage();
442 size_t CColouredOctoMap::memoryUsageNode()
const
444 return m_impl->m_octomap.memoryUsageNode();
446 size_t CColouredOctoMap::memoryFullGrid()
const
448 return m_impl->m_octomap.memoryFullGrid();
450 double CColouredOctoMap::volume() {
return m_impl->m_octomap.volume(); }
451 void CColouredOctoMap::getMetricSize(
double& x,
double& y,
double& z)
453 return m_impl->m_octomap.getMetricSize(x, y, z);
455 void CColouredOctoMap::getMetricSize(
double& x,
double& y,
double& z)
const
457 return m_impl->m_octomap.getMetricSize(x, y, z);
459 void CColouredOctoMap::getMetricMin(
double& x,
double& y,
double& z)
461 return m_impl->m_octomap.getMetricMin(x, y, z);
463 void CColouredOctoMap::getMetricMin(
double& x,
double& y,
double& z)
const
465 return m_impl->m_octomap.getMetricMin(x, y, z);
467 void CColouredOctoMap::getMetricMax(
double& x,
double& y,
double& z)
469 return m_impl->m_octomap.getMetricMax(x, y, z);
471 void CColouredOctoMap::getMetricMax(
double& x,
double& y,
double& z)
const
473 return m_impl->m_octomap.getMetricMax(x, y, z);
475 size_t CColouredOctoMap::calcNumNodes()
const
477 return m_impl->m_octomap.calcNumNodes();
479 size_t CColouredOctoMap::getNumLeafNodes()
const
481 return m_impl->m_octomap.getNumLeafNodes();
483 void CColouredOctoMap::setOccupancyThres(
double prob)
485 m_impl->m_octomap.setOccupancyThres(prob);
487 void CColouredOctoMap::setProbHit(
double prob)
489 m_impl->m_octomap.setProbHit(prob);
491 void CColouredOctoMap::setProbMiss(
double prob)
493 m_impl->m_octomap.setProbMiss(prob);
495 void CColouredOctoMap::setClampingThresMin(
double thresProb)
497 m_impl->m_octomap.setClampingThresMin(thresProb);
499 void CColouredOctoMap::setClampingThresMax(
double thresProb)
501 m_impl->m_octomap.setClampingThresMax(thresProb);
503 double CColouredOctoMap::getOccupancyThres()
const
505 return m_impl->m_octomap.getOccupancyThres();
507 float CColouredOctoMap::getOccupancyThresLog()
const
509 return m_impl->m_octomap.getOccupancyThresLog();
511 double CColouredOctoMap::getProbHit()
const
513 return m_impl->m_octomap.getProbHit();
515 float CColouredOctoMap::getProbHitLog()
const
517 return m_impl->m_octomap.getProbHitLog();
519 double CColouredOctoMap::getProbMiss()
const
521 return m_impl->m_octomap.getProbMiss();
523 float CColouredOctoMap::getProbMissLog()
const
525 return m_impl->m_octomap.getProbMissLog();
527 double CColouredOctoMap::getClampingThresMin()
const
529 return m_impl->m_octomap.getClampingThresMin();
531 float CColouredOctoMap::getClampingThresMinLog()
const
533 return m_impl->m_octomap.getClampingThresMinLog();
535 double CColouredOctoMap::getClampingThresMax()
const
537 return m_impl->m_octomap.getClampingThresMax();
539 float CColouredOctoMap::getClampingThresMaxLog()
const
541 return m_impl->m_octomap.getClampingThresMaxLog();
543 void CColouredOctoMap::internal_clear() { m_impl->m_octomap.clear(); }