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/global_map/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 
39 namespace rtabmap {
40 
42 // RtabmapColorOcTree
44 
45 RtabmapColorOcTreeNode* RtabmapColorOcTreeNode::getChild(unsigned int i) {
46 #ifdef OCTOMAP_PRE_18
47  return static_cast<RtabmapColorOcTreeNode*> (OcTreeNode::getChild(i));
48 #else
49  UFATAL("This function should not be used with octomap >= 1.8");
50  return 0;
51 #endif
52 }
53 const RtabmapColorOcTreeNode* RtabmapColorOcTreeNode::getChild(unsigned int i) const {
54 #ifdef OCTOMAP_PRE_18
55  return static_cast<const RtabmapColorOcTreeNode*> (OcTreeNode::getChild(i));
56 #else
57  UFATAL("This function should not be used with octomap >= 1.8");
58  return 0;
59 #endif
60 }
61 
62 //octomap <1.8
63 bool RtabmapColorOcTreeNode::pruneNode() {
64 #ifdef OCTOMAP_PRE_18
65  // checks for equal occupancy only, color ignored
66  if (!this->collapsible()) return false;
67  // set occupancy value
68  setLogOdds(getChild(0)->getLogOdds());
69  // set color to average color
70  if (isColorSet()) color = getAverageChildColor();
71  // delete children
72  for (unsigned int i=0;i<8;i++) {
73  delete children[i];
74  }
75  delete[] children;
76  children = NULL;
77  return true;
78 #else
79  UFATAL("This function should not be used with octomap >= 1.8");
80  return false;
81 #endif
82 }
83 //octomap <1.8
84 void RtabmapColorOcTreeNode::expandNode() {
85 #ifdef OCTOMAP_PRE_18
86  assert(!hasChildren());
87  for (unsigned int k=0; k<8; k++) {
88  createChild(k);
89  children[k]->setValue(value);
90  getChild(k)->setColor(color);
91  }
92 #else
93  UFATAL("This function should not be used with octomap >= 1.8");
94 #endif
95 }
96 
97 //octomap <1.8
98 bool RtabmapColorOcTreeNode::createChild(unsigned int i) {
99 #ifdef OCTOMAP_PRE_18
100  if (children == NULL) allocChildren();
101  children[i] = new RtabmapColorOcTreeNode();
102  return true;
103 #else
104  UFATAL("This function should not be used with octomap >= 1.8");
105  return false;
106 #endif
107 }
108 
109 void RtabmapColorOcTreeNode::updateOccupancyTypeChildren()
110 {
111  if (children != NULL){
112  int type = kTypeUnknown;
113  for (int i=0; i<8 && type != kTypeObstacle; i++) {
114  RtabmapColorOcTreeNode* child = static_cast<RtabmapColorOcTreeNode*>(children[i]);
115 
116  if (child != NULL && child->getOccupancyType() >= kTypeEmpty) {
117  if(type == kTypeUnknown) {
118  type = child->getOccupancyType();
119  }
120  }
121  }
122  type_ = type;
123  }
124 }
125 
126 RtabmapColorOcTree::RtabmapColorOcTree(double resolution)
127  : OccupancyOcTreeBase<RtabmapColorOcTreeNode>(resolution) {
128  RtabmapColorOcTreeMemberInit.ensureLinking();
129 };
130 
131 RtabmapColorOcTreeNode* RtabmapColorOcTree::setNodeColor(const octomap::OcTreeKey& key,
132  uint8_t r,
133  uint8_t g,
134  uint8_t b) {
135  RtabmapColorOcTreeNode* n = search (key);
136  if (n != 0) {
137  n->setColor(r, g, b);
138  }
139  return n;
140 }
141 
142 bool RtabmapColorOcTree::pruneNode(RtabmapColorOcTreeNode* node) {
143 #ifndef OCTOMAP_PRE_18
144  if (!isNodeCollapsible(node))
145  return false;
146 
147  // set value to children's values (all assumed equal)
148  node->copyData(*(getNodeChild(node, 0)));
149 
150  if (node->isColorSet()) // TODO check
151  node->setColor(node->getAverageChildColor());
152 
153  // delete children
154  for (unsigned int i=0;i<8;i++) {
155  deleteNodeChild(node, i);
156  }
157  delete[] node->children;
158  node->children = NULL;
159 
160  return true;
161 #else
162  UFATAL("This function should not be used with octomap < 1.8");
163  return false;
164 #endif
165 }
166 
167 bool RtabmapColorOcTree::isNodeCollapsible(const RtabmapColorOcTreeNode* node) const{
168 #ifndef OCTOMAP_PRE_18
169  // all children must exist, must not have children of
170  // their own and have the same occupancy probability
171  if (!nodeChildExists(node, 0))
172  return false;
173 
174  const RtabmapColorOcTreeNode* firstChild = getNodeChild(node, 0);
175  if (nodeHasChildren(firstChild))
176  return false;
177 
178  for (unsigned int i = 1; i<8; i++) {
179  // compare nodes only using their occupancy, ignoring color for pruning
180  if (!nodeChildExists(node, i) || nodeHasChildren(getNodeChild(node, i)) || !(getNodeChild(node, i)->getValue() == firstChild->getValue()))
181  return false;
182  }
183 
184  return true;
185 #else
186  UFATAL("This function should not be used with octomap < 1.8");
187  return false;
188 #endif
189 }
190 
191 RtabmapColorOcTreeNode* RtabmapColorOcTree::averageNodeColor(const octomap::OcTreeKey& key,
192  uint8_t r,
193  uint8_t g,
194  uint8_t b) {
195  RtabmapColorOcTreeNode* n = search(key);
196  if (n != 0) {
197  if (n->isColorSet()) {
198  RtabmapColorOcTreeNode::Color prev_color = n->getColor();
199  n->setColor((prev_color.r + r)/2, (prev_color.g + g)/2, (prev_color.b + b)/2);
200  }
201  else {
202  n->setColor(r, g, b);
203  }
204  }
205  return n;
206 }
207 
208 RtabmapColorOcTreeNode* RtabmapColorOcTree::integrateNodeColor(const octomap::OcTreeKey& key,
209  uint8_t r,
210  uint8_t g,
211  uint8_t b) {
212  RtabmapColorOcTreeNode* n = search (key);
213  if (n != 0) {
214  if (n->isColorSet()) {
215  RtabmapColorOcTreeNode::Color prev_color = n->getColor();
216  double node_prob = n->getOccupancy();
217  uint8_t new_r = (uint8_t) ((double) prev_color.r * node_prob
218  + (double) r * (0.99-node_prob));
219  uint8_t new_g = (uint8_t) ((double) prev_color.g * node_prob
220  + (double) g * (0.99-node_prob));
221  uint8_t new_b = (uint8_t) ((double) prev_color.b * node_prob
222  + (double) b * (0.99-node_prob));
223  n->setColor(new_r, new_g, new_b);
224  }
225  else {
226  n->setColor(r, g, b);
227  }
228  }
229  return n;
230 }
231 
232 
233 void RtabmapColorOcTree::updateInnerOccupancy() {
234  this->updateInnerOccupancyRecurs(this->root, 0);
235 }
236 
237 void RtabmapColorOcTree::updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node, unsigned int depth) {
238 #ifndef OCTOMAP_PRE_18
239  // only recurse and update for inner nodes:
240  if (nodeHasChildren(node)){
241  // return early for last level:
242  if (depth < this->tree_depth){
243  for (unsigned int i=0; i<8; i++) {
244  if (nodeChildExists(node, i)) {
245  updateInnerOccupancyRecurs(getNodeChild(node, i), depth+1);
246  }
247  }
248  }
249  node->updateOccupancyChildren();
250  node->updateColorChildren();
251  node->updateOccupancyTypeChildren();
252  }
253 #else
254  // only recurse and update for inner nodes:
255  if (node->hasChildren()){
256  // return early for last level:
257  if (depth < this->tree_depth){
258  for (unsigned int i=0; i<8; i++) {
259  if (node->childExists(i)) {
260  updateInnerOccupancyRecurs(node->getChild(i), depth+1);
261  }
262  }
263  }
264  node->updateOccupancyChildren();
265  node->updateColorChildren();
266  node->updateOccupancyTypeChildren();
267  }
268 #endif
269 }
270 
271 RtabmapColorOcTree::StaticMemberInitializer::StaticMemberInitializer() {
272  RtabmapColorOcTree* tree = new RtabmapColorOcTree(0.1);
273 
274 #ifndef OCTOMAP_PRE_18
275  tree->clearKeyRays();
276 #endif
277 
278  AbstractOcTree::registerTreeType(tree);
279  }
280 
281 #ifndef _WIN32
282 // On Windows, the app freezes on start if the following is defined
283 RtabmapColorOcTree::StaticMemberInitializer RtabmapColorOcTree::RtabmapColorOcTreeMemberInit;
284 #endif
285 
286 
288 // OctoMap
290 
291 OctoMap::OctoMap(const LocalGridCache * cache, const ParametersMap & parameters) :
292  GlobalMap(cache, parameters),
293  hasColor_(false),
294  rangeMax_(Parameters::defaultGridRangeMax()),
295  rayTracing_(Parameters::defaultGridRayTracing()),
296  emptyFloodFillDepth_(Parameters::defaultGridGlobalFloodFillDepth())
297 {
298  octree_ = new RtabmapColorOcTree(cellSize_);
299  if(occupancyThr_ <= 0.0f)
300  {
301  UWARN("Cannot set %s to null for OctoMap, using default value %f instead.",
302  Parameters::kGridGlobalOccupancyThr().c_str(),
303  Parameters::defaultGridGlobalOccupancyThr());
304  occupancyThr_ = Parameters::defaultGridGlobalOccupancyThr();
305  }
306 
307  UDEBUG("occupancyThr_=%f", occupancyThr_);
308  UDEBUG("probHit_=%f", probability(logOddsHit_));
309  UDEBUG("probMiss_=%f", probability(logOddsMiss_));
310  UDEBUG("probClampingMin_=%f", probability(logOddsClampingMin_));
311  UDEBUG("probClampingMax_=%f", probability(logOddsClampingMax_));
312 
313  octree_->setOccupancyThres(occupancyThr_);
314  octree_->setProbHit(probability(logOddsHit_));
315  octree_->setProbMiss(probability(logOddsMiss_));
316  octree_->setClampingThresMin(probability(logOddsClampingMin_));
317  octree_->setClampingThresMax(probability(logOddsClampingMax_));
318 
319  Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);
320  Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
321 
322  Parameters::parse(parameters, Parameters::kGridGlobalFloodFillDepth(), emptyFloodFillDepth_);
323  UASSERT(emptyFloodFillDepth_>=0 && emptyFloodFillDepth_<=16);
324 
325  UDEBUG("rangeMax_ =%f", rangeMax_);
326  UDEBUG("rayTracing_ =%s", rayTracing_?"true":"false");
327  UDEBUG("emptyFloodFillDepth_=%d", emptyFloodFillDepth_);
328 }
329 
330 OctoMap::~OctoMap()
331 {
332  this->clear();
333  delete octree_;
334 }
335 
336 void OctoMap::clear()
337 {
338  octree_->clear();
339  hasColor_ = false;
340  GlobalMap::clear();
341 }
342 
343 unsigned long OctoMap::getMemoryUsed() const
344 {
345  unsigned long memoryUsage = GlobalMap::getMemoryUsed();
346 
347  // Note: size of OctoMap object is missing.
348 
349  return memoryUsage;
350 }
351 
352 bool OctoMap::isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition)
353 {
354  auto nodePtr = octree_->search(startPosition.x(), startPosition.y(), startPosition.z(), treeDepth);
355  if(nodePtr != NULL)
356  {
357  if(!octree_->isNodeOccupied(*nodePtr))
358  {
359  return true;
360  }
361  }
362 
363  return false;
364 }
365 
366 octomap::point3d OctoMap::findCloseEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition)
367 {
368 
369  //try current position
370  if(isValidEmpty(octree_,treeDepth,startPosition))
371  {
372  return startPosition;
373  }
374 
375  //x pos
376  if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x()+octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z())))
377  {
378  return octomap::point3d(startPosition.x()+octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z());
379  }
380 
381  //x neg
382  if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z())))
383  {
384  return octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z());
385  }
386 
387  //y pos
388  if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y()+octree_->getNodeSize(treeDepth), startPosition.z())))
389  {
390  return octomap::point3d(startPosition.x(), startPosition.y()+octree_->getNodeSize(treeDepth), startPosition.z());
391  }
392 
393 
394  //y neg
395  if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z())))
396  {
397  return octomap::point3d(startPosition.x(), startPosition.y()-octree_->getNodeSize(treeDepth), startPosition.z());
398  }
399 
400  //z pos
401  if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()+octree_->getNodeSize(treeDepth))))
402  {
403  return octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()+octree_->getNodeSize(treeDepth));
404  }
405 
406  //z neg
407  if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()-octree_->getNodeSize(treeDepth))))
408  {
409  return octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()-octree_->getNodeSize(treeDepth));
410  }
411 
412  //no valid position
413  return startPosition;
414 }
415 
416 bool OctoMap::isNodeVisited(std::unordered_set<octomap::OcTreeKey,octomap::OcTreeKey::KeyHash> const & EmptyNodes,octomap::OcTreeKey const key)
417 {
418  for(auto it = EmptyNodes.find(key);it != EmptyNodes.end();it++)
419  {
420  if(*it == key)
421  {
422  return true;
423  }
424 
425  }
426  return false;
427 }
428 
429 void OctoMap::floodFill(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition, std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> & EmptyNodes,std::queue<octomap::point3d>& positionToExplore)
430 {
431 
432  auto key = octree_->coordToKey(startPosition,treeDepth);
433  if(!isNodeVisited(EmptyNodes,key))
434  {
435  if(isValidEmpty(octree_,treeDepth,startPosition))
436  {
437  EmptyNodes.insert(key);
438  positionToExplore.push(octomap::point3d(startPosition.x()+octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z()));
439  positionToExplore.push(octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z()));
440  positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y()+octree_->getNodeSize(treeDepth), startPosition.z()));
441  positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y()-octree_->getNodeSize(treeDepth), startPosition.z()));
442  positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()+octree_->getNodeSize(treeDepth)));
443  positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()-octree_->getNodeSize(treeDepth)));
444  }
445  }
446 }
447 
448 
449 std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> OctoMap::findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition)
450 {
451  std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> exploreNode;
452  std::queue<octomap::point3d> positionToExplore;
453 
454  startPosition = findCloseEmpty(octree_,treeDepth, startPosition);
455 
456  floodFill(octree_, treeDepth, startPosition, exploreNode, positionToExplore);
457  while(!positionToExplore.empty())
458  {
459  floodFill(octree_, treeDepth, positionToExplore.front(), exploreNode, positionToExplore);
460  positionToExplore.pop();
461  }
462 
463  return exploreNode;
464 }
465 
466 
467 void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
468 {
469  // Original version from A. Hornung:
470  // https://github.com/OctoMap/octomap_mapping/blob/jade-devel/octomap_server/src/OctomapServer.cpp#L356
471  //
472 
473  int lastId = assembledNodes().size()?assembledNodes().rbegin()->first:0;
474  UDEBUG("Last id = %d", lastId);
475 
476  UDEBUG("newPoses = %d", (int)newPoses.size());
477 
478  if(!newPoses.empty())
479  {
480  float rangeMaxSqrd = rangeMax_*rangeMax_;
481  float cellSize = octree_->getResolution();
482  for(std::list<std::pair<int, Transform> >::const_iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
483  {
484  std::map<int, LocalGrid>::const_iterator localGridIter;
485  localGridIter = cache().find(iter->first);
486  if(localGridIter != cache().end())
487  {
488  cv::Mat ground = localGridIter->second.groundCells;
489  cv::Mat obstacles = localGridIter->second.obstacleCells;
490  cv::Mat emptyCells = localGridIter->second.emptyCells;
491 
492  if(!localGridIter->second.is3D())
493  {
494  UWARN("It seems the local occupancy grids are not 3d, cannot update OctoMap! (ground type=%d, obstacles type=%d, empty type=%d)",
495  ground.type(), obstacles.type(), emptyCells.type());
496  continue;
497  }
498 
499  UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
500 
501  octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
502  sensorOrigin += octomap::point3d(localGridIter->second.viewPoint.x, localGridIter->second.viewPoint.y, localGridIter->second.viewPoint.z);
503 
504  updateMinMax(sensorOrigin);
505 
506  octomap::OcTreeKey tmpKey;
507  if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey))
508  {
509  UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
510  }
511 
512  bool computeRays = rayTracing_ && emptyCells.empty();
513 
514  // instead of direct scan insertion, compute update to filter ground:
515  octomap::KeySet free_cells;
516  // insert ground points only as free:
517  unsigned int maxGroundPts = ground.cols;
518  UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
519  Eigen::Affine3f t = iter->second.toEigen3f();
520  LaserScan tmpGround = LaserScan::backwardCompatibility(ground);
521  UASSERT(tmpGround.size() == (int)maxGroundPts);
522  for (unsigned int i=0; i<maxGroundPts; ++i)
523  {
524  pcl::PointXYZRGB pt;
525  pt = util3d::laserScanToPointRGB(tmpGround, i);
526  pt = pcl::transformPoint(pt, t);
527 
528  octomap::point3d point(pt.x, pt.y, pt.z);
529  bool ignoreOccupiedCell = false;
530  if(rangeMaxSqrd > 0.0f)
531  {
532  octomap::point3d v(pt.x - cellSize - sensorOrigin.x(), pt.y - cellSize - sensorOrigin.y(), pt.z - cellSize - sensorOrigin.z());
533  if(v.norm_sq() > rangeMaxSqrd)
534  {
535  // compute new point to max range
536  v.normalize();
537  v*=rangeMax_;
538  point = sensorOrigin + v;
539  ignoreOccupiedCell=true;
540  }
541  }
542 
543  if(!ignoreOccupiedCell)
544  {
545  // occupied endpoint
546  octomap::OcTreeKey key;
547  if (octree_->coordToKeyChecked(point, key))
548  {
549  if(iter->first >0 && iter->first<lastId)
550  {
551  RtabmapColorOcTreeNode * n = octree_->search(key);
552  if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
553  {
554  // The cell has been updated from more recent node, don't update the cell
555  continue;
556  }
557  }
558 
559  updateMinMax(point);
560  RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
561 
562  if(n)
563  {
564  if(!hasColor_ && !(pt.r ==0 && pt.g == 0 && pt.b == 0) && !(pt.r ==255 && pt.g == 255 && pt.b == 255))
565  {
566  hasColor_ = true;
567  }
568  octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
569  if(iter->first > 0)
570  {
571  n->setNodeRefId(iter->first);
572  n->setPointRef(point);
573  }
574  n->setOccupancyType(RtabmapColorOcTreeNode::kTypeGround);
575  }
576  }
577  }
578 
579  // only clear space (ground points)
580  octomap::KeyRay keyRay;
581  if (computeRays &&
582  (iter->first < 0 || iter->first>lastId) &&
583  octree_->computeRayKeys(sensorOrigin, point, keyRay))
584  {
585  free_cells.insert(keyRay.begin(), keyRay.end());
586  }
587  }
588  UDEBUG("%d: ground cells=%d free cells=%d", iter->first, (int)maxGroundPts, (int)free_cells.size());
589 
590  // all other points: free on ray, occupied on endpoint:
591  unsigned int maxObstaclePts = obstacles.cols;
592  UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
593  LaserScan tmpObstacle = LaserScan::backwardCompatibility(obstacles);
594  UASSERT(tmpObstacle.size() == (int)maxObstaclePts);
595  for (unsigned int i=0; i<maxObstaclePts; ++i)
596  {
597  pcl::PointXYZRGB pt;
598  pt = util3d::laserScanToPointRGB(tmpObstacle, i);
599  pt = pcl::transformPoint(pt, t);
600 
601  octomap::point3d point(pt.x, pt.y, pt.z);
602 
603  bool ignoreOccupiedCell = false;
604  if(rangeMaxSqrd > 0.0f)
605  {
606  octomap::point3d v(pt.x - cellSize - sensorOrigin.x(), pt.y - cellSize - sensorOrigin.y(), pt.z - cellSize - sensorOrigin.z());
607  if(v.norm_sq() > rangeMaxSqrd)
608  {
609  // compute new point to max range
610  v.normalize();
611  v*=rangeMax_;
612  point = sensorOrigin + v;
613  ignoreOccupiedCell=true;
614  }
615  }
616 
617  if(!ignoreOccupiedCell)
618  {
619  // occupied endpoint
620  octomap::OcTreeKey key;
621  if (octree_->coordToKeyChecked(point, key))
622  {
623  if(iter->first >0 && iter->first<lastId)
624  {
625  RtabmapColorOcTreeNode * n = octree_->search(key);
626  if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
627  {
628  // The cell has been updated from more recent node, don't update the cell
629  continue;
630  }
631  }
632 
633  updateMinMax(point);
634 
635  RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
636  if(n)
637  {
638  if(!hasColor_ && !(pt.r ==0 && pt.g == 0 && pt.b == 0) && !(pt.r ==255 && pt.g == 255 && pt.b == 255))
639  {
640  hasColor_ = true;
641  }
642  octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
643  if(iter->first > 0)
644  {
645  n->setNodeRefId(iter->first);
646  n->setPointRef(point);
647  }
648  n->setOccupancyType(RtabmapColorOcTreeNode::kTypeObstacle);
649  }
650  }
651  }
652 
653  // free cells
654  octomap::KeyRay keyRay;
655  if (computeRays &&
656  (iter->first < 0 || iter->first>lastId) &&
657  octree_->computeRayKeys(sensorOrigin, point, keyRay))
658  {
659  free_cells.insert(keyRay.begin(), keyRay.end());
660  }
661  }
662  UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)maxObstaclePts, (int)free_cells.size());
663 
664 
665  // mark free cells only if not seen occupied in this cloud
666  for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
667  {
668  if(iter->first > 0)
669  {
670  RtabmapColorOcTreeNode * n = octree_->search(*it);
671  if(n && n->getNodeRefId() > 0 && n->getNodeRefId() >= iter->first)
672  {
673  // The cell has been updated from current node or more recent node, don't update the cell
674  continue;
675  }
676  }
677 
678  RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false, true);
679  if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
680  {
681  n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
682  if(iter->first > 0)
683  {
684  n->setNodeRefId(iter->first);
685  }
686  }
687  }
688 
689  // all empty cells
690  if(emptyCells.cols)
691  {
692  unsigned int maxEmptyPts = emptyCells.cols;
693  UDEBUG("%d: compute free cells (from %d empty points)", iter->first, (int)maxEmptyPts);
694  LaserScan tmpEmpty = LaserScan::backwardCompatibility(emptyCells);
695  UASSERT(tmpEmpty.size() == (int)maxEmptyPts);
696  for (unsigned int i=0; i<maxEmptyPts; ++i)
697  {
698  pcl::PointXYZ pt;
699  pt = util3d::laserScanToPoint(tmpEmpty, i);
700  pt = pcl::transformPoint(pt, t);
701 
702  octomap::point3d point(pt.x, pt.y, pt.z);
703 
704  bool ignoreCell = false;
705  if(rangeMaxSqrd > 0.0f)
706  {
707  octomap::point3d v(pt.x - sensorOrigin.x(), pt.y - sensorOrigin.y(), pt.z - sensorOrigin.z());
708  if(v.norm_sq() > rangeMaxSqrd)
709  {
710  ignoreCell=true;
711  }
712  }
713 
714  if(!ignoreCell)
715  {
716  octomap::OcTreeKey key;
717  if (octree_->coordToKeyChecked(point, key))
718  {
719 
720  if(iter->first >0)
721  {
722  RtabmapColorOcTreeNode * n = octree_->search(key);
723  if(n && n->getNodeRefId() > 0 && n->getNodeRefId() >= iter->first)
724  {
725  // The cell has been updated from current node or more recent node, don't update the cell
726  continue;
727  }
728  }
729 
730  updateMinMax(point);
731 
732  RtabmapColorOcTreeNode * n = octree_->updateNode(key, false, true);
733  if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
734  {
735  n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
736  if(iter->first > 0)
737  {
738  n->setNodeRefId(iter->first);
739  }
740  }
741  }
742  }
743  }
744  }
745 
746  if(emptyCells.cols || !free_cells.empty())
747  {
748  octree_->updateInnerOccupancy();
749  }
750 
751  // compress map
752  //if(newPoses.size() > 1)
753  //{
754  // octree_->prune();
755  //}
756 
757  addAssembledNode(iter->first, iter->second);
758  UDEBUG("%d: end", iter->first);
759  }
760  else
761  {
762  UDEBUG("Did not find %d in cache", iter->first);
763  }
764  }
765  }
766 
767  if(emptyFloodFillDepth_>0)
768  {
769  UTimer t;
770 
771  auto key = octree_->coordToKey(0, 0, 0, emptyFloodFillDepth_);
772  auto pos = octree_->keyToCoord(key);
773 
774  std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> EmptyNodes = findEmptyNode(octree_,emptyFloodFillDepth_, pos);
775  std::vector<octomap::OcTreeKey> nodeToDelete;
776 
777  for (RtabmapColorOcTree::iterator it = octree_->begin_leafs(emptyFloodFillDepth_); it != octree_->end_leafs(); ++it)
778  {
779  if(!octree_->isNodeOccupied(*it))
780  {
781  if(!isNodeVisited(EmptyNodes,it.getKey()))
782  {
783  nodeToDelete.push_back(it.getKey());
784  }
785  }
786  }
787 
788  for(unsigned int y=0; y < nodeToDelete.size(); y++)
789  {
790  octree_->deleteNode(nodeToDelete[y],emptyFloodFillDepth_);
791  }
792  UDEBUG("Flood Fill: deleted %d empty cells (%fs)", (int)nodeToDelete.size(), t.ticks());
793  }
794 }
795 
796 void OctoMap::updateMinMax(const octomap::point3d & point)
797 {
798  if(point.x() < minValues_[0])
799  {
800  minValues_[0] = point.x();
801  }
802  if(point.y() < minValues_[1])
803  {
804  minValues_[1] = point.y();
805  }
806  if(point.z() < minValues_[2])
807  {
808  minValues_[2] = point.z();
809  }
810  if(point.x() > maxValues_[0])
811  {
812  maxValues_[0] = point.x();
813  }
814  if(point.y() > maxValues_[1])
815  {
816  maxValues_[1] = point.y();
817  }
818  if(point.z() > maxValues_[2])
819  {
820  maxValues_[2] = point.z();
821  }
822 }
823 
824 
825 pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
826  unsigned int treeDepth,
827  std::vector<int> * obstacleIndices,
828  std::vector<int> * emptyIndices,
829  std::vector<int> * groundIndices,
830  bool originalRefPoints,
831  std::vector<int> * frontierIndices,
832  std::vector<double> * cloudProb) const
833 {
834  UASSERT(treeDepth <= octree_->getTreeDepth());
835  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
836  if(cloudProb)
837  {
838  cloudProb->resize(octree_->size());
839  }
840  UDEBUG("depth=%d (maxDepth=%d) octree = %d",
841  (int)treeDepth, (int)octree_->getTreeDepth(), (int)octree_->size());
842  cloud->resize(octree_->size());
843  if(obstacleIndices)
844  {
845  obstacleIndices->resize(octree_->size());
846  }
847  if(emptyIndices)
848  {
849  emptyIndices->resize(octree_->size());
850  }
851  if(frontierIndices)
852  {
853  frontierIndices->resize(octree_->size());
854  }
855  if(groundIndices)
856  {
857  groundIndices->resize(octree_->size());
858  }
859 
860  if(treeDepth == 0)
861  {
862  treeDepth = octree_->getTreeDepth();
863  }
864 
865  double minZ = minValues_[2];
866  double maxZ = maxValues_[2];
867 
868  bool addAllPoints = obstacleIndices == 0 && groundIndices == 0 && emptyIndices == 0;
869  int oi=0;
870  int si=0;
871  int ei=0;
872  int fi=0;
873  int gi=0;
874  float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
875 
876  for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
877  {
878  if(octree_->isNodeOccupied(*it) && (obstacleIndices != 0 || groundIndices != 0 || addAllPoints))
879  {
880  octomap::point3d pt = octree_->keyToCoord(it.getKey());
881  if(cloudProb)
882  {
883  (*cloudProb)[oi] = it->getOccupancy();
884  }
885  if(octree_->getTreeDepth() == it.getDepth() && hasColor_)
886  {
887  (*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
888  }
889  else
890  {
891  // Gradiant color on z axis
892  float H = (maxZ - pt.z())*299.0f/(maxZ-minZ);
893  float r,g,b;
894  util2d::HSVtoRGB(&r, &g, &b, H, 1, 1);
895  (*cloud)[oi].r = r*255.0f;
896  (*cloud)[oi].g = g*255.0f;
897  (*cloud)[oi].b = b*255.0f;
898  }
899 
900  if(originalRefPoints && it->getOccupancyType() > 0)
901  {
902  const octomap::point3d & p = it->getPointRef();
903  (*cloud)[oi].x = p.x();
904  (*cloud)[oi].y = p.y();
905  (*cloud)[oi].z = p.z();
906  }
907  else
908  {
909  (*cloud)[oi].x = pt.x()-halfCellSize;
910  (*cloud)[oi].y = pt.y()-halfCellSize;
911  (*cloud)[oi].z = pt.z();
912  }
913 
914  if(it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeGround)
915  {
916  if(groundIndices)
917  {
918  groundIndices->at(gi++) = oi;
919  }
920  }
921  else if(obstacleIndices)
922  {
923  obstacleIndices->at(si++) = oi;
924  }
925 
926  ++oi;
927  }
928  else if(!octree_->isNodeOccupied(*it) && (emptyIndices != 0 || addAllPoints || frontierIndices !=0))
929  {
930  octomap::point3d pt = octree_->keyToCoord(it.getKey());
931  if(cloudProb)
932  {
933  (*cloudProb)[oi] = it->getOccupancy();
934  }
935  if(frontierIndices !=0 &&
936  (!octree_->search( pt.x()+octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) || !octree_->search( pt.x()-octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) ||
937  !octree_->search( pt.x(), pt.y()+octree_->getNodeSize(treeDepth), pt.z(), treeDepth) || !octree_->search( pt.x(), pt.y()-octree_->getNodeSize(treeDepth), pt.z(), treeDepth) ||
938  !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 ?
939  {
940  //unknown neighbor FACE cell
941  frontierIndices->at(fi++) = oi;
942  }
943 
944 
945  (*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
946  (*cloud)[oi].x = pt.x()-halfCellSize;
947  (*cloud)[oi].y = pt.y()-halfCellSize;
948  (*cloud)[oi].z = pt.z();
949 
950  if(emptyIndices)
951  {
952  emptyIndices->at(ei++) = oi;
953  }
954 
955  ++oi;
956  }
957  }
958 
959  cloud->resize(oi);
960  if(cloudProb)
961  {
962  cloudProb->resize(oi);
963  }
964  if(obstacleIndices)
965  {
966  obstacleIndices->resize(si);
967  UDEBUG("obstacle=%d", si);
968  }
969  if(emptyIndices)
970  {
971  emptyIndices->resize(ei);
972  UDEBUG("empty=%d", ei);
973  }
974  if(frontierIndices)
975  {
976  frontierIndices->resize(fi);
977  UDEBUG("frontier=%d", fi);
978  }
979  if(groundIndices)
980  {
981  groundIndices->resize(gi);
982  UDEBUG("ground=%d", gi);
983  }
984 
985  UDEBUG("");
986  return cloud;
987 }
988 
989 cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCellSize, float minGridSize, unsigned int treeDepth)
990 {
991  UDEBUG("minGridSize=%f, treeDepth=%d", minGridSize, (int)treeDepth);
992  UASSERT(treeDepth <= octree_->getTreeDepth());
993  if(treeDepth == 0)
994  {
995  treeDepth = octree_->getTreeDepth();
996  }
997 
998  gridCellSize = octree_->getNodeSize(treeDepth);
999 
1000  cv::Mat obstaclesMat = cv::Mat(1, (int)octree_->size(), CV_32FC2);
1001  cv::Mat groundMat = cv::Mat(1, (int)octree_->size(), CV_32FC2);
1002  int gi=0;
1003  int oi=0;
1004  cv::Vec2f * oPtr = obstaclesMat.ptr<cv::Vec2f>(0,0);
1005  cv::Vec2f * gPtr = groundMat.ptr<cv::Vec2f>(0,0);
1006  float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
1007  for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
1008  {
1009  octomap::point3d pt = octree_->keyToCoord(it.getKey());
1010  if(octree_->isNodeOccupied(*it) &&
1011  it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeObstacle)
1012  {
1013  // projected on ground
1014  oPtr[oi][0] = pt.x()-halfCellSize;
1015  oPtr[oi][1] = pt.y()-halfCellSize;
1016  ++oi;
1017  }
1018  else
1019  {
1020  // projected on ground
1021  gPtr[gi][0] = pt.x()-halfCellSize;
1022  gPtr[gi][1] = pt.y()-halfCellSize;
1023  ++gi;
1024  }
1025  }
1026  obstaclesMat = obstaclesMat(cv::Range::all(), cv::Range(0, oi));
1027  groundMat = groundMat(cv::Range::all(), cv::Range(0, gi));
1028 
1029  std::map<int, Transform> poses;
1030  poses.insert(std::make_pair(1, Transform::getIdentity()));
1031  std::map<int, std::pair<cv::Mat, cv::Mat> > maps;
1032  maps.insert(std::make_pair(1, std::make_pair(groundMat, obstaclesMat)));
1033 
1034  cv::Mat map = util3d::create2DMapFromOccupancyLocalMaps(
1035  poses,
1036  maps,
1037  gridCellSize,
1038  xMin, yMin,
1039  minGridSize,
1040  false);
1041  UDEBUG("");
1042  return map;
1043 }
1044 
1045 bool OctoMap::writeBinary(const std::string & path)
1046 {
1047  return octree_->writeBinary(path);
1048 }
1049 
1050 } /* namespace rtabmap */
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[ *:*] noreverse nowriteback set trange[ *:*] noreverse nowriteback set urange[ *:*] noreverse nowriteback set vrange[ *:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
rtabmap::GlobalMap::logOddsHit_
float logOddsHit_
Definition: GlobalMap.h:87
rtabmap::GlobalMap::clear
virtual void clear()
Definition: GlobalMap.cpp:85
rtabmap::OctoMap::rangeMax_
float rangeMax_
Definition: global_map/OctoMap.h:220
rtabmap::util2d::HSVtoRGB
void RTABMAP_CORE_EXPORT HSVtoRGB(float *r, float *g, float *b, float h, float s, float v)
Definition: util2d.cpp:2047
rtabmap::RtabmapColorOcTreeNode::pruneNode
bool pruneNode()
Definition: OctoMap.cpp:63
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:133
Eigen::Transform
b
Array< int, 3, 1 > b
rtabmap::RtabmapColorOcTreeNode::expandNode
void expandNode()
Definition: OctoMap.cpp:84
rtabmap::GlobalMap::logOddsMiss_
float logOddsMiss_
Definition: GlobalMap.h:88
rtabmap::OctoMap::assemble
virtual void assemble(const std::list< std::pair< int, Transform > > &newPoses)
Definition: OctoMap.cpp:467
tree
rtabmap::RtabmapColorOcTreeNode::createChild
bool createChild(unsigned int i)
Definition: OctoMap.cpp:98
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
rtabmap::OctoMap::octree_
RtabmapColorOcTree * octree_
Definition: global_map/OctoMap.h:218
rtabmap::OctoMap::createProjectionMap
cv::Mat createProjectionMap(float &xMin, float &yMin, float &gridCellSize, float minGridSize=0.0f, unsigned int treeDepth=0)
Definition: OctoMap.cpp:989
rtabmap::OctoMap::isNodeVisited
static bool isNodeVisited(std::unordered_set< octomap::OcTreeKey, octomap::OcTreeKey::KeyHash > const &EmptyNodes, octomap::OcTreeKey const key)
Definition: OctoMap.cpp:416
rtabmap::RtabmapColorOcTreeNode::kTypeEmpty
@ kTypeEmpty
Definition: global_map/OctoMap.h:56
rtabmap::RtabmapColorOcTreeNode::kTypeObstacle
@ kTypeObstacle
Definition: global_map/OctoMap.h:56
end
end
rtabmap::OctoMap::emptyFloodFillDepth_
unsigned int emptyFloodFillDepth_
Definition: global_map/OctoMap.h:222
type
rtabmap::OctoMap::writeBinary
bool writeBinary(const std::string &path)
Definition: OctoMap.cpp:1045
rtabmap::GlobalMap::getMemoryUsed
virtual unsigned long getMemoryUsed() const
Definition: GlobalMap.cpp:93
y
Matrix3f y
rtabmap::LocalGridCache
Definition: LocalGrid.h:56
rtabmap::RtabmapColorOcTreeNode::getChild
RtabmapColorOcTreeNode * getChild(unsigned int i)
Definition: OctoMap.cpp:45
rtabmap::RtabmapColorOcTree::RtabmapColorOcTreeMemberInit
static StaticMemberInitializer RtabmapColorOcTreeMemberInit
static member to ensure static initialization (only once)
Definition: global_map/OctoMap.h:171
rtabmap::RtabmapColorOcTreeNode::kTypeUnknown
@ kTypeUnknown
Definition: global_map/OctoMap.h:56
rtabmap::LaserScan
Definition: LaserScan.h:37
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:825
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UTimer.h
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:131
rtabmap::OctoMap::~OctoMap
virtual ~OctoMap()
Definition: OctoMap.cpp:330
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
rtabmap::RtabmapColorOcTree::StaticMemberInitializer::StaticMemberInitializer
StaticMemberInitializer()
Definition: OctoMap.cpp:271
n
int n
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
point
point
UFATAL
#define UFATAL(...)
rtabmap::RtabmapColorOcTree::updateInnerOccupancy
void updateInnerOccupancy()
Definition: OctoMap.cpp:233
util3d_transforms.h
g
float g
rtabmap::GlobalMap::logOddsClampingMin_
float logOddsClampingMin_
Definition: GlobalMap.h:89
rtabmap::OctoMap::isValidEmpty
static bool isValidEmpty(RtabmapColorOcTree *octree_, unsigned int treeDepth, octomap::point3d startPosition)
Definition: OctoMap.cpp:352
rtabmap::OctoMap::clear
virtual void clear()
Definition: OctoMap.cpp:336
rtabmap::GlobalMap
Definition: GlobalMap.h:40
rtabmap::RtabmapColorOcTreeNode::RtabmapColorOcTreeNode
RtabmapColorOcTreeNode()
Definition: global_map/OctoMap.h:61
rtabmap::util3d::laserScanToPoint
pcl::PointXYZ RTABMAP_CORE_EXPORT laserScanToPoint(const LaserScan &laserScan, int index)
Definition: util3d.cpp:2428
rtabmap::util3d::create2DMapFromOccupancyLocalMaps
cv::Mat RTABMAP_CORE_EXPORT 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
util3d_filtering.h
rtabmap::OctoMap::findEmptyNode
static std::unordered_set< octomap::OcTreeKey, octomap::OcTreeKey::KeyHash > findEmptyNode(RtabmapColorOcTree *octree_, unsigned int treeDepth, octomap::point3d startPosition)
Definition: OctoMap.cpp:449
all
static const Eigen::internal::all_t all
rtabmap::OctoMap::rayTracing_
bool rayTracing_
Definition: global_map/OctoMap.h:221
rtabmap::GlobalMap::addAssembledNode
void addAssembledNode(int id, const Transform &pose)
Definition: GlobalMap.cpp:160
rtabmap::RtabmapColorOcTree::isNodeCollapsible
virtual bool isNodeCollapsible(const RtabmapColorOcTreeNode *node) const
Definition: OctoMap.cpp:167
rtabmap::GlobalMap::minValues_
double minValues_[3]
Definition: GlobalMap.h:92
rtabmap::RtabmapColorOcTree::updateInnerOccupancyRecurs
void updateInnerOccupancyRecurs(RtabmapColorOcTreeNode *node, unsigned int depth)
Definition: OctoMap.cpp:237
rtabmap::RtabmapColorOcTree::integrateNodeColor
RtabmapColorOcTreeNode * integrateNodeColor(const octomap::OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: OctoMap.cpp:208
rtabmap::GlobalMap::occupancyThr_
float occupancyThr_
Definition: GlobalMap.h:86
rtabmap::OctoMap::updateMinMax
void updateMinMax(const octomap::point3d &point)
Definition: OctoMap.cpp:796
UASSERT
#define UASSERT(condition)
p
Point3_ p(2)
rtabmap::RtabmapColorOcTreeNode::kTypeGround
@ kTypeGround
Definition: global_map/OctoMap.h:56
rtabmap::Parameters
Definition: Parameters.h:170
util3d_mapping.h
rtabmap::OctoMap::hasColor_
bool hasColor_
Definition: global_map/OctoMap.h:219
rtabmap::util3d::laserScanToPointRGB
pcl::PointXYZRGB RTABMAP_CORE_EXPORT laserScanToPointRGB(const LaserScan &laserScan, int index, unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2465
path
path
key
const gtsam::Symbol key( 'X', 0)
glm::uint8_t
detail::uint8 uint8_t
Definition: fwd.hpp:908
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::RtabmapColorOcTreeNode::updateOccupancyTypeChildren
void updateOccupancyTypeChildren()
Definition: OctoMap.cpp:109
rtabmap::RtabmapColorOcTree
Definition: global_map/OctoMap.h:87
ULogger.h
ULogger class and convenient macros.
rtabmap::RtabmapColorOcTreeNode::type_
int type_
Definition: global_map/OctoMap.h:82
rtabmap::OctoMap::getMemoryUsed
virtual unsigned long getMemoryUsed() const
Definition: OctoMap.cpp:343
rtabmap::GlobalMap::probability
static double probability(double logodds)
Definition: GlobalMap.h:48
rtabmap::RtabmapColorOcTree::StaticMemberInitializer::ensureLinking
void ensureLinking()
Definition: global_map/OctoMap.h:168
util2d.h
rtabmap::RtabmapColorOcTree::RtabmapColorOcTree
RtabmapColorOcTree(double resolution)
Default constructor, sets resolution of leafs.
Definition: OctoMap.cpp:126
rtabmap::RtabmapColorOcTree::setNodeColor
RtabmapColorOcTreeNode * setNodeColor(const octomap::OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: OctoMap.cpp:131
uint8_t
::uint8_t uint8_t
OctoMap.h
iter
iterator iter(handle obj)
rtabmap::RtabmapColorOcTreeNode::getOccupancyType
int getOccupancyType() const
Definition: global_map/OctoMap.h:68
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
v
Array< int, Dynamic, 1 > v
rtabmap::GlobalMap::maxValues_
double maxValues_[3]
Definition: GlobalMap.h:93
UDEBUG
#define UDEBUG(...)
UTimer
Definition: UTimer.h:46
rtabmap::GlobalMap::cache
const std::map< int, LocalGrid > & cache() const
Definition: GlobalMap.h:76
rtabmap::RtabmapColorOcTreeNode
Definition: global_map/OctoMap.h:53
rtabmap::GlobalMap::logOddsClampingMax_
float logOddsClampingMax_
Definition: GlobalMap.h:90
rtabmap::RtabmapColorOcTree::pruneNode
virtual bool pruneNode(RtabmapColorOcTreeNode *node)
Definition: OctoMap.cpp:142
rtabmap::GlobalMap::cellSize_
float cellSize_
Definition: GlobalMap.h:83
NULL
#define NULL
false
#define false
Definition: ConvertUTF.c:56
root
root
pos
t
Point2 t(10, 10)
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::RtabmapColorOcTree::StaticMemberInitializer
Definition: global_map/OctoMap.h:159
UERROR
#define UERROR(...)
rtabmap::GlobalMap::assembledNodes
const std::map< int, Transform > & assembledNodes() const
Definition: GlobalMap.h:78
rtabmap::OctoMap::OctoMap
OctoMap(const LocalGridCache *cache, const ParametersMap &parameters=ParametersMap())
Definition: OctoMap.cpp:291
value
value
i
int i
rtabmap::RtabmapColorOcTree::averageNodeColor
RtabmapColorOcTreeNode * averageNodeColor(const octomap::OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: OctoMap.cpp:191
rtabmap::OctoMap::findCloseEmpty
static octomap::point3d findCloseEmpty(RtabmapColorOcTree *octree_, unsigned int treeDepth, octomap::point3d startPosition)
Definition: OctoMap.cpp:366
rtabmap::OctoMap::floodFill
static void floodFill(RtabmapColorOcTree *octree_, unsigned int treeDepth, octomap::point3d startPosition, std::unordered_set< octomap::OcTreeKey, octomap::OcTreeKey::KeyHash > &EmptyNodes, std::queue< octomap::point3d > &positionToExplore)
Definition: OctoMap.cpp:429

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:13