OctoMap.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <rtabmap/core/OctoMap.h>
29 #include <rtabmap/utilite/ULogger.h>
30 #include <rtabmap/utilite/UStl.h>
31 #include <rtabmap/utilite/UTimer.h>
32 #include <rtabmap/core/util3d_transforms.h>
33 #include <rtabmap/core/util3d_filtering.h>
34 #include <rtabmap/core/util3d_mapping.h>
35 #include <rtabmap/core/util2d.h>
36 #include <pcl/common/transforms.h>
37 
38 namespace rtabmap {
39 
41 // RtabmapColorOcTree
43 
44 RtabmapColorOcTreeNode* RtabmapColorOcTreeNode::getChild(unsigned int i) {
45 #ifdef OCTOMAP_PRE_18
46  return static_cast<RtabmapColorOcTreeNode*> (OcTreeNode::getChild(i));
47 #else
48  UFATAL("This function should not be used with octomap >= 1.8");
49  return 0;
50 #endif
51 }
52 const RtabmapColorOcTreeNode* RtabmapColorOcTreeNode::getChild(unsigned int i) const {
53 #ifdef OCTOMAP_PRE_18
54  return static_cast<const RtabmapColorOcTreeNode*> (OcTreeNode::getChild(i));
55 #else
56  UFATAL("This function should not be used with octomap >= 1.8");
57  return 0;
58 #endif
59 }
60 
61 //octomap <1.8
62 bool RtabmapColorOcTreeNode::pruneNode() {
63 #ifdef OCTOMAP_PRE_18
64  // checks for equal occupancy only, color ignored
65  if (!this->collapsible()) return false;
66  // set occupancy value
67  setLogOdds(getChild(0)->getLogOdds());
68  // set color to average color
69  if (isColorSet()) color = getAverageChildColor();
70  // delete children
71  for (unsigned int i=0;i<8;i++) {
72  delete children[i];
73  }
74  delete[] children;
75  children = NULL;
76  return true;
77 #else
78  UFATAL("This function should not be used with octomap >= 1.8");
79  return false;
80 #endif
81 }
82 //octomap <1.8
83 void RtabmapColorOcTreeNode::expandNode() {
84 #ifdef OCTOMAP_PRE_18
85  assert(!hasChildren());
86  for (unsigned int k=0; k<8; k++) {
87  createChild(k);
88  children[k]->setValue(value);
89  getChild(k)->setColor(color);
90  }
91 #else
92  UFATAL("This function should not be used with octomap >= 1.8");
93 #endif
94 }
95 
96 //octomap <1.8
97 bool RtabmapColorOcTreeNode::createChild(unsigned int i) {
98 #ifdef OCTOMAP_PRE_18
99  if (children == NULL) allocChildren();
100  children[i] = new RtabmapColorOcTreeNode();
101  return true;
102 #else
103  UFATAL("This function should not be used with octomap >= 1.8");
104  return false;
105 #endif
106 }
107 
108 RtabmapColorOcTree::RtabmapColorOcTree(double resolution)
109  : OccupancyOcTreeBase<RtabmapColorOcTreeNode>(resolution) {
110  RtabmapColorOcTreeMemberInit.ensureLinking();
111 };
112 
113 RtabmapColorOcTreeNode* RtabmapColorOcTree::setNodeColor(const octomap::OcTreeKey& key,
114  uint8_t r,
115  uint8_t g,
116  uint8_t b) {
117  RtabmapColorOcTreeNode* n = search (key);
118  if (n != 0) {
119  n->setColor(r, g, b);
120  }
121  return n;
122 }
123 
124 bool RtabmapColorOcTree::pruneNode(RtabmapColorOcTreeNode* node) {
125 #ifndef OCTOMAP_PRE_18
126  if (!isNodeCollapsible(node))
127  return false;
128 
129  // set value to children's values (all assumed equal)
130  node->copyData(*(getNodeChild(node, 0)));
131 
132  if (node->isColorSet()) // TODO check
133  node->setColor(node->getAverageChildColor());
134 
135  // delete children
136  for (unsigned int i=0;i<8;i++) {
137  deleteNodeChild(node, i);
138  }
139  delete[] node->children;
140  node->children = NULL;
141 
142  return true;
143 #else
144  UFATAL("This function should not be used with octomap < 1.8");
145  return false;
146 #endif
147 }
148 
149 bool RtabmapColorOcTree::isNodeCollapsible(const RtabmapColorOcTreeNode* node) const{
150 #ifndef OCTOMAP_PRE_18
151  // all children must exist, must not have children of
152  // their own and have the same occupancy probability
153  if (!nodeChildExists(node, 0))
154  return false;
155 
156  const RtabmapColorOcTreeNode* firstChild = getNodeChild(node, 0);
157  if (nodeHasChildren(firstChild))
158  return false;
159 
160  for (unsigned int i = 1; i<8; i++) {
161  // compare nodes only using their occupancy, ignoring color for pruning
162  if (!nodeChildExists(node, i) || nodeHasChildren(getNodeChild(node, i)) || !(getNodeChild(node, i)->getValue() == firstChild->getValue()))
163  return false;
164  }
165 
166  return true;
167 #else
168  UFATAL("This function should not be used with octomap < 1.8");
169  return false;
170 #endif
171 }
172 
173 RtabmapColorOcTreeNode* RtabmapColorOcTree::averageNodeColor(const octomap::OcTreeKey& key,
174  uint8_t r,
175  uint8_t g,
176  uint8_t b) {
177  RtabmapColorOcTreeNode* n = search(key);
178  if (n != 0) {
179  if (n->isColorSet()) {
180  RtabmapColorOcTreeNode::Color prev_color = n->getColor();
181  n->setColor((prev_color.r + r)/2, (prev_color.g + g)/2, (prev_color.b + b)/2);
182  }
183  else {
184  n->setColor(r, g, b);
185  }
186  }
187  return n;
188 }
189 
190 RtabmapColorOcTreeNode* RtabmapColorOcTree::integrateNodeColor(const octomap::OcTreeKey& key,
191  uint8_t r,
192  uint8_t g,
193  uint8_t b) {
194  RtabmapColorOcTreeNode* n = search (key);
195  if (n != 0) {
196  if (n->isColorSet()) {
197  RtabmapColorOcTreeNode::Color prev_color = n->getColor();
198  double node_prob = n->getOccupancy();
199  uint8_t new_r = (uint8_t) ((double) prev_color.r * node_prob
200  + (double) r * (0.99-node_prob));
201  uint8_t new_g = (uint8_t) ((double) prev_color.g * node_prob
202  + (double) g * (0.99-node_prob));
203  uint8_t new_b = (uint8_t) ((double) prev_color.b * node_prob
204  + (double) b * (0.99-node_prob));
205  n->setColor(new_r, new_g, new_b);
206  }
207  else {
208  n->setColor(r, g, b);
209  }
210  }
211  return n;
212 }
213 
214 
215 void RtabmapColorOcTree::updateInnerOccupancy() {
216  this->updateInnerOccupancyRecurs(this->root, 0);
217 }
218 
219 void RtabmapColorOcTree::updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth) {
220 #ifndef OCTOMAP_PRE_18
221  // only recurse and update for inner nodes:
222  if (nodeHasChildren(node)){
223  // return early for last level:
224  if (depth < this->tree_depth){
225  for (unsigned int i=0; i<8; i++) {
226  if (nodeChildExists(node, i)) {
227  updateInnerOccupancyRecurs(getNodeChild(node, i), depth+1);
228  }
229  }
230  }
231  node->updateOccupancyChildren();
232  node->updateColorChildren();
233  }
234 #else
235  // only recurse and update for inner nodes:
236  if (node->hasChildren()){
237  // return early for last level:
238  if (depth < this->tree_depth){
239  for (unsigned int i=0; i<8; i++) {
240  if (node->childExists(i)) {
241  updateInnerOccupancyRecurs(node->getChild(i), depth+1);
242  }
243  }
244  }
245  node->updateOccupancyChildren();
246  node->updateColorChildren();
247  }
248 #endif
249 }
250 
251 RtabmapColorOcTree::StaticMemberInitializer::StaticMemberInitializer() {
252  RtabmapColorOcTree* tree = new RtabmapColorOcTree(0.1);
253 
254 #ifndef OCTOMAP_PRE_18
255  tree->clearKeyRays();
256 #endif
257 
258  AbstractOcTree::registerTreeType(tree);
259  }
260 
261 #ifndef _WIN32
262 // On Windows, the app freezes on start if the following is defined
263 RtabmapColorOcTree::StaticMemberInitializer RtabmapColorOcTree::RtabmapColorOcTreeMemberInit;
264 #endif
265 
266 
268 // OctoMap
270 
271 OctoMap::OctoMap(const ParametersMap & parameters) :
272  hasColor_(false),
273  fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
274  updateError_(Parameters::defaultGridGlobalUpdateError()),
275  rangeMax_(Parameters::defaultGridRangeMax()),
276  rayTracing_(Parameters::defaultGridRayTracing())
277 {
278  float cellSize = Parameters::defaultGridCellSize();
279  Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
280  UASSERT(cellSize>0.0f);
281 
282  minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
283  maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
284 
285  float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
286  float probHit = Parameters::defaultGridGlobalProbHit();
287  float probMiss = Parameters::defaultGridGlobalProbMiss();
288  float clampingMin = Parameters::defaultGridGlobalProbClampingMin();
289  float clampingMax = Parameters::defaultGridGlobalProbClampingMax();
290  Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr);
291  Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit);
292  Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss);
293  Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), clampingMin);
294  Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), clampingMax);
295 
296  octree_ = new RtabmapColorOcTree(cellSize);
297  if(occupancyThr <= 0.0f)
298  {
299  UWARN("Cannot set %s to null for OctoMap, using default value %f instead.",
300  Parameters::kGridGlobalOccupancyThr().c_str(),
301  Parameters::defaultGridGlobalOccupancyThr());
302  occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
303  }
304  octree_->setOccupancyThres(occupancyThr);
305  octree_->setProbHit(probHit);
306  octree_->setProbMiss(probMiss);
307  octree_->setClampingThresMin(clampingMin);
308  octree_->setClampingThresMax(clampingMax);
309  Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
310  Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
311  Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);
312  Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
313 }
314 
315 OctoMap::OctoMap(float cellSize, float occupancyThr, bool fullUpdate, float updateError) :
316  octree_(new RtabmapColorOcTree(cellSize)),
317  hasColor_(false),
318  fullUpdate_(fullUpdate),
319  updateError_(updateError),
320  rangeMax_(0.0f),
321  rayTracing_(true)
322 {
323  minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
324  maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
325 
326  octree_->setOccupancyThres(occupancyThr);
327  UASSERT(cellSize>0.0f);
328 }
329 
330 OctoMap::~OctoMap()
331 {
332  this->clear();
333  delete octree_;
334 }
335 
336 void OctoMap::clear()
337 {
338  octree_->clear();
339  cache_.clear();
340  cacheClouds_.clear();
341  cacheViewPoints_.clear();
342  addedNodes_.clear();
343  hasColor_ = false;
344  minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
345  maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
346 }
347 
348 void OctoMap::addToCache(int nodeId,
349  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
350  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
351  const pcl::PointXYZ & viewPoint)
352 {
353  UDEBUG("nodeId=%d", nodeId);
354  if(nodeId < 0)
355  {
356  UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
357  return;
358  }
359  cacheClouds_.erase(nodeId==0?-1:nodeId);
360  cacheClouds_.insert(std::make_pair(nodeId==0?-1:nodeId, std::make_pair(ground, obstacles)));
361  uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
362 }
363 void OctoMap::addToCache(int nodeId,
364  const cv::Mat & ground,
365  const cv::Mat & obstacles,
366  const cv::Mat & empty,
367  const cv::Point3f & viewPoint)
368 {
369  UDEBUG("nodeId=%d", nodeId);
370  if(nodeId < 0)
371  {
372  UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
373  return;
374  }
375  UASSERT_MSG(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", ground.type()).c_str());
376  UASSERT_MSG(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", obstacles.type()).c_str());
377  UASSERT_MSG(empty.empty() || empty.type() == CV_32FC3 || empty.type() == CV_32FC(4) || empty.type() == CV_32FC(6), uFormat("Are local occupancy grids not 3d? (opencv type=%d)", empty.type()).c_str());
378  uInsert(cache_, std::make_pair(nodeId==0?-1:nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
379  uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, viewPoint));
380 }
381 
382 bool OctoMap::update(const std::map<int, Transform> & poses)
383 {
384  UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
385 
386  // First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
387  bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
388  bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
389  std::map<int, Transform> transforms;
390  std::map<int, Transform> updatedAddedNodes;
391  float updateErrorSqrd = updateError_*updateError_;
392  for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
393  {
394  std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
395  if(jter != poses.end())
396  {
397  graphChanged = false;
398  UASSERT(!iter->second.isNull() && !jter->second.isNull());
399  Transform t = Transform::getIdentity();
400  if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
401  {
402  t = jter->second * iter->second.inverse();
403  graphOptimized = true;
404  }
405  transforms.insert(std::make_pair(jter->first, t));
406  updatedAddedNodes.insert(std::make_pair(jter->first, jter->second));
407  }
408  else
409  {
410  UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first);
411  }
412  }
413 
414  if(graphOptimized || graphChanged)
415  {
416  if(graphChanged)
417  {
418  UWARN("Graph has changed! The whole map should be rebuilt.");
419  }
420  else
421  {
422  UINFO("Graph optimized!");
423  }
424 
425  minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
426  maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
427 
428  if(fullUpdate_ || graphChanged)
429  {
430  // clear all but keep cache
431  octree_->clear();
432  addedNodes_.clear();
433  hasColor_ = false;
434  }
435  else
436  {
437  RtabmapColorOcTree * newOcTree = new RtabmapColorOcTree(octree_->getResolution());
438  int copied=0;
439  int count=0;
440  UTimer t;
441  for (RtabmapColorOcTree::iterator it = octree_->begin(); it != octree_->end(); ++it, ++count)
442  {
443  RtabmapColorOcTreeNode & nOld = *it;
444  if(nOld.getNodeRefId() > 0)
445  {
446  std::map<int, Transform>::iterator jter = transforms.find(nOld.getNodeRefId());
447  if(jter != transforms.end())
448  {
449  octomap::point3d pt;
450  std::map<int, Transform>::iterator pter = addedNodes_.find(nOld.getNodeRefId());
451  UASSERT(pter != addedNodes_.end());
452 
453  if(nOld.getOccupancyType() > 0)
454  {
455  pt = nOld.getPointRef();
456  }
457  else
458  {
459  pt = octree_->keyToCoord(it.getKey());
460  }
461 
462  cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
463  cvPt = util3d::transformPoint(cvPt, jter->second);
464  octomap::point3d ptTransformed(cvPt.x, cvPt.y, cvPt.z);
465 
466  octomap::OcTreeKey key;
467  if(newOcTree->coordToKeyChecked(ptTransformed, key))
468  {
469  RtabmapColorOcTreeNode * n = newOcTree->search(key);
470  if(n)
471  {
472  if(n->getNodeRefId() > nOld.getNodeRefId())
473  {
474  // The cell has been updated from more recent node, don't update the cell
475  continue;
476  }
477  else if(nOld.getOccupancyType() <= 0 && n->getOccupancyType() > 0)
478  {
479  // empty cells cannot overwrite ground/obstacle cells
480  continue;
481  }
482  }
483 
484  RtabmapColorOcTreeNode * nNew = newOcTree->updateNode(key, nOld.getLogOdds());
485  if(nNew)
486  {
487  ++copied;
488  updateMinMax(ptTransformed);
489  nNew->setNodeRefId(nOld.getNodeRefId());
490  if(nOld.getOccupancyType() > 0)
491  {
492  nNew->setPointRef(pt);
493  }
494  nNew->setOccupancyType(nOld.getOccupancyType());
495  nNew->setColor(nOld.getColor());
496  }
497  else
498  {
499  UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
500  }
501  }
502  else
503  {
504  UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
505  }
506  }
507  else if(jter == transforms.end())
508  {
509  // Note: normal if old nodes were transfered to LTM
510  //UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
511  }
512  }
513  }
514  UINFO("Graph optimization detected, moved %d/%d in %fs", copied, count, t.ticks());
515  delete octree_;
516  octree_ = newOcTree;
517 
518  //update added poses
519  addedNodes_ = updatedAddedNodes;
520  }
521  }
522 
523  // Original version from A. Hornung:
524  // https://github.com/OctoMap/octomap_mapping/blob/jade-devel/octomap_server/src/OctomapServer.cpp#L356
525  //
526  std::list<std::pair<int, Transform> > orderedPoses;
527 
528  int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
529  UDEBUG("Last id = %d", lastId);
530 
531  // add old poses that were not in the current map (they were just retrieved from LTM)
532  for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
533  {
534  if(addedNodes_.find(iter->first) == addedNodes_.end())
535  {
536  orderedPoses.push_back(*iter);
537  }
538  }
539 
540  // insert zero after
541  if(poses.find(0) != poses.end())
542  {
543  orderedPoses.push_back(std::make_pair(-1, poses.at(0)));
544  }
545 
546  UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
547 
548  if(!orderedPoses.empty())
549  {
550  float rangeMaxSqrd = rangeMax_*rangeMax_;
551  float cellSize = octree_->getResolution();
552  for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
553  {
554  std::map<int, std::pair<const pcl::PointCloud<pcl::PointXYZRGB>::Ptr, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
555  std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator occupancyIter;
556  std::map<int, cv::Point3f>::iterator viewPointIter;
557  cloudIter = cacheClouds_.find(iter->first);
558  occupancyIter = cache_.find(iter->first);
559  viewPointIter = cacheViewPoints_.find(iter->first);
560  if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
561  {
562  UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
563 
564  UASSERT(viewPointIter != cacheViewPoints_.end());
565  octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
566  sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
567 
568  updateMinMax(sensorOrigin);
569 
570  octomap::OcTreeKey tmpKey;
571  if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
572  || !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
573  {
574  UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
575  }
576 
577  bool computeRays = rayTracing_ && (occupancyIter == cache_.end() || occupancyIter->second.second.empty());
578 
579  // instead of direct scan insertion, compute update to filter ground:
580  octomap::KeySet free_cells;
581  // insert ground points only as free:
582  unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.first.cols:cloudIter->second.first->size();
583  UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
584  Eigen::Affine3f t = iter->second.toEigen3f();
585  LaserScan tmpGround;
586  if(occupancyIter != cache_.end())
587  {
588  tmpGround = LaserScan::backwardCompatibility(occupancyIter->second.first.first);
589  UASSERT(tmpGround.size() == (int)maxGroundPts);
590  }
591  for (unsigned int i=0; i<maxGroundPts; ++i)
592  {
593  pcl::PointXYZRGB pt;
594  if(occupancyIter != cache_.end())
595  {
596  pt = util3d::laserScanToPointRGB(tmpGround, i);
597  pt = pcl::transformPoint(pt, t);
598  }
599  else
600  {
601  pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
602  }
603  octomap::point3d point(pt.x, pt.y, pt.z);
604  bool ignoreOccupiedCell = false;
605  if(rangeMaxSqrd > 0.0f)
606  {
607  octomap::point3d v(pt.x - cellSize - sensorOrigin.x(), pt.y - cellSize - sensorOrigin.y(), pt.z - cellSize - sensorOrigin.z());
608  if(v.norm_sq() > rangeMaxSqrd)
609  {
610  // compute new point to max range
611  v.normalize();
612  v*=rangeMax_;
613  point = sensorOrigin + v;
614  ignoreOccupiedCell=true;
615  }
616  }
617 
618  if(!ignoreOccupiedCell)
619  {
620  // occupied endpoint
621  octomap::OcTreeKey key;
622  if (octree_->coordToKeyChecked(point, key))
623  {
624  if(iter->first >0 && iter->first<lastId)
625  {
626  RtabmapColorOcTreeNode * n = octree_->search(key);
627  if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
628  {
629  // The cell has been updated from more recent node, don't update the cell
630  continue;
631  }
632  }
633 
634  updateMinMax(point);
635  RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
636 
637  if(n)
638  {
639  if(!hasColor_ && !(pt.r ==0 && pt.g == 0 && pt.b == 0) && !(pt.r ==255 && pt.g == 255 && pt.b == 255))
640  {
641  hasColor_ = true;
642  }
643  octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
644  if(iter->first > 0)
645  {
646  n->setNodeRefId(iter->first);
647  n->setPointRef(point);
648  }
649  n->setOccupancyType(RtabmapColorOcTreeNode::kTypeGround);
650  }
651  }
652  }
653 
654  // only clear space (ground points)
655  octomap::KeyRay keyRay;
656  if (computeRays &&
657  (iter->first < 0 || iter->first>lastId) &&
658  octree_->computeRayKeys(sensorOrigin, point, keyRay))
659  {
660  free_cells.insert(keyRay.begin(), keyRay.end());
661  }
662  }
663  UDEBUG("%d: ground cells=%d free cells=%d", iter->first, (int)maxGroundPts, (int)free_cells.size());
664 
665  // all other points: free on ray, occupied on endpoint:
666  unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.first.second.cols:cloudIter->second.second->size();
667  UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
668  LaserScan tmpObstacle;
669  if(occupancyIter != cache_.end())
670  {
671  tmpObstacle = LaserScan::backwardCompatibility(occupancyIter->second.first.second);
672  UASSERT(tmpObstacle.size() == (int)maxObstaclePts);
673  }
674  for (unsigned int i=0; i<maxObstaclePts; ++i)
675  {
676  pcl::PointXYZRGB pt;
677  if(occupancyIter != cache_.end())
678  {
679  pt = util3d::laserScanToPointRGB(tmpObstacle, i);
680  pt = pcl::transformPoint(pt, t);
681  }
682  else
683  {
684  pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
685  }
686 
687  octomap::point3d point(pt.x, pt.y, pt.z);
688 
689  bool ignoreOccupiedCell = false;
690  if(rangeMaxSqrd > 0.0f)
691  {
692  octomap::point3d v(pt.x - cellSize - sensorOrigin.x(), pt.y - cellSize - sensorOrigin.y(), pt.z - cellSize - sensorOrigin.z());
693  if(v.norm_sq() > rangeMaxSqrd)
694  {
695  // compute new point to max range
696  v.normalize();
697  v*=rangeMax_;
698  point = sensorOrigin + v;
699  ignoreOccupiedCell=true;
700  }
701  }
702 
703  if(!ignoreOccupiedCell)
704  {
705  // occupied endpoint
706  octomap::OcTreeKey key;
707  if (octree_->coordToKeyChecked(point, key))
708  {
709  if(iter->first >0 && iter->first<lastId)
710  {
711  RtabmapColorOcTreeNode * n = octree_->search(key);
712  if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
713  {
714  // The cell has been updated from more recent node, don't update the cell
715  continue;
716  }
717  }
718 
719  updateMinMax(point);
720 
721  RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
722  if(n)
723  {
724  if(!hasColor_ && !(pt.r ==0 && pt.g == 0 && pt.b == 0) && !(pt.r ==255 && pt.g == 255 && pt.b == 255))
725  {
726  hasColor_ = true;
727  }
728  octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
729  if(iter->first > 0)
730  {
731  n->setNodeRefId(iter->first);
732  n->setPointRef(point);
733  }
734  n->setOccupancyType(RtabmapColorOcTreeNode::kTypeObstacle);
735  }
736  }
737  }
738 
739  // free cells
740  octomap::KeyRay keyRay;
741  if (computeRays &&
742  (iter->first < 0 || iter->first>lastId) &&
743  octree_->computeRayKeys(sensorOrigin, point, keyRay))
744  {
745  free_cells.insert(keyRay.begin(), keyRay.end());
746  }
747  }
748  UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)maxObstaclePts, (int)free_cells.size());
749 
750 
751  // mark free cells only if not seen occupied in this cloud
752  for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
753  {
754  if(iter->first > 0)
755  {
756  RtabmapColorOcTreeNode * n = octree_->search(*it);
757  if(n && n->getNodeRefId() > 0 && n->getNodeRefId() >= iter->first)
758  {
759  // The cell has been updated from current node or more recent node, don't update the cell
760  continue;
761  }
762  }
763 
764  RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false, orderedPoses.size() == 1);
765  if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
766  {
767  n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
768  if(iter->first > 0)
769  {
770  n->setNodeRefId(iter->first);
771  }
772  }
773  }
774 
775  // all empty cells
776  if(occupancyIter != cache_.end() && occupancyIter->second.second.cols)
777  {
778  unsigned int maxEmptyPts = occupancyIter->second.second.cols;
779  UDEBUG("%d: compute free cells (from %d empty points)", iter->first, (int)maxEmptyPts);
780  LaserScan tmpEmpty = LaserScan::backwardCompatibility(occupancyIter->second.second);
781  UASSERT(tmpEmpty.size() == (int)maxEmptyPts);
782  for (unsigned int i=0; i<maxEmptyPts; ++i)
783  {
784  pcl::PointXYZ pt;
785  pt = util3d::laserScanToPoint(tmpEmpty, i);
786  pt = pcl::transformPoint(pt, t);
787 
788  octomap::point3d point(pt.x, pt.y, pt.z);
789 
790  bool ignoreCell = false;
791  if(rangeMaxSqrd > 0.0f)
792  {
793  octomap::point3d v(pt.x - sensorOrigin.x(), pt.y - sensorOrigin.y(), pt.z - sensorOrigin.z());
794  if(v.norm_sq() > rangeMaxSqrd)
795  {
796  ignoreCell=true;
797  }
798  }
799 
800  if(!ignoreCell)
801  {
802  octomap::OcTreeKey key;
803  if (octree_->coordToKeyChecked(point, key))
804  {
805 
806  if(iter->first >0)
807  {
808  RtabmapColorOcTreeNode * n = octree_->search(key);
809  if(n && n->getNodeRefId() > 0 && n->getNodeRefId() >= iter->first)
810  {
811  // The cell has been updated from current node or more recent node, don't update the cell
812  continue;
813  }
814  }
815 
816  updateMinMax(point);
817 
818  RtabmapColorOcTreeNode * n = octree_->updateNode(key, false);
819  if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
820  {
821  n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
822  if(iter->first > 0)
823  {
824  n->setNodeRefId(iter->first);
825  }
826  }
827  }
828  }
829  }
830  //octree_->updateInnerOccupancy();
831  }
832 
833  // compress map
834  //if(orderedPoses.size() > 1)
835  //{
836  // octree_->prune();
837  //}
838 
839  // ignore negative ids as they are temporary clouds
840  if(iter->first > 0)
841  {
842  addedNodes_.insert(*iter);
843  }
844  UDEBUG("%d: end", iter->first);
845  }
846  else
847  {
848  UDEBUG("Did not find %d in cache", iter->first);
849  }
850  }
851  }
852 
853  if(!fullUpdate_)
854  {
855  cache_.clear();
856  cacheClouds_.clear();
857  cacheViewPoints_.clear();
858  }
859  return !orderedPoses.empty() || graphOptimized || graphChanged;
860 }
861 
862 void OctoMap::updateMinMax(const octomap::point3d & point)
863 {
864  if(point.x() < minValues_[0])
865  {
866  minValues_[0] = point.x();
867  }
868  if(point.y() < minValues_[1])
869  {
870  minValues_[1] = point.y();
871  }
872  if(point.z() < minValues_[2])
873  {
874  minValues_[2] = point.z();
875  }
876  if(point.x() > maxValues_[0])
877  {
878  maxValues_[0] = point.x();
879  }
880  if(point.y() > maxValues_[1])
881  {
882  maxValues_[1] = point.y();
883  }
884  if(point.z() > maxValues_[2])
885  {
886  maxValues_[2] = point.z();
887  }
888 }
889 
890 pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
891  unsigned int treeDepth,
892  std::vector<int> * obstacleIndices,
893  std::vector<int> * emptyIndices,
894  std::vector<int> * groundIndices,
895  bool originalRefPoints,
896  std::vector<int> * frontierIndices,
897  std::vector<double> * cloudProb) const
898 {
899  UASSERT(treeDepth <= octree_->getTreeDepth());
900  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
901  if(cloudProb)
902  {
903  cloudProb->resize(octree_->size());
904  }
905  UDEBUG("depth=%d (maxDepth=%d) octree = %d",
906  (int)treeDepth, (int)octree_->getTreeDepth(), (int)octree_->size());
907  cloud->resize(octree_->size());
908  if(obstacleIndices)
909  {
910  obstacleIndices->resize(octree_->size());
911  }
912  if(emptyIndices)
913  {
914  emptyIndices->resize(octree_->size());
915  }
916  if(frontierIndices)
917  {
918  frontierIndices->resize(octree_->size());
919  }
920  if(groundIndices)
921  {
922  groundIndices->resize(octree_->size());
923  }
924 
925  if(treeDepth == 0)
926  {
927  treeDepth = octree_->getTreeDepth();
928  }
929 
930  double minZ = minValues_[2];
931  double maxZ = maxValues_[2];
932 
933  bool addAllPoints = obstacleIndices == 0 && groundIndices == 0 && emptyIndices == 0;
934  int oi=0;
935  int si=0;
936  int ei=0;
937  int fi=0;
938  int gi=0;
939  float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
940  for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
941  {
942  if(octree_->isNodeOccupied(*it) && (obstacleIndices != 0 || groundIndices != 0 || addAllPoints))
943  {
944  octomap::point3d pt = octree_->keyToCoord(it.getKey());
945  if(cloudProb)
946  {
947  (*cloudProb)[oi] = it->getOccupancy();
948  }
949  if(octree_->getTreeDepth() == it.getDepth() && hasColor_)
950  {
951  (*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
952  }
953  else
954  {
955  // Gradiant color on z axis
956  float H = (maxZ - pt.z())*299.0f/(maxZ-minZ);
957  float r,g,b;
958  util2d::HSVtoRGB(&r, &g, &b, H, 1, 1);
959  (*cloud)[oi].r = r*255.0f;
960  (*cloud)[oi].g = g*255.0f;
961  (*cloud)[oi].b = b*255.0f;
962  }
963 
964  if(originalRefPoints && it->getOccupancyType() > 0)
965  {
966  const octomap::point3d & p = it->getPointRef();
967  (*cloud)[oi].x = p.x();
968  (*cloud)[oi].y = p.y();
969  (*cloud)[oi].z = p.z();
970  }
971  else
972  {
973  (*cloud)[oi].x = pt.x()-halfCellSize;
974  (*cloud)[oi].y = pt.y()-halfCellSize;
975  (*cloud)[oi].z = pt.z();
976  }
977 
978  if(it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeGround)
979  {
980  if(groundIndices)
981  {
982  groundIndices->at(gi++) = oi;
983  }
984  }
985  else if(obstacleIndices)
986  {
987  obstacleIndices->at(si++) = oi;
988  }
989 
990  ++oi;
991  }
992  else if(!octree_->isNodeOccupied(*it) && (emptyIndices != 0 || addAllPoints || frontierIndices !=0))
993  {
994  octomap::point3d pt = octree_->keyToCoord(it.getKey());
995  if(cloudProb)
996  {
997  (*cloudProb)[oi] = it->getOccupancy();
998  }
999 
1000  if(frontierIndices !=0 &&
1001  (!octree_->search( pt.x()+octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) || !octree_->search( pt.x()-octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) ||
1002  !octree_->search( pt.x(), pt.y()+octree_->getNodeSize(treeDepth), pt.z(), treeDepth) || !octree_->search( pt.x(), pt.y()-octree_->getNodeSize(treeDepth), pt.z(), treeDepth) ||
1003  !octree_->search( pt.x(), pt.y(), pt.z()+octree_->getNodeSize(treeDepth), treeDepth) || !octree_->search( pt.x(), pt.y(), pt.z()-octree_->getNodeSize(treeDepth), treeDepth) )) //ajouter 1 au key ?
1004  {
1005  //unknown neighbor FACE cell
1006  frontierIndices->at(fi++) = oi;
1007  }
1008 
1009 
1010  (*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
1011  (*cloud)[oi].x = pt.x()-halfCellSize;
1012  (*cloud)[oi].y = pt.y()-halfCellSize;
1013  (*cloud)[oi].z = pt.z();
1014  if(emptyIndices)
1015  {
1016  emptyIndices->at(ei++) = oi;
1017  }
1018 
1019  ++oi;
1020  }
1021  }
1022 
1023  cloud->resize(oi);
1024  if(cloudProb)
1025  {
1026  cloudProb->resize(oi);
1027  }
1028  if(obstacleIndices)
1029  {
1030  obstacleIndices->resize(si);
1031  UDEBUG("obstacle=%d", si);
1032  }
1033  if(emptyIndices)
1034  {
1035  emptyIndices->resize(ei);
1036  UDEBUG("empty=%d", ei);
1037  }
1038  if(frontierIndices)
1039  {
1040  frontierIndices->resize(fi);
1041  UDEBUG("frontier=%d", fi);
1042  }
1043  if(groundIndices)
1044  {
1045  groundIndices->resize(gi);
1046  UDEBUG("ground=%d", gi);
1047  }
1048 
1049  UDEBUG("");
1050  return cloud;
1051 }
1052 
1053 cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCellSize, float minGridSize, unsigned int treeDepth)
1054 {
1055  UDEBUG("minGridSize=%f, treeDepth=%d", minGridSize, (int)treeDepth);
1056  UASSERT(treeDepth <= octree_->getTreeDepth());
1057  if(treeDepth == 0)
1058  {
1059  treeDepth = octree_->getTreeDepth();
1060  }
1061 
1062  gridCellSize = octree_->getNodeSize(treeDepth);
1063 
1064  cv::Mat obstaclesMat = cv::Mat(1, (int)octree_->size(), CV_32FC2);
1065  cv::Mat groundMat = cv::Mat(1, (int)octree_->size(), CV_32FC2);
1066  int gi=0;
1067  int oi=0;
1068  cv::Vec2f * oPtr = obstaclesMat.ptr<cv::Vec2f>(0,0);
1069  cv::Vec2f * gPtr = groundMat.ptr<cv::Vec2f>(0,0);
1070  for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
1071  {
1072  octomap::point3d pt = octree_->keyToCoord(it.getKey());
1073  if(octree_->isNodeOccupied(*it) && it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeObstacle)
1074  {
1075  // projected on ground
1076  oPtr[oi][0] = pt.x();
1077  oPtr[oi][1] = pt.y();
1078  ++oi;
1079  }
1080  else
1081  {
1082  // projected on ground
1083  gPtr[gi][0] = pt.x();
1084  gPtr[gi][1] = pt.y();
1085  ++gi;
1086  }
1087  }
1088  obstaclesMat = obstaclesMat(cv::Range::all(), cv::Range(0, oi));
1089  groundMat = groundMat(cv::Range::all(), cv::Range(0, gi));
1090 
1091  std::map<int, Transform> poses;
1092  poses.insert(std::make_pair(1, Transform::getIdentity()));
1093  std::map<int, std::pair<cv::Mat, cv::Mat> > maps;
1094  maps.insert(std::make_pair(1, std::make_pair(groundMat, obstaclesMat)));
1095 
1096  cv::Mat map = util3d::create2DMapFromOccupancyLocalMaps(
1097  poses,
1098  maps,
1099  gridCellSize,
1100  xMin, yMin,
1101  minGridSize,
1102  false);
1103  UDEBUG("");
1104  return map;
1105 }
1106 
1107 bool OctoMap::writeBinary(const std::string & path)
1108 {
1109  return octree_->writeBinary(path);
1110 }
1111 
1112 } /* namespace rtabmap */
NULL
#define NULL
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:476
octomap::KeyRay::begin
iterator begin()
octomap::OcTreeBaseImpl::begin
iterator begin(unsigned char maxDepth=0) const
UTimer
Definition: UTimer.h:46
octomap::ColorOcTreeNode::getAverageChildColor
ColorOcTreeNode::Color getAverageChildColor() const
rtabmap::util3d::create2DMapFromOccupancyLocalMaps
cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(const std::map< int, Transform > &poses, const std::map< int, std::pair< cv::Mat, cv::Mat > > &occupancy, float cellSize, float &xMin, float &yMin, float minMapSize=0.0f, bool erode=false, float footprintRadius=0.0f)
Definition: util3d_mapping.cpp:191
OcTreeDataNode< float >::getValue
float getValue() const
octomap::ColorOcTreeNode::isColorSet
bool isColorSet() const
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::RtabmapColorOcTree::averageNodeColor
RtabmapColorOcTreeNode * averageNodeColor(const octomap::OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: OctoMap.cpp:173
octomath::Vector3::normalize
Vector3 & normalize()
f
f
rtabmap::OctoMap::updateMinMax
void updateMinMax(const octomap::point3d &point)
Definition: OctoMap.cpp:862
octomap::OccupancyOcTreeBase::updateNode
virtual NODE * updateNode(const OcTreeKey &key, float log_odds_update, bool lazy_eval=false)
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:380
rtabmap::RtabmapColorOcTree::RtabmapColorOcTreeMemberInit
static StaticMemberInitializer RtabmapColorOcTreeMemberInit
static member to ensure static initialization (only once)
Definition: OctoMap.h:166
rtabmap::OctoMap::addedNodes_
std::map< int, Transform > addedNodes_
Definition: OctoMap.h:225
octomap::OcTreeBaseImpl::end
const iterator end() const
rtabmap::RtabmapColorOcTreeNode::createChild
bool createChild(unsigned int i)
Definition: OctoMap.cpp:97
rtabmap::OctoMap::fullUpdate_
bool fullUpdate_
Definition: OctoMap.h:227
rtabmap::OctoMap::cacheViewPoints_
std::map< int, cv::Point3f > cacheViewPoints_
Definition: OctoMap.h:223
rtabmap::RtabmapColorOcTreeNode::getChild
RtabmapColorOcTreeNode * getChild(unsigned int i)
Definition: OctoMap.cpp:44
glm::all
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
octomap::OcTreeBaseImpl< RtabmapColorOcTreeNode, AbstractOccupancyOcTree >::search
RtabmapColorOcTreeNode * search(double x, double y, double z, unsigned int depth=0) const
rtabmap::util3d::laserScanToPoint
pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const LaserScan &laserScan, int index)
Definition: util3d.cpp:2487
octomap::OcTreeBaseImpl::getTreeDepth
unsigned int getTreeDepth() const
octomap::OcTreeNode::updateOccupancyChildren
void updateOccupancyChildren()
rtabmap::RtabmapColorOcTree::isNodeCollapsible
virtual bool isNodeCollapsible(const RtabmapColorOcTreeNode *node) const
Definition: OctoMap.cpp:149
OcTreeDataNode< float >::childExists
bool childExists(unsigned int i) const
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:102
octomap::OcTreeBaseImpl::coordToKeyChecked
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
rtabmap::RtabmapColorOcTreeNode::setPointRef
void setPointRef(const octomap::point3d &point)
Definition: OctoMap.h:63
rtabmap::util3d::laserScanToPointRGB
pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const LaserScan &laserScan, int index, unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2522
UFATAL
#define UFATAL(...)
rtabmap::RtabmapColorOcTreeNode::setNodeRefId
void setNodeRefId(int nodeRefId)
Definition: OctoMap.h:61
octomap::OcTreeBaseImpl::keyToCoord
double keyToCoord(unsigned short int key, unsigned depth) const
util3d_mapping.h
glm::uint8_t
detail::uint8 uint8_t
Definition: fwd.hpp:908
UASSERT
#define UASSERT(condition)
rtabmap::OctoMap::update
bool update(const std::map< int, Transform > &poses)
Definition: OctoMap.cpp:382
UStl.h
Wrappers of STL for convenient functions.
octomap::OcTreeBaseImpl::computeRayKeys
bool computeRayKeys(const point3d &origin, const point3d &end, KeyRay &ray) const
octomap::OcTreeNode::setLogOdds
void setLogOdds(float l)
rtabmap::OctoMap::addToCache
void addToCache(int nodeId, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &ground, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &obstacles, const pcl::PointXYZ &viewPoint)
Definition: OctoMap.cpp:348
rtabmap::OctoMap::cache_
std::map< int, std::pair< std::pair< cv::Mat, cv::Mat >, cv::Mat > > cache_
Definition: OctoMap.h:221
rtabmap::RtabmapColorOcTreeNode::getPointRef
const octomap::point3d & getPointRef() const
Definition: OctoMap.h:66
true
#define true
Definition: ConvertUTF.c:57
rtabmap::RtabmapColorOcTree::integrateNodeColor
RtabmapColorOcTreeNode * integrateNodeColor(const octomap::OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: OctoMap.cpp:190
rtabmap::RtabmapColorOcTreeNode::getOccupancyType
int getOccupancyType() const
Definition: OctoMap.h:65
OcTreeDataNode< float >::children
OcTreeDataNode< float > ** children
rtabmap::OctoMap::cacheClouds_
std::map< int, std::pair< const pcl::PointCloud< pcl::PointXYZRGB >::Ptr, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr > > cacheClouds_
Definition: OctoMap.h:222
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
util3d_filtering.h
rtabmap::OctoMap::octree_
RtabmapColorOcTree * octree_
Definition: OctoMap.h:224
OcTreeDataNode< float >::collapsible
bool collapsible() const
octomap::OcTreeNode::getLogOdds
float getLogOdds() const
octomap::KeySet
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
tree
tree
rtabmap::RtabmapColorOcTree::RtabmapColorOcTree
RtabmapColorOcTree(double resolution)
Default constructor, sets resolution of leafs.
Definition: OctoMap.cpp:108
octomap::OcTreeNode::getOccupancy
double getOccupancy() const
util3d_transforms.h
rtabmap::RtabmapColorOcTreeNode::setOccupancyType
void setOccupancyType(char type)
Definition: OctoMap.h:62
rtabmap::RtabmapColorOcTree::updateInnerOccupancyRecurs
void updateInnerOccupancyRecurs(RtabmapColorOcTreeNode *node, unsigned int depth)
Definition: OctoMap.cpp:219
octomap::KeyRay::end
iterator end()
false
#define false
Definition: ConvertUTF.c:56
rtabmap::OctoMap::minValues_
double minValues_[3]
Definition: OctoMap.h:231
rtabmap::OctoMap::~OctoMap
virtual ~OctoMap()
Definition: OctoMap.cpp:330
rtabmap::OctoMap::createCloud
pcl::PointCloud< pcl::PointXYZRGB >::Ptr createCloud(unsigned int treeDepth=0, std::vector< int > *obstacleIndices=0, std::vector< int > *emptyIndices=0, std::vector< int > *groundIndices=0, bool originalRefPoints=true, std::vector< int > *frontierIndices=0, std::vector< double > *cloudProb=0) const
Definition: OctoMap.cpp:890
rtabmap::OctoMap::rangeMax_
float rangeMax_
Definition: OctoMap.h:229
UDEBUG
#define UDEBUG(...)
octomap::OcTreeBaseImpl::getResolution
double getResolution() const
rtabmap::OctoMap::writeBinary
bool writeBinary(const std::string &path)
Definition: OctoMap.cpp:1107
octomap::point3d
octomath::Vector3 point3d
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
rtabmap::OctoMap::OctoMap
OctoMap(const ParametersMap &parameters)
Definition: OctoMap.cpp:271
octomap::OcTreeBaseImpl::getNodeSize
double getNodeSize(unsigned depth) const
rtabmap::RtabmapColorOcTree::setNodeColor
RtabmapColorOcTreeNode * setNodeColor(const octomap::OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: OctoMap.cpp:113
rtabmap::OctoMap::rayTracing_
bool rayTracing_
Definition: OctoMap.h:230
rtabmap::OctoMap::createProjectionMap
cv::Mat createProjectionMap(float &xMin, float &yMin, float &gridCellSize, float minGridSize=0.0f, unsigned int treeDepth=0)
Definition: OctoMap.cpp:1053
rtabmap::OctoMap::maxValues_
double maxValues_[3]
Definition: OctoMap.h:232
rtabmap::util2d::HSVtoRGB
void RTABMAP_EXP HSVtoRGB(float *r, float *g, float *b, float h, float s, float v)
Definition: util2d.cpp:2047
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::RtabmapColorOcTree::pruneNode
virtual bool pruneNode(RtabmapColorOcTreeNode *node)
Definition: OctoMap.cpp:124
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:125
octomap::OcTreeBaseImpl::size
virtual size_t size() const
rtabmap::OctoMap::updateError_
float updateError_
Definition: OctoMap.h:228
octomap::ColorOcTreeNode::getColor
Color getColor() const
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
OcTreeDataNode< float >::hasChildren
bool hasChildren() const
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59