18 #include <octomap/OcTree.h>
19 #include <octomap/octomap.h>
38 COctoMap::TMapDefinition::TMapDefinition() =
default;
39 void COctoMap::TMapDefinition::loadFromConfigFile_map_specific(
41 const std::string& sectionNamePrefix)
44 const std::string sSectCreation =
45 sectionNamePrefix + string(
"_creationOpts");
48 insertionOpts.loadFromConfigFile(
49 source, sectionNamePrefix +
string(
"_insertOpts"));
50 likelihoodOpts.loadFromConfigFile(
51 source, sectionNamePrefix +
string(
"_likelihoodOpts"));
54 void COctoMap::TMapDefinition::dumpToTextStream_map_specific(
55 std::ostream&
out)
const
59 this->insertionOpts.dumpToTextStream(
out);
60 this->likelihoodOpts.dumpToTextStream(
out);
85 COctoMap::~COctoMap() =
default;
86 uint8_t COctoMap::serializeGetVersion()
const {
return 3; }
89 this->likelihoodOptions.writeToStream(
out);
90 this->renderingOptions.writeToStream(
out);
91 out << genericMapParams;
94 const_cast<octomap::OcTree*
>(&m_impl->m_octomap)->writeBinary(ss);
95 const std::string& buf = ss.str();
108 "Deserialization of old versions of this class was "
109 "discontinued in MRPT 1.9.9 [no CMemoryChunk]");
114 this->likelihoodOptions.readFromStream(in);
115 if (version >= 1) this->renderingOptions.readFromStream(in);
116 if (version >= 2) in >> genericMapParams;
125 std::stringstream ss;
128 m_impl->m_octomap.readBinary(ss);
137 bool COctoMap::internal_insertObservation(
140 octomap::point3d sensorPt;
141 octomap::Pointcloud scan;
142 if (!internal_build_PointCloud_for_observation(
143 obs, robotPose, sensorPt, scan))
146 m_impl->m_octomap.insertPointCloud(
147 scan, sensorPt, insertionOptions.maxrange, insertionOptions.pruning);
157 octomap::OcTree::tree_iterator it_end = m_impl->m_octomap.end_tree();
159 const unsigned char max_depth = 0;
161 const TColor general_color_u(
162 general_color.
R * 255, general_color.
G * 255, general_color.
B * 255,
163 general_color.
A * 255);
174 const size_t nLeafs = this->getNumLeafNodes();
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);
183 for (octomap::OcTree::tree_iterator it =
184 m_impl->m_octomap.begin_tree(max_depth);
187 const octomap::point3d vx_center = it.getCoordinate();
188 const double vx_length = it.getSize();
189 const double L = 0.5 * vx_length;
194 const double occ = it->getOccupancy();
195 if ((occ >= 0.5 && renderingOptions.generateOccupiedVoxels) ||
196 (occ < 0.5 && renderingOptions.generateFreeVoxels))
202 case COctoMapVoxels::FIXED:
203 vx_color = general_color_u;
205 case COctoMapVoxels::COLOR_FROM_HEIGHT:
206 coefc = 255 * inv_dz * (vx_center.z() - zmin);
208 coefc * general_color.
R, coefc * general_color.
G,
209 coefc * general_color.
B, 255.0 * general_color.
A);
212 case COctoMapVoxels::COLOR_FROM_OCCUPANCY:
213 coefc = 240 * (1 - occ) + 15;
215 coefc * general_color.
R, coefc * general_color.
G,
216 coefc * general_color.
B, 255.0 * general_color.
A);
219 case COctoMapVoxels::TRANSPARENCY_FROM_OCCUPANCY:
220 coeft = 255 - 510 * (1 - occ);
226 255 * general_color.
R, 255 * general_color.
G,
227 255 * general_color.
B, coeft);
230 case COctoMapVoxels::TRANS_AND_COLOR_FROM_OCCUPANCY:
231 coefc = 240 * (1 - occ) + 15;
233 coefc * general_color.
R, coefc * general_color.
G,
234 coefc * general_color.
B, 50);
237 case COctoMapVoxels::MIXED:
238 coefc = 255 * inv_dz * (vx_center.z() - zmin);
239 coeft = 255 - 510 * (1 - occ);
245 coefc * general_color.
R, coefc * general_color.
G,
246 coefc * general_color.
B, coeft);
253 const size_t vx_set = (m_impl->m_octomap.isNodeOccupied(*it))
260 TPoint3D(vx_center.x(), vx_center.y(), vx_center.z()),
261 vx_length, vx_color));
264 else if (renderingOptions.generateGridLines)
268 vx_center.x() - L, vx_center.y() - L, vx_center.z() - L);
270 vx_center.x() + L, vx_center.y() + L, vx_center.z() + L);
283 this->getMetricMin(bbmin.
x, bbmin.
y, bbmin.
z);
284 this->getMetricMax(bbmax.
x, bbmax.
y, bbmax.
z);
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)
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);
298 void COctoMap::updateVoxel(
299 const double x,
const double y,
const double z,
bool occupied)
301 m_impl->m_octomap.updateNode(x, y, z, occupied);
303 bool COctoMap::isPointWithinOctoMap(
304 const float x,
const float y,
const float z)
const
306 octomap::OcTreeKey key;
307 return m_impl->m_octomap.coordToKeyChecked(octomap::point3d(x, y, z), key);
310 double COctoMap::getResolution()
const
312 return m_impl->m_octomap.getResolution();
314 unsigned int COctoMap::getTreeDepth()
const
316 return m_impl->m_octomap.getTreeDepth();
319 size_t COctoMap::memoryUsage()
const {
return m_impl->m_octomap.memoryUsage(); }
320 size_t COctoMap::memoryUsageNode()
const
322 return m_impl->m_octomap.memoryUsageNode();
324 size_t COctoMap::memoryFullGrid()
const
326 return m_impl->m_octomap.memoryFullGrid();
328 double COctoMap::volume() {
return m_impl->m_octomap.volume(); }
329 void COctoMap::getMetricSize(
double& x,
double& y,
double& z)
331 return m_impl->m_octomap.getMetricSize(x, y, z);
333 void COctoMap::getMetricSize(
double& x,
double& y,
double& z)
const
335 return m_impl->m_octomap.getMetricSize(x, y, z);
337 void COctoMap::getMetricMin(
double& x,
double& y,
double& z)
339 return m_impl->m_octomap.getMetricMin(x, y, z);
341 void COctoMap::getMetricMin(
double& x,
double& y,
double& z)
const
343 return m_impl->m_octomap.getMetricMin(x, y, z);
345 void COctoMap::getMetricMax(
double& x,
double& y,
double& z)
347 return m_impl->m_octomap.getMetricMax(x, y, z);
349 void COctoMap::getMetricMax(
double& x,
double& y,
double& z)
const
351 return m_impl->m_octomap.getMetricMax(x, y, z);
353 size_t COctoMap::calcNumNodes()
const
355 return m_impl->m_octomap.calcNumNodes();
357 size_t COctoMap::getNumLeafNodes()
const
359 return m_impl->m_octomap.getNumLeafNodes();
361 void COctoMap::setOccupancyThres(
double prob)
363 m_impl->m_octomap.setOccupancyThres(prob);
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)
369 m_impl->m_octomap.setClampingThresMin(thresProb);
371 void COctoMap::setClampingThresMax(
double thresProb)
373 m_impl->m_octomap.setClampingThresMax(thresProb);
375 double COctoMap::getOccupancyThres()
const
377 return m_impl->m_octomap.getOccupancyThres();
379 float COctoMap::getOccupancyThresLog()
const
381 return m_impl->m_octomap.getOccupancyThresLog();
383 double COctoMap::getProbHit()
const {
return m_impl->m_octomap.getProbHit(); }
384 float COctoMap::getProbHitLog()
const
386 return m_impl->m_octomap.getProbHitLog();
388 double COctoMap::getProbMiss()
const {
return m_impl->m_octomap.getProbMiss(); }
389 float COctoMap::getProbMissLog()
const
391 return m_impl->m_octomap.getProbMissLog();
393 double COctoMap::getClampingThresMin()
const
395 return m_impl->m_octomap.getClampingThresMin();
397 float COctoMap::getClampingThresMinLog()
const
399 return m_impl->m_octomap.getClampingThresMinLog();
401 double COctoMap::getClampingThresMax()
const
403 return m_impl->m_octomap.getClampingThresMax();
405 float COctoMap::getClampingThresMaxLog()
const
407 return m_impl->m_octomap.getClampingThresMaxLog();
409 void COctoMap::internal_clear() { m_impl->m_octomap.clear(); }