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 
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 ParametersMap & parameters) :
292  hasColor_(false),
293  fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
294  updateError_(Parameters::defaultGridGlobalUpdateError()),
295  rangeMax_(Parameters::defaultGridRangeMax()),
296  rayTracing_(Parameters::defaultGridRayTracing()),
297  emptyFloodFillDepth_(Parameters::defaultGridGlobalFloodFillDepth())
298 {
299  float cellSize = Parameters::defaultGridCellSize();
300  Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
301  UASSERT(cellSize>0.0f);
302 
303  minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
304  maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
305 
306  float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
307  float probHit = Parameters::defaultGridGlobalProbHit();
308  float probMiss = Parameters::defaultGridGlobalProbMiss();
309  float clampingMin = Parameters::defaultGridGlobalProbClampingMin();
310  float clampingMax = Parameters::defaultGridGlobalProbClampingMax();
311  Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr);
312  Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit);
313  Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss);
314  Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), clampingMin);
315  Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), clampingMax);
316 
317  octree_ = new RtabmapColorOcTree(cellSize);
318  if(occupancyThr <= 0.0f)
319  {
320  UWARN("Cannot set %s to null for OctoMap, using default value %f instead.",
321  Parameters::kGridGlobalOccupancyThr().c_str(),
322  Parameters::defaultGridGlobalOccupancyThr());
323  occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
324  }
325  octree_->setOccupancyThres(occupancyThr);
326  octree_->setProbHit(probHit);
327  octree_->setProbMiss(probMiss);
328  octree_->setClampingThresMin(clampingMin);
329  octree_->setClampingThresMax(clampingMax);
330  Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
331 
332  Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
333  Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);
334  Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
335  Parameters::parse(parameters, Parameters::kGridGlobalFloodFillDepth(), emptyFloodFillDepth_);
336  UASSERT(emptyFloodFillDepth_>=0 && emptyFloodFillDepth_<=16);
337 
338  UDEBUG("fullUpdate_ =%s", fullUpdate_?"true":"false");
339  UDEBUG("updateError_ =%f", updateError_);
340  UDEBUG("rangeMax_ =%f", rangeMax_);
341  UDEBUG("rayTracing_ =%s", rayTracing_?"true":"false");
342  UDEBUG("emptyFloodFillDepth_=%d", emptyFloodFillDepth_);
343 }
344 
345 OctoMap::~OctoMap()
346 {
347  this->clear();
348  delete octree_;
349 }
350 
351 void OctoMap::clear()
352 {
353  octree_->clear();
354  cache_.clear();
355  cacheClouds_.clear();
356  cacheViewPoints_.clear();
357  addedNodes_.clear();
358  hasColor_ = false;
359  minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
360  maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
361 }
362 
363 void OctoMap::addToCache(int nodeId,
364  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
365  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
366  const pcl::PointXYZ & viewPoint)
367 {
368  UDEBUG("nodeId=%d", nodeId);
369  if(nodeId < 0)
370  {
371  UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
372  return;
373  }
374  cacheClouds_.erase(nodeId==0?-1:nodeId);
375  cacheClouds_.insert(std::make_pair(nodeId==0?-1:nodeId, std::make_pair(ground, obstacles)));
376  uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
377 }
378 void OctoMap::addToCache(int nodeId,
379  const cv::Mat & ground,
380  const cv::Mat & obstacles,
381  const cv::Mat & empty,
382  const cv::Point3f & viewPoint)
383 {
384  UDEBUG("nodeId=%d", nodeId);
385  if(nodeId < 0)
386  {
387  UWARN("Cannot add nodes with negative id (nodeId=%d)", nodeId);
388  return;
389  }
390  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());
391  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());
392  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());
393  uInsert(cache_, std::make_pair(nodeId==0?-1:nodeId, std::make_pair(std::make_pair(ground, obstacles), empty)));
394  uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, viewPoint));
395 }
396 
397 bool OctoMap::isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition)
398 {
399  auto nodePtr = octree_->search(startPosition.x(), startPosition.y(), startPosition.z(), treeDepth);
400  if(nodePtr != NULL)
401  {
402  if(!octree_->isNodeOccupied(*nodePtr))
403  {
404  return true;
405  }
406  }
407 
408  return false;
409 }
410 
411 octomap::point3d OctoMap::findCloseEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition)
412 {
413 
414  //try current position
415  if(isValidEmpty(octree_,treeDepth,startPosition))
416  {
417  return startPosition;
418  }
419 
420  //x pos
421  if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x()+octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z())))
422  {
423  return octomap::point3d(startPosition.x()+octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z());
424  }
425 
426  //x neg
427  if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z())))
428  {
429  return octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z());
430  }
431 
432  //y pos
433  if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y()+octree_->getNodeSize(treeDepth), startPosition.z())))
434  {
435  return octomap::point3d(startPosition.x(), startPosition.y()+octree_->getNodeSize(treeDepth), startPosition.z());
436  }
437 
438 
439  //y neg
440  if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z())))
441  {
442  return octomap::point3d(startPosition.x(), startPosition.y()-octree_->getNodeSize(treeDepth), startPosition.z());
443  }
444 
445  //z pos
446  if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()+octree_->getNodeSize(treeDepth))))
447  {
448  return octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()+octree_->getNodeSize(treeDepth));
449  }
450 
451  //z neg
452  if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()-octree_->getNodeSize(treeDepth))))
453  {
454  return octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()-octree_->getNodeSize(treeDepth));
455  }
456 
457  //no valid position
458  return startPosition;
459 }
460 
461 bool OctoMap::isNodeVisited(std::unordered_set<octomap::OcTreeKey,octomap::OcTreeKey::KeyHash> const & EmptyNodes,octomap::OcTreeKey const key)
462 {
463  for(auto it = EmptyNodes.find(key);it != EmptyNodes.end();it++)
464  {
465  if(*it == key)
466  {
467  return true;
468  }
469 
470  }
471  return false;
472 }
473 
474 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)
475 {
476 
477  auto key = octree_->coordToKey(startPosition,treeDepth);
478  if(!isNodeVisited(EmptyNodes,key))
479  {
480  if(isValidEmpty(octree_,treeDepth,startPosition))
481  {
482  EmptyNodes.insert(key);
483  positionToExplore.push(octomap::point3d(startPosition.x()+octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z()));
484  positionToExplore.push(octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z()));
485  positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y()+octree_->getNodeSize(treeDepth), startPosition.z()));
486  positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y()-octree_->getNodeSize(treeDepth), startPosition.z()));
487  positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()+octree_->getNodeSize(treeDepth)));
488  positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()-octree_->getNodeSize(treeDepth)));
489  }
490  }
491 }
492 
493 
494 std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> OctoMap::findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition)
495 {
496  std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> exploreNode;
497  std::queue<octomap::point3d> positionToExplore;
498 
499  startPosition = findCloseEmpty(octree_,treeDepth, startPosition);
500 
501  floodFill(octree_, treeDepth, startPosition, exploreNode, positionToExplore);
502  while(!positionToExplore.empty())
503  {
504  floodFill(octree_, treeDepth, positionToExplore.front(), exploreNode, positionToExplore);
505  positionToExplore.pop();
506  }
507 
508  return exploreNode;
509 }
510 
511 
512 bool OctoMap::update(const std::map<int, Transform> & poses)
513 {
514  UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
515 
516  // First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
517  bool graphOptimized = false; // If a loop closure happened (e.g., poses are modified)
518  bool graphChanged = addedNodes_.size()>0; // If the new map doesn't have any node from the previous map
519  std::map<int, Transform> transforms;
520  std::map<int, Transform> updatedAddedNodes;
521  float updateErrorSqrd = updateError_*updateError_;
522  for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
523  {
524  std::map<int, Transform>::const_iterator jter = poses.find(iter->first);
525  if(jter != poses.end())
526  {
527  graphChanged = false;
528  UASSERT(!iter->second.isNull() && !jter->second.isNull());
529  Transform t = Transform::getIdentity();
530  if(iter->second.getDistanceSquared(jter->second) > updateErrorSqrd)
531  {
532  t = jter->second * iter->second.inverse();
533  graphOptimized = true;
534  }
535  transforms.insert(std::make_pair(jter->first, t));
536  updatedAddedNodes.insert(std::make_pair(jter->first, jter->second));
537  }
538  else
539  {
540  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);
541  }
542  }
543 
544  if(graphOptimized || graphChanged)
545  {
546  if(graphChanged)
547  {
548  UWARN("Graph has changed! The whole map should be rebuilt.");
549  }
550  else
551  {
552  UINFO("Graph optimized!");
553  }
554 
555  minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
556  maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
557 
558  if(fullUpdate_ || graphChanged)
559  {
560  // clear all but keep cache
561  octree_->clear();
562  addedNodes_.clear();
563  hasColor_ = false;
564  }
565  else
566  {
567  RtabmapColorOcTree * newOcTree = new RtabmapColorOcTree(octree_->getResolution());
568  int copied=0;
569  int count=0;
570  UTimer t;
571  for (RtabmapColorOcTree::iterator it = octree_->begin(); it != octree_->end(); ++it, ++count)
572  {
573  RtabmapColorOcTreeNode & nOld = *it;
574  if(nOld.getNodeRefId() > 0)
575  {
576  std::map<int, Transform>::iterator jter = transforms.find(nOld.getNodeRefId());
577  if(jter != transforms.end())
578  {
579  octomap::point3d pt;
580  std::map<int, Transform>::iterator pter = addedNodes_.find(nOld.getNodeRefId());
581  UASSERT(pter != addedNodes_.end());
582 
583  if(nOld.getOccupancyType() > 0)
584  {
585  pt = nOld.getPointRef();
586  }
587  else
588  {
589  pt = octree_->keyToCoord(it.getKey());
590  }
591 
592  cv::Point3f cvPt(pt.x(), pt.y(), pt.z());
593  cvPt = util3d::transformPoint(cvPt, jter->second);
594  octomap::point3d ptTransformed(cvPt.x, cvPt.y, cvPt.z);
595 
596  octomap::OcTreeKey key;
597  if(newOcTree->coordToKeyChecked(ptTransformed, key))
598  {
599  RtabmapColorOcTreeNode * n = newOcTree->search(key);
600  if(n)
601  {
602  if(n->getNodeRefId() > nOld.getNodeRefId())
603  {
604  // The cell has been updated from more recent node, don't update the cell
605  continue;
606  }
607  else if(nOld.getOccupancyType() <= 0 && n->getOccupancyType() > 0)
608  {
609  // empty cells cannot overwrite ground/obstacle cells
610  continue;
611  }
612  }
613 
614  RtabmapColorOcTreeNode * nNew = newOcTree->updateNode(key, nOld.getLogOdds());
615  if(nNew)
616  {
617  ++copied;
618  updateMinMax(ptTransformed);
619  nNew->setNodeRefId(nOld.getNodeRefId());
620  if(nOld.getOccupancyType() > 0)
621  {
622  nNew->setPointRef(pt);
623  }
624  nNew->setOccupancyType(nOld.getOccupancyType());
625  nNew->setColor(nOld.getColor());
626  }
627  else
628  {
629  UERROR("Could not update node at (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
630  }
631  }
632  else
633  {
634  UERROR("Could not find key for (%f,%f,%f)", cvPt.x, cvPt.y, cvPt.z);
635  }
636  }
637  else if(jter == transforms.end())
638  {
639  // Note: normal if old nodes were transfered to LTM
640  //UWARN("Could not find a transform for point linked to node %d (transforms=%d)", iter->second.nodeRefId_, (int)transforms.size());
641  }
642  }
643  }
644  UINFO("Graph optimization detected, moved %d/%d in %fs", copied, count, t.ticks());
645  delete octree_;
646  octree_ = newOcTree;
647 
648  //update added poses
649  addedNodes_ = updatedAddedNodes;
650  }
651  }
652 
653  // Original version from A. Hornung:
654  // https://github.com/OctoMap/octomap_mapping/blob/jade-devel/octomap_server/src/OctomapServer.cpp#L356
655  //
656  std::list<std::pair<int, Transform> > orderedPoses;
657 
658  int lastId = addedNodes_.size()?addedNodes_.rbegin()->first:0;
659  UDEBUG("Last id = %d", lastId);
660 
661  // add old poses that were not in the current map (they were just retrieved from LTM)
662  for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter)
663  {
664  if(addedNodes_.find(iter->first) == addedNodes_.end())
665  {
666  orderedPoses.push_back(*iter);
667  }
668  }
669 
670  // insert zero after
671  if(poses.find(0) != poses.end())
672  {
673  orderedPoses.push_back(std::make_pair(-1, poses.at(0)));
674  }
675 
676  UDEBUG("orderedPoses = %d", (int)orderedPoses.size());
677 
678  if(!orderedPoses.empty())
679  {
680  float rangeMaxSqrd = rangeMax_*rangeMax_;
681  float cellSize = octree_->getResolution();
682  for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
683  {
684  std::map<int, std::pair<const pcl::PointCloud<pcl::PointXYZRGB>::Ptr, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
685  std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator occupancyIter;
686  std::map<int, cv::Point3f>::iterator viewPointIter;
687  cloudIter = cacheClouds_.find(iter->first);
688  occupancyIter = cache_.find(iter->first);
689  viewPointIter = cacheViewPoints_.find(iter->first);
690  if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
691  {
692  UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
693 
694  UASSERT(viewPointIter != cacheViewPoints_.end());
695  octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
696  sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
697 
698  updateMinMax(sensorOrigin);
699 
700  octomap::OcTreeKey tmpKey;
701  if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
702  || !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
703  {
704  UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
705  }
706 
707  bool computeRays = rayTracing_ && (occupancyIter == cache_.end() || occupancyIter->second.second.empty());
708 
709  // instead of direct scan insertion, compute update to filter ground:
710  octomap::KeySet free_cells;
711  // insert ground points only as free:
712  unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.first.cols:cloudIter->second.first->size();
713  UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
714  Eigen::Affine3f t = iter->second.toEigen3f();
715  LaserScan tmpGround;
716  if(occupancyIter != cache_.end())
717  {
718  tmpGround = LaserScan::backwardCompatibility(occupancyIter->second.first.first);
719  UASSERT(tmpGround.size() == (int)maxGroundPts);
720  }
721  for (unsigned int i=0; i<maxGroundPts; ++i)
722  {
723  pcl::PointXYZRGB pt;
724  if(occupancyIter != cache_.end())
725  {
726  pt = util3d::laserScanToPointRGB(tmpGround, i);
727  pt = pcl::transformPoint(pt, t);
728  }
729  else
730  {
731  pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
732  }
733  octomap::point3d point(pt.x, pt.y, pt.z);
734  bool ignoreOccupiedCell = false;
735  if(rangeMaxSqrd > 0.0f)
736  {
737  octomap::point3d v(pt.x - cellSize - sensorOrigin.x(), pt.y - cellSize - sensorOrigin.y(), pt.z - cellSize - sensorOrigin.z());
738  if(v.norm_sq() > rangeMaxSqrd)
739  {
740  // compute new point to max range
741  v.normalize();
742  v*=rangeMax_;
743  point = sensorOrigin + v;
744  ignoreOccupiedCell=true;
745  }
746  }
747 
748  if(!ignoreOccupiedCell)
749  {
750  // occupied endpoint
751  octomap::OcTreeKey key;
752  if (octree_->coordToKeyChecked(point, key))
753  {
754  if(iter->first >0 && iter->first<lastId)
755  {
756  RtabmapColorOcTreeNode * n = octree_->search(key);
757  if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
758  {
759  // The cell has been updated from more recent node, don't update the cell
760  continue;
761  }
762  }
763 
764  updateMinMax(point);
765  RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
766 
767  if(n)
768  {
769  if(!hasColor_ && !(pt.r ==0 && pt.g == 0 && pt.b == 0) && !(pt.r ==255 && pt.g == 255 && pt.b == 255))
770  {
771  hasColor_ = true;
772  }
773  octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
774  if(iter->first > 0)
775  {
776  n->setNodeRefId(iter->first);
777  n->setPointRef(point);
778  }
779  n->setOccupancyType(RtabmapColorOcTreeNode::kTypeGround);
780  }
781  }
782  }
783 
784  // only clear space (ground points)
785  octomap::KeyRay keyRay;
786  if (computeRays &&
787  (iter->first < 0 || iter->first>lastId) &&
788  octree_->computeRayKeys(sensorOrigin, point, keyRay))
789  {
790  free_cells.insert(keyRay.begin(), keyRay.end());
791  }
792  }
793  UDEBUG("%d: ground cells=%d free cells=%d", iter->first, (int)maxGroundPts, (int)free_cells.size());
794 
795  // all other points: free on ray, occupied on endpoint:
796  unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.first.second.cols:cloudIter->second.second->size();
797  UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
798  LaserScan tmpObstacle;
799  if(occupancyIter != cache_.end())
800  {
801  tmpObstacle = LaserScan::backwardCompatibility(occupancyIter->second.first.second);
802  UASSERT(tmpObstacle.size() == (int)maxObstaclePts);
803  }
804  for (unsigned int i=0; i<maxObstaclePts; ++i)
805  {
806  pcl::PointXYZRGB pt;
807  if(occupancyIter != cache_.end())
808  {
809  pt = util3d::laserScanToPointRGB(tmpObstacle, i);
810  pt = pcl::transformPoint(pt, t);
811  }
812  else
813  {
814  pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
815  }
816 
817  octomap::point3d point(pt.x, pt.y, pt.z);
818 
819  bool ignoreOccupiedCell = false;
820  if(rangeMaxSqrd > 0.0f)
821  {
822  octomap::point3d v(pt.x - cellSize - sensorOrigin.x(), pt.y - cellSize - sensorOrigin.y(), pt.z - cellSize - sensorOrigin.z());
823  if(v.norm_sq() > rangeMaxSqrd)
824  {
825  // compute new point to max range
826  v.normalize();
827  v*=rangeMax_;
828  point = sensorOrigin + v;
829  ignoreOccupiedCell=true;
830  }
831  }
832 
833  if(!ignoreOccupiedCell)
834  {
835  // occupied endpoint
836  octomap::OcTreeKey key;
837  if (octree_->coordToKeyChecked(point, key))
838  {
839  if(iter->first >0 && iter->first<lastId)
840  {
841  RtabmapColorOcTreeNode * n = octree_->search(key);
842  if(n && n->getNodeRefId() > 0 && n->getNodeRefId() > iter->first)
843  {
844  // The cell has been updated from more recent node, don't update the cell
845  continue;
846  }
847  }
848 
849  updateMinMax(point);
850 
851  RtabmapColorOcTreeNode * n = octree_->updateNode(key, true);
852  if(n)
853  {
854  if(!hasColor_ && !(pt.r ==0 && pt.g == 0 && pt.b == 0) && !(pt.r ==255 && pt.g == 255 && pt.b == 255))
855  {
856  hasColor_ = true;
857  }
858  octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
859  if(iter->first > 0)
860  {
861  n->setNodeRefId(iter->first);
862  n->setPointRef(point);
863  }
864  n->setOccupancyType(RtabmapColorOcTreeNode::kTypeObstacle);
865  }
866  }
867  }
868 
869  // free cells
870  octomap::KeyRay keyRay;
871  if (computeRays &&
872  (iter->first < 0 || iter->first>lastId) &&
873  octree_->computeRayKeys(sensorOrigin, point, keyRay))
874  {
875  free_cells.insert(keyRay.begin(), keyRay.end());
876  }
877  }
878  UDEBUG("%d: occupied cells=%d free cells=%d", iter->first, (int)maxObstaclePts, (int)free_cells.size());
879 
880 
881  // mark free cells only if not seen occupied in this cloud
882  for(octomap::KeySet::iterator it = free_cells.begin(), end=free_cells.end(); it!= end; ++it)
883  {
884  if(iter->first > 0)
885  {
886  RtabmapColorOcTreeNode * n = octree_->search(*it);
887  if(n && n->getNodeRefId() > 0 && n->getNodeRefId() >= iter->first)
888  {
889  // The cell has been updated from current node or more recent node, don't update the cell
890  continue;
891  }
892  }
893 
894  RtabmapColorOcTreeNode * n = octree_->updateNode(*it, false, true);
895  if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
896  {
897  n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
898  if(iter->first > 0)
899  {
900  n->setNodeRefId(iter->first);
901  }
902  }
903  }
904 
905  // all empty cells
906  if(occupancyIter != cache_.end() && occupancyIter->second.second.cols)
907  {
908  unsigned int maxEmptyPts = occupancyIter->second.second.cols;
909  UDEBUG("%d: compute free cells (from %d empty points)", iter->first, (int)maxEmptyPts);
910  LaserScan tmpEmpty = LaserScan::backwardCompatibility(occupancyIter->second.second);
911  UASSERT(tmpEmpty.size() == (int)maxEmptyPts);
912  for (unsigned int i=0; i<maxEmptyPts; ++i)
913  {
914  pcl::PointXYZ pt;
915  pt = util3d::laserScanToPoint(tmpEmpty, i);
916  pt = pcl::transformPoint(pt, t);
917 
918  octomap::point3d point(pt.x, pt.y, pt.z);
919 
920  bool ignoreCell = false;
921  if(rangeMaxSqrd > 0.0f)
922  {
923  octomap::point3d v(pt.x - sensorOrigin.x(), pt.y - sensorOrigin.y(), pt.z - sensorOrigin.z());
924  if(v.norm_sq() > rangeMaxSqrd)
925  {
926  ignoreCell=true;
927  }
928  }
929 
930  if(!ignoreCell)
931  {
932  octomap::OcTreeKey key;
933  if (octree_->coordToKeyChecked(point, key))
934  {
935 
936  if(iter->first >0)
937  {
938  RtabmapColorOcTreeNode * n = octree_->search(key);
939  if(n && n->getNodeRefId() > 0 && n->getNodeRefId() >= iter->first)
940  {
941  // The cell has been updated from current node or more recent node, don't update the cell
942  continue;
943  }
944  }
945 
946  updateMinMax(point);
947 
948  RtabmapColorOcTreeNode * n = octree_->updateNode(key, false, true);
949  if(n && n->getOccupancyType() == RtabmapColorOcTreeNode::kTypeUnknown)
950  {
951  n->setOccupancyType(RtabmapColorOcTreeNode::kTypeEmpty);
952  if(iter->first > 0)
953  {
954  n->setNodeRefId(iter->first);
955  }
956  }
957  }
958  }
959  }
960  }
961 
962  if((occupancyIter != cache_.end() && occupancyIter->second.second.cols) || !free_cells.empty())
963  {
964  octree_->updateInnerOccupancy();
965  }
966 
967  // compress map
968  //if(orderedPoses.size() > 1)
969  //{
970  // octree_->prune();
971  //}
972 
973  // ignore negative ids as they are temporary clouds
974  if(iter->first > 0)
975  {
976  addedNodes_.insert(*iter);
977  }
978  UDEBUG("%d: end", iter->first);
979  }
980  else
981  {
982  UDEBUG("Did not find %d in cache", iter->first);
983  }
984  }
985  }
986 
987  if(emptyFloodFillDepth_>0)
988  {
989  UTimer t;
990 
991  auto key = octree_->coordToKey(0, 0, 0, emptyFloodFillDepth_);
992  auto pos = octree_->keyToCoord(key);
993 
994  std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> EmptyNodes = findEmptyNode(octree_,emptyFloodFillDepth_, pos);
995  std::vector<octomap::OcTreeKey> nodeToDelete;
996 
997  for (RtabmapColorOcTree::iterator it = octree_->begin_leafs(emptyFloodFillDepth_); it != octree_->end_leafs(); ++it)
998  {
999  if(!octree_->isNodeOccupied(*it))
1000  {
1001  if(!isNodeVisited(EmptyNodes,it.getKey()))
1002  {
1003  nodeToDelete.push_back(it.getKey());
1004  }
1005  }
1006  }
1007 
1008 
1009  for(unsigned int y=0; y < nodeToDelete.size(); y++)
1010  {
1011  octree_->deleteNode(nodeToDelete[y],emptyFloodFillDepth_);
1012  }
1013  UDEBUG("Flood Fill: deleted %d empty cells (%fs)", (int)nodeToDelete.size(), t.ticks());
1014  }
1015 
1016  if(!fullUpdate_)
1017  {
1018  cache_.clear();
1019  cacheClouds_.clear();
1020  cacheViewPoints_.clear();
1021  }
1022  return !orderedPoses.empty() || graphOptimized || graphChanged || emptyFloodFillDepth_>0;
1023 }
1024 
1025 void OctoMap::updateMinMax(const octomap::point3d & point)
1026 {
1027  if(point.x() < minValues_[0])
1028  {
1029  minValues_[0] = point.x();
1030  }
1031  if(point.y() < minValues_[1])
1032  {
1033  minValues_[1] = point.y();
1034  }
1035  if(point.z() < minValues_[2])
1036  {
1037  minValues_[2] = point.z();
1038  }
1039  if(point.x() > maxValues_[0])
1040  {
1041  maxValues_[0] = point.x();
1042  }
1043  if(point.y() > maxValues_[1])
1044  {
1045  maxValues_[1] = point.y();
1046  }
1047  if(point.z() > maxValues_[2])
1048  {
1049  maxValues_[2] = point.z();
1050  }
1051 }
1052 
1053 
1054 pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
1055  unsigned int treeDepth,
1056  std::vector<int> * obstacleIndices,
1057  std::vector<int> * emptyIndices,
1058  std::vector<int> * groundIndices,
1059  bool originalRefPoints,
1060  std::vector<int> * frontierIndices,
1061  std::vector<double> * cloudProb) const
1062 {
1063  UASSERT(treeDepth <= octree_->getTreeDepth());
1064  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
1065  if(cloudProb)
1066  {
1067  cloudProb->resize(octree_->size());
1068  }
1069  UDEBUG("depth=%d (maxDepth=%d) octree = %d",
1070  (int)treeDepth, (int)octree_->getTreeDepth(), (int)octree_->size());
1071  cloud->resize(octree_->size());
1072  if(obstacleIndices)
1073  {
1074  obstacleIndices->resize(octree_->size());
1075  }
1076  if(emptyIndices)
1077  {
1078  emptyIndices->resize(octree_->size());
1079  }
1080  if(frontierIndices)
1081  {
1082  frontierIndices->resize(octree_->size());
1083  }
1084  if(groundIndices)
1085  {
1086  groundIndices->resize(octree_->size());
1087  }
1088 
1089  if(treeDepth == 0)
1090  {
1091  treeDepth = octree_->getTreeDepth();
1092  }
1093 
1094  double minZ = minValues_[2];
1095  double maxZ = maxValues_[2];
1096 
1097  bool addAllPoints = obstacleIndices == 0 && groundIndices == 0 && emptyIndices == 0;
1098  int oi=0;
1099  int si=0;
1100  int ei=0;
1101  int fi=0;
1102  int gi=0;
1103  float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
1104 
1105  for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
1106  {
1107  if(octree_->isNodeOccupied(*it) && (obstacleIndices != 0 || groundIndices != 0 || addAllPoints))
1108  {
1109  octomap::point3d pt = octree_->keyToCoord(it.getKey());
1110  if(cloudProb)
1111  {
1112  (*cloudProb)[oi] = it->getOccupancy();
1113  }
1114  if(octree_->getTreeDepth() == it.getDepth() && hasColor_)
1115  {
1116  (*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
1117  }
1118  else
1119  {
1120  // Gradiant color on z axis
1121  float H = (maxZ - pt.z())*299.0f/(maxZ-minZ);
1122  float r,g,b;
1123  util2d::HSVtoRGB(&r, &g, &b, H, 1, 1);
1124  (*cloud)[oi].r = r*255.0f;
1125  (*cloud)[oi].g = g*255.0f;
1126  (*cloud)[oi].b = b*255.0f;
1127  }
1128 
1129  if(originalRefPoints && it->getOccupancyType() > 0)
1130  {
1131  const octomap::point3d & p = it->getPointRef();
1132  (*cloud)[oi].x = p.x();
1133  (*cloud)[oi].y = p.y();
1134  (*cloud)[oi].z = p.z();
1135  }
1136  else
1137  {
1138  (*cloud)[oi].x = pt.x()-halfCellSize;
1139  (*cloud)[oi].y = pt.y()-halfCellSize;
1140  (*cloud)[oi].z = pt.z();
1141  }
1142 
1143  if(it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeGround)
1144  {
1145  if(groundIndices)
1146  {
1147  groundIndices->at(gi++) = oi;
1148  }
1149  }
1150  else if(obstacleIndices)
1151  {
1152  obstacleIndices->at(si++) = oi;
1153  }
1154 
1155  ++oi;
1156  }
1157  else if(!octree_->isNodeOccupied(*it) && (emptyIndices != 0 || addAllPoints || frontierIndices !=0))
1158  {
1159  octomap::point3d pt = octree_->keyToCoord(it.getKey());
1160  if(cloudProb)
1161  {
1162  (*cloudProb)[oi] = it->getOccupancy();
1163  }
1164  if(frontierIndices !=0 &&
1165  (!octree_->search( pt.x()+octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) || !octree_->search( pt.x()-octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) ||
1166  !octree_->search( pt.x(), pt.y()+octree_->getNodeSize(treeDepth), pt.z(), treeDepth) || !octree_->search( pt.x(), pt.y()-octree_->getNodeSize(treeDepth), pt.z(), treeDepth) ||
1167  !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 ?
1168  {
1169  //unknown neighbor FACE cell
1170  frontierIndices->at(fi++) = oi;
1171  }
1172 
1173 
1174  (*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
1175  (*cloud)[oi].x = pt.x()-halfCellSize;
1176  (*cloud)[oi].y = pt.y()-halfCellSize;
1177  (*cloud)[oi].z = pt.z();
1178 
1179  if(emptyIndices)
1180  {
1181  emptyIndices->at(ei++) = oi;
1182  }
1183 
1184  ++oi;
1185  }
1186  }
1187 
1188  cloud->resize(oi);
1189  if(cloudProb)
1190  {
1191  cloudProb->resize(oi);
1192  }
1193  if(obstacleIndices)
1194  {
1195  obstacleIndices->resize(si);
1196  UDEBUG("obstacle=%d", si);
1197  }
1198  if(emptyIndices)
1199  {
1200  emptyIndices->resize(ei);
1201  UDEBUG("empty=%d", ei);
1202  }
1203  if(frontierIndices)
1204  {
1205  frontierIndices->resize(fi);
1206  UDEBUG("frontier=%d", fi);
1207  }
1208  if(groundIndices)
1209  {
1210  groundIndices->resize(gi);
1211  UDEBUG("ground=%d", gi);
1212  }
1213 
1214  UDEBUG("");
1215  return cloud;
1216 }
1217 
1218 cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCellSize, float minGridSize, unsigned int treeDepth)
1219 {
1220  UDEBUG("minGridSize=%f, treeDepth=%d", minGridSize, (int)treeDepth);
1221  UASSERT(treeDepth <= octree_->getTreeDepth());
1222  if(treeDepth == 0)
1223  {
1224  treeDepth = octree_->getTreeDepth();
1225  }
1226 
1227  gridCellSize = octree_->getNodeSize(treeDepth);
1228 
1229  cv::Mat obstaclesMat = cv::Mat(1, (int)octree_->size(), CV_32FC2);
1230  cv::Mat groundMat = cv::Mat(1, (int)octree_->size(), CV_32FC2);
1231  int gi=0;
1232  int oi=0;
1233  cv::Vec2f * oPtr = obstaclesMat.ptr<cv::Vec2f>(0,0);
1234  cv::Vec2f * gPtr = groundMat.ptr<cv::Vec2f>(0,0);
1235  float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
1236  for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
1237  {
1238  octomap::point3d pt = octree_->keyToCoord(it.getKey());
1239  if(octree_->isNodeOccupied(*it) &&
1240  it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeObstacle)
1241  {
1242  // projected on ground
1243  oPtr[oi][0] = pt.x()-halfCellSize;
1244  oPtr[oi][1] = pt.y()-halfCellSize;
1245  ++oi;
1246  }
1247  else
1248  {
1249  // projected on ground
1250  gPtr[gi][0] = pt.x()-halfCellSize;
1251  gPtr[gi][1] = pt.y()-halfCellSize;
1252  ++gi;
1253  }
1254  }
1255  obstaclesMat = obstaclesMat(cv::Range::all(), cv::Range(0, oi));
1256  groundMat = groundMat(cv::Range::all(), cv::Range(0, gi));
1257 
1258  std::map<int, Transform> poses;
1259  poses.insert(std::make_pair(1, Transform::getIdentity()));
1260  std::map<int, std::pair<cv::Mat, cv::Mat> > maps;
1261  maps.insert(std::make_pair(1, std::make_pair(groundMat, obstaclesMat)));
1262 
1263  cv::Mat map = util3d::create2DMapFromOccupancyLocalMaps(
1264  poses,
1265  maps,
1266  gridCellSize,
1267  xMin, yMin,
1268  minGridSize,
1269  false);
1270  UDEBUG("");
1271  return map;
1272 }
1273 
1274 bool OctoMap::writeBinary(const std::string & path)
1275 {
1276  return octree_->writeBinary(path);
1277 }
1278 
1279 } /* namespace rtabmap */
NULL
#define NULL
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
octomap::KeyRay::begin
iterator begin()
UTimer
Definition: UTimer.h:46
octomap::OcTreeBaseImpl::end
const iterator end() 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
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:1054
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:191
octomath::Vector3::normalize
Vector3 & normalize()
f
f
rtabmap::OctoMap::isNodeVisited
static bool isNodeVisited(std::unordered_set< octomap::OcTreeKey, octomap::OcTreeKey::KeyHash > const &EmptyNodes, octomap::OcTreeKey const key)
Definition: OctoMap.cpp:461
rtabmap::OctoMap::updateMinMax
void updateMinMax(const octomap::point3d &point)
Definition: OctoMap.cpp:1025
octomap::OcTreeNode::getOccupancy
double getOccupancy() const
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:401
rtabmap::RtabmapColorOcTree::RtabmapColorOcTreeMemberInit
static StaticMemberInitializer RtabmapColorOcTreeMemberInit
static member to ensure static initialization (only once)
Definition: OctoMap.h:170
rtabmap::OctoMap::addedNodes_
std::map< int, Transform > addedNodes_
Definition: OctoMap.h:234
octomap::OcTreeBaseImpl::keyToCoord
double keyToCoord(key_type key, unsigned depth) const
octomap::OcTreeBaseImpl< RtabmapColorOcTreeNode, AbstractOccupancyOcTree >::deleteNodeChild
void deleteNodeChild(RtabmapColorOcTreeNode *node, unsigned int childIdx)
rtabmap::RtabmapColorOcTreeNode::createChild
bool createChild(unsigned int i)
Definition: OctoMap.cpp:98
rtabmap::OctoMap::fullUpdate_
bool fullUpdate_
Definition: OctoMap.h:236
octomap::OcTreeBaseImpl::deleteNode
bool deleteNode(double x, double y, double z, unsigned int depth=0)
rtabmap::OctoMap::cacheViewPoints_
std::map< int, cv::Point3f > cacheViewPoints_
Definition: OctoMap.h:232
rtabmap::RtabmapColorOcTreeNode::getChild
RtabmapColorOcTreeNode * getChild(unsigned int i)
Definition: OctoMap.cpp:45
glm::all
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
octomap::OcTreeBaseImpl< RtabmapColorOcTreeNode, AbstractOccupancyOcTree >::getNodeChild
RtabmapColorOcTreeNode * getNodeChild(RtabmapColorOcTreeNode *node, unsigned int childIdx) const
octomap::OcTreeNode::getLogOdds
float getLogOdds() const
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::util3d::laserScanToPoint
pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const LaserScan &laserScan, int index)
Definition: util3d.cpp:2302
rtabmap::OctoMap::findCloseEmpty
static octomap::point3d findCloseEmpty(RtabmapColorOcTree *octree_, unsigned int treeDepth, octomap::point3d startPosition)
Definition: OctoMap.cpp:411
rtabmap::OctoMap::isValidEmpty
static bool isValidEmpty(RtabmapColorOcTree *octree_, unsigned int treeDepth, octomap::point3d startPosition)
Definition: OctoMap.cpp:397
octomap::OcTreeNode::updateOccupancyChildren
void updateOccupancyChildren()
octomap::OcTreeBaseImpl::coordToKey
key_type coordToKey(double coordinate) const
rtabmap::RtabmapColorOcTreeNode::setPointRef
void setPointRef(const octomap::point3d &point)
Definition: OctoMap.h:65
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:2337
UFATAL
#define UFATAL(...)
rtabmap::RtabmapColorOcTreeNode::setNodeRefId
void setNodeRefId(int nodeRefId)
Definition: OctoMap.h:63
util3d_mapping.h
glm::uint8_t
detail::uint8 uint8_t
Definition: fwd.hpp:908
octomap::OcTreeBaseImpl::begin_leafs
leaf_iterator begin_leafs(unsigned char maxDepth=0) const
UASSERT
#define UASSERT(condition)
rtabmap::OctoMap::update
bool update(const std::map< int, Transform > &poses)
Definition: OctoMap.cpp:512
UStl.h
Wrappers of STL for convenient functions.
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:363
rtabmap::OctoMap::cache_
std::map< int, std::pair< std::pair< cv::Mat, cv::Mat >, cv::Mat > > cache_
Definition: OctoMap.h:230
octomap::OcTreeBaseImpl< RtabmapColorOcTreeNode, AbstractOccupancyOcTree >::search
RtabmapColorOcTreeNode * search(double x, double y, double z, unsigned int depth=0) const
rtabmap::RtabmapColorOcTree::integrateNodeColor
RtabmapColorOcTreeNode * integrateNodeColor(const octomap::OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: OctoMap.cpp:208
rtabmap::OctoMap::cacheClouds_
std::map< int, std::pair< const pcl::PointCloud< pcl::PointXYZRGB >::Ptr, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr > > cacheClouds_
Definition: OctoMap.h:231
OcTreeDataNode< float >::children
AbstractOcTreeNode ** children
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
octomap::OcTreeBaseImpl::begin
iterator begin(unsigned char maxDepth=0) const
octomap::OcTreeBaseImpl::coordToKeyChecked
bool coordToKeyChecked(const point3d &coord, OcTreeKey &key) const
util3d_filtering.h
rtabmap::OctoMap::OctoMap
OctoMap(const ParametersMap &parameters=ParametersMap())
Definition: OctoMap.cpp:291
rtabmap::OctoMap::octree_
RtabmapColorOcTree * octree_
Definition: OctoMap.h:233
octomap::OcTreeBaseImpl::getResolution
double getResolution() const
octomap::ColorOcTreeNode::getAverageChildColor
ColorOcTreeNode::Color getAverageChildColor() const
octomap::OcTreeBaseImpl::getNodeSize
double getNodeSize(unsigned depth) const
octomap::KeySet
unordered_ns::unordered_set< OcTreeKey, OcTreeKey::KeyHash > KeySet
rtabmap::RtabmapColorOcTree::isNodeCollapsible
virtual bool isNodeCollapsible(const RtabmapColorOcTreeNode *node) const
Definition: OctoMap.cpp:167
tree
tree
rtabmap::RtabmapColorOcTree::RtabmapColorOcTree
RtabmapColorOcTree(double resolution)
Default constructor, sets resolution of leafs.
Definition: OctoMap.cpp:126
rtabmap::OctoMap::emptyFloodFillDepth_
unsigned int emptyFloodFillDepth_
Definition: OctoMap.h:240
rtabmap::RtabmapColorOcTreeNode::getPointRef
const octomap::point3d & getPointRef() const
Definition: OctoMap.h:68
util3d_transforms.h
rtabmap::RtabmapColorOcTreeNode::setOccupancyType
void setOccupancyType(char type)
Definition: OctoMap.h:64
rtabmap::RtabmapColorOcTree::updateInnerOccupancyRecurs
void updateInnerOccupancyRecurs(RtabmapColorOcTreeNode *node, unsigned int depth)
Definition: OctoMap.cpp:237
octomap::KeyRay::end
iterator end()
false
#define false
Definition: ConvertUTF.c:56
octomap::OcTreeBaseImpl::end_leafs
const leaf_iterator end_leafs() const
rtabmap::OctoMap::minValues_
double minValues_[3]
Definition: OctoMap.h:241
rtabmap::OctoMap::~OctoMap
virtual ~OctoMap()
Definition: OctoMap.cpp:345
rtabmap::OctoMap::rangeMax_
float rangeMax_
Definition: OctoMap.h:238
OcTreeDataNode< float >::getValue
float getValue() const
UDEBUG
#define UDEBUG(...)
rtabmap::OctoMap::writeBinary
bool writeBinary(const std::string &path)
Definition: OctoMap.cpp:1274
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
octomap::OcTreeBaseImpl::size
virtual size_t size() const
rtabmap::RtabmapColorOcTree::setNodeColor
RtabmapColorOcTreeNode * setNodeColor(const octomap::OcTreeKey &key, uint8_t r, uint8_t g, uint8_t b)
Definition: OctoMap.cpp:131
rtabmap::OctoMap::rayTracing_
bool rayTracing_
Definition: OctoMap.h:239
rtabmap::OctoMap::createProjectionMap
cv::Mat createProjectionMap(float &xMin, float &yMin, float &gridCellSize, float minGridSize=0.0f, unsigned int treeDepth=0)
Definition: OctoMap.cpp:1218
rtabmap::OctoMap::maxValues_
double maxValues_[3]
Definition: OctoMap.h:242
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:365
rtabmap::RtabmapColorOcTree::pruneNode
virtual bool pruneNode(RtabmapColorOcTreeNode *node)
Definition: OctoMap.cpp:142
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< RtabmapColorOcTreeNode, AbstractOccupancyOcTree >::nodeChildExists
bool nodeChildExists(const RtabmapColorOcTreeNode *node, unsigned int childIdx) const
octomap::OcTreeBaseImpl< RtabmapColorOcTreeNode, AbstractOccupancyOcTree >::nodeHasChildren
bool nodeHasChildren(const RtabmapColorOcTreeNode *node) const
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:126
end
end
rtabmap::OctoMap::updateError_
float updateError_
Definition: OctoMap.h:237
octomap::ColorOcTreeNode::copyData
void copyData(const ColorOcTreeNode &from)
octomap::OcTreeBaseImpl::getTreeDepth
unsigned int getTreeDepth() const
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
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:474
octomap::OcTreeBaseImpl::computeRayKeys
bool computeRayKeys(const point3d &origin, const point3d &end, KeyRay &ray) const
rtabmap::OctoMap::findEmptyNode
static std::unordered_set< octomap::OcTreeKey, octomap::OcTreeKey::KeyHash > findEmptyNode(RtabmapColorOcTree *octree_, unsigned int treeDepth, octomap::point3d startPosition)
Definition: OctoMap.cpp:494
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29