MapsManager.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  documentation and/or other materials provided with the distribution.
11  * Neither the name of the Universite de Sherbrooke nor the
12  names of its contributors may be used to endorse or promote products
13  derived from this software without specific prior written permission.
14 
15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
19 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 */
26 
27 #include "rtabmap_ros/MapsManager.h"
28 
29 #include <rtabmap/utilite/ULogger.h>
30 #include <rtabmap/utilite/UTimer.h>
31 #include <rtabmap/utilite/UStl.h>
32 #include <rtabmap/utilite/UConversion.h>
33 #include <rtabmap/core/util3d_mapping.h>
34 #include <rtabmap/core/util3d_filtering.h>
35 #include <rtabmap/core/util3d_transforms.h>
36 #include <rtabmap/core/util2d.h>
37 #include <rtabmap/core/Memory.h>
38 #include <rtabmap/core/Graph.h>
39 #include <rtabmap/core/Version.h>
40 #include <rtabmap/core/OccupancyGrid.h>
41 
42 #include <pcl/search/kdtree.h>
43 
44 #include <nav_msgs/OccupancyGrid.h>
45 #include <ros/ros.h>
46 
47 #include <pcl_conversions/pcl_conversions.h>
48 
49 #ifdef WITH_OCTOMAP_MSGS
50 #ifdef RTABMAP_OCTOMAP
51 #include <octomap_msgs/conversions.h>
52 #include <octomap/ColorOcTree.h>
53 #include <rtabmap/core/OctoMap.h>
54 #endif
55 #endif
56 
57 using namespace rtabmap;
58 
59 MapsManager::MapsManager() :
60  cloudOutputVoxelized_(true),
61  cloudSubtractFiltering_(false),
62  cloudSubtractFilteringMinNeighbors_(2),
63  mapFilterRadius_(0.0),
64  mapFilterAngle_(30.0), // degrees
65  mapCacheCleanup_(true),
66  negativePosesIgnored_(true),
67  negativeScanEmptyRayTracing_(true),
68  assembledObstacles_(new pcl::PointCloud<pcl::PointXYZRGB>),
69  assembledGround_(new pcl::PointCloud<pcl::PointXYZRGB>),
70  occupancyGrid_(new OccupancyGrid),
71  octomap_(0),
72  octomapTreeDepth_(16)
73 {
74 }
75 
76 void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::string & name, bool usePublicNamespace)
77 {
78  // common map stuff
79  pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
80  pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
81  pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_);
82  pnh.param("map_negative_poses_ignored", negativePosesIgnored_, negativePosesIgnored_);
83  pnh.param("map_negative_scan_empty_ray_tracing", negativeScanEmptyRayTracing_, negativeScanEmptyRayTracing_);
84 
85  if(pnh.hasParam("scan_output_voxelized"))
86  {
87  ROS_WARN("Parameter \"scan_output_voxelized\" has been "
88  "removed. Use \"cloud_output_voxelized\" instead.");
89  if(!pnh.hasParam("cloud_output_voxelized"))
90  {
91  pnh.getParam("scan_output_voxelized", cloudOutputVoxelized_);
92  }
93  }
94  pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_);
95  pnh.param("cloud_subtract_filtering", cloudSubtractFiltering_, cloudSubtractFiltering_);
96  pnh.param("cloud_subtract_filtering_min_neighbors", cloudSubtractFilteringMinNeighbors_, cloudSubtractFilteringMinNeighbors_);
97 
98  ROS_INFO("%s(maps): map_filter_radius = %f", name.c_str(), mapFilterRadius_);
99  ROS_INFO("%s(maps): map_filter_angle = %f", name.c_str(), mapFilterAngle_);
100  ROS_INFO("%s(maps): map_cleanup = %s", name.c_str(), mapCacheCleanup_?"true":"false");
101  ROS_INFO("%s(maps): map_negative_poses_ignored = %s", name.c_str(), negativePosesIgnored_?"true":"false");
102  ROS_INFO("%s(maps): map_negative_scan_ray_tracing = %s", name.c_str(), negativeScanEmptyRayTracing_?"true":"false");
103  ROS_INFO("%s(maps): cloud_output_voxelized = %s", name.c_str(), cloudOutputVoxelized_?"true":"false");
104  ROS_INFO("%s(maps): cloud_subtract_filtering = %s", name.c_str(), cloudSubtractFiltering_?"true":"false");
105  ROS_INFO("%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_);
106 
107 #ifdef WITH_OCTOMAP_MSGS
108 #ifdef RTABMAP_OCTOMAP
109  octomap_ = new OctoMap(occupancyGrid_->getCellSize(), 0.5, occupancyGrid_->isFullUpdate(), occupancyGrid_->getUpdateError());
110  pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_);
111  if(octomapTreeDepth_ > 16)
112  {
113  ROS_WARN("octomap_tree_depth maximum is 16");
114  octomapTreeDepth_ = 16;
115  }
116  else if(octomapTreeDepth_ < 0)
117  {
118  ROS_WARN("octomap_tree_depth cannot be negative, set to 16 instead");
119  octomapTreeDepth_ = 16;
120  }
121  ROS_INFO("%s(maps): octomap_tree_depth = %d", name.c_str(), octomapTreeDepth_);
122 #endif
123 #endif
124 
125  // If true, the last message published on
126  // the map topics will be saved and sent to new subscribers when they
127  // connect
128  bool latch = true;
129  pnh.param("latch", latch, latch);
130 
131  // mapping topics
132  ros::NodeHandle * nht;
133  if(usePublicNamespace)
134  {
135  nht = &nh;
136  }
137  else
138  {
139  nht = &pnh;
140  }
141  gridMapPub_ = nht->advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
142  gridProbMapPub_ = nht->advertise<nav_msgs::OccupancyGrid>("grid_prob_map", 1, latch);
143  cloudMapPub_ = nht->advertise<sensor_msgs::PointCloud2>("cloud_map", 1, latch);
144  cloudObstaclesPub_ = nht->advertise<sensor_msgs::PointCloud2>("cloud_obstacles", 1, latch);
145  cloudGroundPub_ = nht->advertise<sensor_msgs::PointCloud2>("cloud_ground", 1, latch);
146 
147  // deprecated
148  projMapPub_ = nht->advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
149  scanMapPub_ = nht->advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
150 
151 #ifdef WITH_OCTOMAP_MSGS
152 #ifdef RTABMAP_OCTOMAP
153  octoMapPubBin_ = nht->advertise<octomap_msgs::Octomap>("octomap_binary", 1, latch);
154  octoMapPubFull_ = nht->advertise<octomap_msgs::Octomap>("octomap_full", 1, latch);
155  octoMapCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_occupied_space", 1, latch);
156  octoMapObstacleCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_obstacles", 1, latch);
157  octoMapGroundCloud_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_ground", 1, latch);
158  octoMapEmptySpace_ = nht->advertise<sensor_msgs::PointCloud2>("octomap_empty_space", 1, latch);
159  octoMapProj_ = nht->advertise<nav_msgs::OccupancyGrid>("octomap_grid", 1, latch);
160 #endif
161 #endif
162 }
163 
164 MapsManager::~MapsManager() {
165  clear();
166 
167  delete occupancyGrid_;
168 
169 #ifdef WITH_OCTOMAP_MSGS
170 #ifdef RTABMAP_OCTOMAP
171  if(octomap_)
172  {
173  delete octomap_;
174  octomap_ = 0;
175  }
176 #endif
177 #endif
178 }
179 
180 void parameterMoved(
181  ros::NodeHandle & nh,
182  const std::string & rosName,
183  const std::string & parameterName,
184  ParametersMap & parameters)
185 {
186  if(nh.hasParam(rosName))
187  {
188  ParametersMap::const_iterator iter = Parameters::getDefaultParameters().find(parameterName);
189  if(iter != Parameters::getDefaultParameters().end())
190  {
191  ROS_WARN("Parameter \"%s\" has moved from "
192  "rtabmap_ros to rtabmap library. Use "
193  "parameter \"%s\" instead. The value is still "
194  "copied to new parameter name.",
195  rosName.c_str(),
196  parameterName.c_str());
197  std::string type = Parameters::getType(parameterName);
198  if(type.compare("float") || type.compare("double"))
199  {
200  double v = uStr2Double(iter->second);
201  nh.getParam(rosName, v);
202  parameters.insert(ParametersPair(parameterName, uNumber2Str(v)));
203  }
204  else if(type.compare("int") || type.compare("unsigned int"))
205  {
206  int v = uStr2Int(iter->second);
207  nh.getParam(rosName, v);
208  parameters.insert(ParametersPair(parameterName, uNumber2Str(v)));
209  }
210  else if(type.compare("bool"))
211  {
212  bool v = uStr2Bool(iter->second);
213  nh.getParam(rosName, v);
214  if(rosName.compare("grid_incremental") == 0)
215  {
216  v = !v; // new parameter is called kGridGlobalFullUpdate(), which is the inverse
217  }
218  parameters.insert(ParametersPair(parameterName, uNumber2Str(v)));
219 
220  }
221  else
222  {
223  ROS_ERROR("Not handled type \"%s\" for parameter \"%s\"", type.c_str(), parameterName.c_str());
224  }
225  }
226  else
227  {
228  ROS_ERROR("Parameter \"%s\" not found in default parameters.", parameterName.c_str());
229  }
230  }
231 }
232 
233 void MapsManager::backwardCompatibilityParameters(ros::NodeHandle & pnh, ParametersMap & parameters) const
234 {
235  // removed
236  if(pnh.hasParam("cloud_frustum_culling"))
237  {
238  ROS_WARN("Parameter \"cloud_frustum_culling\" has been removed. OctoMap topics "
239  "already do it. You can remove it from your launch file.");
240  }
241 
242  // moved
243  parameterMoved(pnh, "cloud_decimation", Parameters::kGridDepthDecimation(), parameters);
244  parameterMoved(pnh, "cloud_max_depth", Parameters::kGridRangeMax(), parameters);
245  parameterMoved(pnh, "cloud_min_depth", Parameters::kGridRangeMin(), parameters);
246  parameterMoved(pnh, "cloud_voxel_size", Parameters::kGridCellSize(), parameters);
247  parameterMoved(pnh, "cloud_floor_culling_height", Parameters::kGridMaxGroundHeight(), parameters);
248  parameterMoved(pnh, "cloud_ceiling_culling_height", Parameters::kGridMaxObstacleHeight(), parameters);
249  parameterMoved(pnh, "cloud_noise_filtering_radius", Parameters::kGridNoiseFilteringRadius(), parameters);
250  parameterMoved(pnh, "cloud_noise_filtering_min_neighbors", Parameters::kGridNoiseFilteringMinNeighbors(), parameters);
251  parameterMoved(pnh, "scan_decimation", Parameters::kGridScanDecimation(), parameters);
252  parameterMoved(pnh, "scan_voxel_size", Parameters::kGridCellSize(), parameters);
253  parameterMoved(pnh, "proj_max_ground_angle", Parameters::kGridMaxGroundAngle(), parameters);
254  parameterMoved(pnh, "proj_min_cluster_size", Parameters::kGridMinClusterSize(), parameters);
255  parameterMoved(pnh, "proj_max_height", Parameters::kGridMaxObstacleHeight(), parameters);
256  parameterMoved(pnh, "proj_max_obstacles_height", Parameters::kGridMaxObstacleHeight(), parameters);
257  parameterMoved(pnh, "proj_max_ground_height", Parameters::kGridMaxGroundHeight(), parameters);
258 
259  parameterMoved(pnh, "proj_detect_flat_obstacles", Parameters::kGridFlatObstacleDetected(), parameters);
260  parameterMoved(pnh, "proj_map_frame", Parameters::kGridMapFrameProjection(), parameters);
261  parameterMoved(pnh, "grid_unknown_space_filled", Parameters::kGridScan2dUnknownSpaceFilled(), parameters);
262  parameterMoved(pnh, "grid_cell_size", Parameters::kGridCellSize(), parameters);
263  parameterMoved(pnh, "grid_incremental", Parameters::kGridGlobalFullUpdate(), parameters);
264  parameterMoved(pnh, "grid_size", Parameters::kGridGlobalMinSize(), parameters);
265  parameterMoved(pnh, "grid_eroded", Parameters::kGridGlobalEroded(), parameters);
266  parameterMoved(pnh, "grid_footprint_radius", Parameters::kGridGlobalFootprintRadius(), parameters);
267 
268 #ifdef WITH_OCTOMAP_MSGS
269 #ifdef RTABMAP_OCTOMAP
270  parameterMoved(pnh, "octomap_ground_is_obstacle", Parameters::kGridGroundIsObstacle(), parameters);
271  parameterMoved(pnh, "octomap_occupancy_thr", Parameters::kGridGlobalOccupancyThr(), parameters);
272 #endif
273 #endif
274 }
275 
276 void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
277 {
278  parameters_ = parameters;
279  occupancyGrid_->parseParameters(parameters_);
280 
281 #ifdef WITH_OCTOMAP_MSGS
282 #ifdef RTABMAP_OCTOMAP
283  if(octomap_)
284  {
285  delete octomap_;
286  octomap_ = 0;
287  }
288  octomap_ = new OctoMap(parameters_);
289 #endif
290 #endif
291 }
292 
293 void MapsManager::set2DMap(
294  const cv::Mat & map,
295  float xMin,
296  float yMin,
297  float cellSize,
298  const std::map<int, rtabmap::Transform> & poses,
299  const rtabmap::Memory * memory)
300 {
301  occupancyGrid_->setMap(map, xMin, yMin, cellSize, poses);
302  //update cache in case the map should be updated
303  if(memory)
304  {
305  for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
306  {
307  std::map<int, std::pair< std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator jter = gridMaps_.find(iter->first);
308  if(!uContains(gridMaps_, iter->first))
309  {
310  rtabmap::SensorData data;
311  data = memory->getSignatureDataConst(iter->first, false, false, false, true);
312  if(data.gridCellSize() == 0.0f)
313  {
314  ROS_WARN("Local occupancy grid doesn't exist for node %d", iter->first);
315  }
316  else
317  {
318  cv::Mat ground, obstacles, emptyCells;
319  data.uncompressData(
320  0,
321  0,
322  0,
323  0,
324  &ground,
325  &obstacles,
326  &emptyCells);
327 
328  uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
329  uInsert(gridMapsViewpoints_, std::make_pair(iter->first, data.gridViewPoint()));
330  occupancyGrid_->addToCache(iter->first, ground, obstacles, emptyCells);
331  }
332  }
333  else
334  {
335  occupancyGrid_->addToCache(iter->first, jter->second.first.first, jter->second.first.second, jter->second.second);
336  }
337  }
338  }
339 }
340 
341 void MapsManager::clear()
342 {
343  gridMaps_.clear();
344  gridMapsViewpoints_.clear();
345  assembledGround_->clear();
346  assembledObstacles_->clear();
347  assembledGroundPoses_.clear();
348  assembledObstaclePoses_.clear();
349  assembledGroundIndex_.release();
350  assembledObstacleIndex_.release();
351  groundClouds_.clear();
352  obstacleClouds_.clear();
353  occupancyGrid_->clear();
354 #ifdef WITH_OCTOMAP_MSGS
355 #ifdef RTABMAP_OCTOMAP
356  octomap_->clear();
357 #endif
358 #endif
359 }
360 
361 bool MapsManager::hasSubscribers() const
362 {
363  return cloudMapPub_.getNumSubscribers() != 0 ||
364  cloudObstaclesPub_.getNumSubscribers() != 0 ||
365  cloudGroundPub_.getNumSubscribers() != 0 ||
366  projMapPub_.getNumSubscribers() != 0 ||
367  gridMapPub_.getNumSubscribers() != 0 ||
368  gridProbMapPub_.getNumSubscribers() != 0 ||
369  scanMapPub_.getNumSubscribers() != 0 ||
370  octoMapPubBin_.getNumSubscribers() != 0 ||
371  octoMapPubFull_.getNumSubscribers() != 0 ||
372  octoMapCloud_.getNumSubscribers() != 0 ||
373  octoMapObstacleCloud_.getNumSubscribers() != 0 ||
374  octoMapGroundCloud_.getNumSubscribers() != 0 ||
375  octoMapEmptySpace_.getNumSubscribers() != 0 ||
376  octoMapProj_.getNumSubscribers() != 0;
377 }
378 
379 std::map<int, Transform> MapsManager::getFilteredPoses(const std::map<int, Transform> & poses)
380 {
381  if(mapFilterRadius_ > 0.0)
382  {
383  // filter nodes
384  double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
385  return rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
386  }
387  return std::map<int, Transform>();
388 }
389 
390 std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
391  const std::map<int, rtabmap::Transform> & poses,
392  const rtabmap::Memory * memory,
393  bool updateGrid,
394  bool updateOctomap,
395  const std::map<int, rtabmap::Signature> & signatures)
396 {
397  bool updateGridCache = updateGrid || updateOctomap;
398  if(!updateGrid && !updateOctomap)
399  {
400  // all false, update only those where we have subscribers
401  updateOctomap =
402  octoMapPubBin_.getNumSubscribers() != 0 ||
403  octoMapPubFull_.getNumSubscribers() != 0 ||
404  octoMapCloud_.getNumSubscribers() != 0 ||
405  octoMapObstacleCloud_.getNumSubscribers() != 0 ||
406  octoMapGroundCloud_.getNumSubscribers() != 0 ||
407  octoMapEmptySpace_.getNumSubscribers() != 0 ||
408  octoMapProj_.getNumSubscribers() != 0;
409 
410  updateGrid = projMapPub_.getNumSubscribers() != 0 ||
411  gridMapPub_.getNumSubscribers() != 0 ||
412  gridProbMapPub_.getNumSubscribers() != 0;
413 
414  updateGridCache = updateOctomap || updateGrid ||
415  cloudMapPub_.getNumSubscribers() != 0 ||
416  cloudObstaclesPub_.getNumSubscribers() != 0 ||
417  cloudGroundPub_.getNumSubscribers() != 0 ||
418  scanMapPub_.getNumSubscribers() != 0;
419  }
420 
421 #ifndef WITH_OCTOMAP_MSGS
422  updateOctomap = false;
423 #endif
424 #ifndef RTABMAP_OCTOMAP
425  updateOctomap = false;
426 #endif
427 
428 
429  UDEBUG("Updating map caches...");
430 
431  if(!memory && signatures.size() == 0)
432  {
433  ROS_ERROR("Memory and signatures should not be both null!?");
434  return std::map<int, rtabmap::Transform>();
435  }
436 
437  std::map<int, rtabmap::Transform> filteredPoses;
438 
439  // update cache
440  if(updateGridCache)
441  {
442  // filter nodes
443  if(mapFilterRadius_ > 0.0)
444  {
445  UDEBUG("Filter nodes...");
446  double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
447  filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
448  for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
449  {
450  if(iter->first <=0)
451  {
452  // make sure to keep latest data
453  filteredPoses.insert(*iter);
454  }
455  else
456  {
457  break;
458  }
459  }
460  }
461  else
462  {
463  filteredPoses = poses;
464  }
465 
466  if(negativePosesIgnored_)
467  {
468  for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end();)
469  {
470  if(iter->first <= 0)
471  {
472  filteredPoses.erase(iter++);
473  }
474  else
475  {
476  ++iter;
477  }
478  }
479  }
480 
481  bool longUpdate = false;
482  UTimer longUpdateTimer;
483  if(filteredPoses.size() > 20)
484  {
485  if(updateGridCache && gridMaps_.size() < 5)
486  {
487  ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-gridMaps_.size()));
488  longUpdate = true;
489  }
490 #ifdef WITH_OCTOMAP_MSGS
491 #ifdef RTABMAP_OCTOMAP
492  if(updateOctomap && octomap_->addedNodes().size() < 5)
493  {
494  ROS_WARN("Many clouds should be added to octomap (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-octomap_->addedNodes().size()));
495  longUpdate = true;
496  }
497 #endif
498 #endif
499  }
500 
501  bool occupancySavedInDB = memory && uStrNumCmp(memory->getDatabaseVersion(), "0.11.10")>=0?true:false;
502 
503  for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
504  {
505  if(!iter->second.isNull())
506  {
507  rtabmap::SensorData data;
508  if(updateGridCache && (iter->first < 0 || !uContains(gridMaps_, iter->first)))
509  {
510  UDEBUG("Data required for %d", iter->first);
511  std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
512  if(findIter != signatures.end())
513  {
514  data = findIter->second.sensorData();
515  }
516  else if(memory)
517  {
518  data = memory->getSignatureDataConst(iter->first, occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, !occupancyGrid_->isGridFromDepth() && !occupancySavedInDB, false, true);
519  }
520 
521  if(data.id() != 0)
522  {
523  UDEBUG("Adding grid map %d to cache...", iter->first);
524  cv::Point3f viewPoint;
525  cv::Mat ground, obstacles, emptyCells;
526  if(iter->first > 0)
527  {
528  cv::Mat rgb, depth;
529  LaserScan scan;
530  bool generateGrid = data.gridCellSize() == 0.0f;
531  static bool warningShown = false;
532  if(occupancySavedInDB && generateGrid && !warningShown)
533  {
534  warningShown = true;
535  UWARN("Occupancy grid for location %d should be added to global map (e..g, a ROS node is subscribed to "
536  "any occupancy grid output) but it cannot be found "
537  "in memory. For convenience, the occupancy "
538  "grid is regenerated. Make sure parameter \"%s\" is true to "
539  "avoid this warning for the next locations added to map. For older "
540  "locations already in database without an occupancy grid map, you can use the "
541  "\"rtabmap-databaseViewer\" to regenerate the missing occupancy grid maps and "
542  "save them back in the database for next sessions. This warning is only shown once.",
543  data.id(), Parameters::kRGBDCreateOccupancyGrid().c_str());
544  }
545  if(memory && occupancySavedInDB && generateGrid)
546  {
547  // if we are here, it is because we loaded a database with old nodes not having occupancy grid set
548  // try reload again
549  data = memory->getSignatureDataConst(iter->first, occupancyGrid_->isGridFromDepth(), !occupancyGrid_->isGridFromDepth(), false, false);
550  }
551  data.uncompressData(
552  occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0,
553  occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0,
554  !occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0,
555  0,
556  generateGrid?0:&ground,
557  generateGrid?0:&obstacles,
558  generateGrid?0:&emptyCells);
559 
560  if(generateGrid)
561  {
562  Signature tmp(data);
563  tmp.setPose(iter->second);
564  occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
565  uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
566  uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
567  }
568  else
569  {
570  viewPoint = data.gridViewPoint();
571  uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
572  uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
573  }
574  }
575  else
576  {
577  // generate tmp occupancy grid for negative ids (assuming data is already uncompressed)
578  // For negative laser scans, fill empty space?
579  bool unknownSpaceFilled = Parameters::defaultGridScan2dUnknownSpaceFilled();
580  Parameters::parse(parameters_, Parameters::kGridScan2dUnknownSpaceFilled(), unknownSpaceFilled);
581 
582  if(unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_)
583  {
584  ParametersMap parameters;
585  parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(negativeScanEmptyRayTracing_)));
586  occupancyGrid_->parseParameters(parameters);
587  }
588 
589  cv::Mat rgb, depth;
590  LaserScan scan;
591  bool generateGrid = data.gridCellSize() == 0.0f || (unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_);
592  data.uncompressData(
593  occupancyGrid_->isGridFromDepth() && generateGrid?&rgb:0,
594  occupancyGrid_->isGridFromDepth() && generateGrid?&depth:0,
595  !occupancyGrid_->isGridFromDepth() && generateGrid?&scan:0,
596  0,
597  generateGrid?0:&ground,
598  generateGrid?0:&obstacles,
599  generateGrid?0:&emptyCells);
600 
601  if(generateGrid)
602  {
603  Signature tmp(data);
604  tmp.setPose(iter->second);
605  occupancyGrid_->createLocalMap(tmp, ground, obstacles, emptyCells, viewPoint);
606  uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
607  uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
608  }
609  else
610  {
611  viewPoint = data.gridViewPoint();
612  uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(std::make_pair(ground, obstacles), emptyCells)));
613  uInsert(gridMapsViewpoints_, std::make_pair(iter->first, viewPoint));
614  }
615 
616  // put back
617  if(unknownSpaceFilled != negativeScanEmptyRayTracing_ && negativeScanEmptyRayTracing_)
618  {
619  ParametersMap parameters;
620  parameters.insert(ParametersPair(Parameters::kGridScan2dUnknownSpaceFilled(), uBool2Str(unknownSpaceFilled)));
621  occupancyGrid_->parseParameters(parameters);
622  }
623  }
624  }
625  else if(memory)
626  {
627  ROS_ERROR("Data missing for node %d to update the maps", iter->first);
628  }
629  }
630 
631  if(updateGrid &&
632  (iter->first < 0 ||
633  occupancyGrid_->addedNodes().find(iter->first) == occupancyGrid_->addedNodes().end()))
634  {
635  std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator mter = gridMaps_.find(iter->first);
636  if(mter != gridMaps_.end())
637  {
638  if(!mter->second.first.first.empty() || !mter->second.first.second.empty())
639  {
640  occupancyGrid_->addToCache(iter->first, mter->second.first.first, mter->second.first.second, mter->second.second);
641  }
642  }
643  }
644 
645 #ifdef WITH_OCTOMAP_MSGS
646 #ifdef RTABMAP_OCTOMAP
647  if(updateOctomap &&
648  (iter->first < 0 ||
649  octomap_->addedNodes().find(iter->first) == octomap_->addedNodes().end()))
650  {
651  std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator mter = gridMaps_.find(iter->first);
652  std::map<int, cv::Point3f>::iterator pter = gridMapsViewpoints_.find(iter->first);
653  if(mter != gridMaps_.end() && pter!=gridMapsViewpoints_.end())
654  {
655  if((mter->second.first.first.empty() || mter->second.first.first.channels() > 2) &&
656  (mter->second.first.second.empty() || mter->second.first.second.channels() > 2))
657  {
658  octomap_->addToCache(iter->first, mter->second.first.first, mter->second.first.second, mter->second.second, pter->second);
659  }
660  else if(!mter->second.first.first.empty() && !mter->second.first.second.empty())
661  {
662  ROS_WARN("Node %d: Cannot update octomap with 2D occupancy grids. "
663  "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see "
664  "all occupancy grid parameters.",
665  iter->first);
666  }
667  }
668  }
669 #endif
670 #endif
671  }
672  else
673  {
674  ROS_ERROR("Pose null for node %d", iter->first);
675  }
676  }
677 
678  if(updateGrid)
679  {
680  occupancyGrid_->update(filteredPoses);
681  }
682 
683 #ifdef WITH_OCTOMAP_MSGS
684 #ifdef RTABMAP_OCTOMAP
685  if(updateOctomap)
686  {
687  UTimer time;
688  octomap_->update(filteredPoses);
689  ROS_INFO("Octomap update time = %fs", time.ticks());
690  }
691 #endif
692 #endif
693  for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=gridMaps_.begin();
694  iter!=gridMaps_.end();)
695  {
696  if(!uContains(poses, iter->first))
697  {
698  UASSERT(gridMapsViewpoints_.erase(iter->first) != 0);
699  gridMaps_.erase(iter++);
700  }
701  else
702  {
703  ++iter;
704  }
705  }
706 
707  for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=groundClouds_.begin();
708  iter!=groundClouds_.end();)
709  {
710  if(!uContains(poses, iter->first))
711  {
712  groundClouds_.erase(iter++);
713  }
714  else
715  {
716  ++iter;
717  }
718  }
719 
720  for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=obstacleClouds_.begin();
721  iter!=obstacleClouds_.end();)
722  {
723  if(!uContains(poses, iter->first))
724  {
725  obstacleClouds_.erase(iter++);
726  }
727  else
728  {
729  ++iter;
730  }
731  }
732 
733  if(longUpdate)
734  {
735  ROS_WARN("Map(s) updated! (%f s)", longUpdateTimer.ticks());
736  }
737  }
738 
739  return filteredPoses;
740 }
741 
742 pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
743  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
744  const rtabmap::FlannIndex & substractCloudIndex,
745  float radiusSearch,
746  int minNeighborsInRadius)
747 {
748  UASSERT(minNeighborsInRadius > 0);
749  UASSERT(substractCloudIndex.indexedFeatures());
750 
751  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
752  output->resize(cloud->size());
753  int oi = 0; // output iterator
754  for(unsigned int i=0; i<cloud->size(); ++i)
755  {
756  std::vector<std::vector<size_t> > kIndices;
757  std::vector<std::vector<float> > kDistances;
758  cv::Mat pt = (cv::Mat_<float>(1, 3) << cloud->at(i).x, cloud->at(i).y, cloud->at(i).z);
759  substractCloudIndex.radiusSearch(pt, kIndices, kDistances, radiusSearch, minNeighborsInRadius, 32, 0, false);
760  if(kIndices.size() == 1 && kIndices[0].size() < minNeighborsInRadius)
761  {
762  output->at(oi++) = cloud->at(i);
763  }
764  }
765  output->resize(oi);
766  return output;
767 }
768 
769 void MapsManager::publishMaps(
770  const std::map<int, rtabmap::Transform> & poses,
771  const ros::Time & stamp,
772  const std::string & mapFrameId)
773 {
774  UDEBUG("Publishing maps...");
775 
776  // publish maps
777  if(cloudMapPub_.getNumSubscribers() ||
778  scanMapPub_.getNumSubscribers() ||
779  cloudObstaclesPub_.getNumSubscribers() ||
780  cloudGroundPub_.getNumSubscribers())
781  {
782  // generate the assembled cloud!
783  UTimer time;
784 
785  if(scanMapPub_.getNumSubscribers())
786  {
787  if(parameters_.find(Parameters::kGridFromDepth()) != parameters_.end() &&
788  uStr2Bool(parameters_.at(Parameters::kGridFromDepth())))
789  {
790  ROS_WARN("/scan_map topic is deprecated! Subscribe to /cloud_map topic "
791  "instead with <param name=\"%s\" type=\"string\" value=\"false\"/>. "
792  "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see "
793  "all occupancy grid parameters.",
794  Parameters::kGridFromDepth().c_str());
795  }
796  else
797  {
798  ROS_WARN("/scan_map topic is deprecated! Subscribe to /cloud_map topic instead.");
799  }
800  }
801 
802  // detect if the graph has changed, if so, recreate the clouds
803  bool graphGroundOptimized = false;
804  bool graphObstacleOptimized = false;
805  bool updateGround = cloudMapPub_.getNumSubscribers() ||
806  scanMapPub_.getNumSubscribers() ||
807  cloudGroundPub_.getNumSubscribers();
808  bool updateObstacles = cloudMapPub_.getNumSubscribers() ||
809  scanMapPub_.getNumSubscribers() ||
810  cloudObstaclesPub_.getNumSubscribers();
811  bool graphGroundChanged = updateGround;
812  bool graphObstacleChanged = updateObstacles;
813  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
814  {
815  std::map<int, Transform>::const_iterator jter;
816  if(updateGround)
817  {
818  jter = assembledGroundPoses_.find(iter->first);
819  if(jter != assembledGroundPoses_.end())
820  {
821  graphGroundChanged = false;
822  UASSERT(!iter->second.isNull() && !jter->second.isNull());
823  if(iter->second.getDistanceSquared(jter->second) > 0.0001)
824  {
825  graphGroundOptimized = true;
826  }
827  }
828  }
829  if(updateObstacles)
830  {
831  jter = assembledObstaclePoses_.find(iter->first);
832  if(jter != assembledObstaclePoses_.end())
833  {
834  graphObstacleChanged = false;
835  UASSERT(!iter->second.isNull() && !jter->second.isNull());
836  if(iter->second.getDistanceSquared(jter->second) > 0.0001)
837  {
838  graphObstacleOptimized = true;
839  }
840  }
841  }
842  }
843  int countObstacles = 0;
844  int countGrounds = 0;
845  int previousIndexedGroundSize = assembledGroundIndex_.indexedFeatures();
846  int previousIndexedObstacleSize = assembledObstacleIndex_.indexedFeatures();
847  if(graphGroundOptimized || graphGroundChanged)
848  {
849  int previousSize = assembledGround_->size();
850  assembledGround_->clear();
851  assembledGround_->reserve(previousSize);
852  assembledGroundPoses_.clear();
853  assembledGroundIndex_.release();
854  }
855  if(graphObstacleOptimized || graphObstacleChanged )
856  {
857  int previousSize = assembledObstacles_->size();
858  assembledObstacles_->clear();
859  assembledObstacles_->reserve(previousSize);
860  assembledObstaclePoses_.clear();
861  assembledObstacleIndex_.release();
862  }
863 
864  if(graphGroundOptimized || graphObstacleOptimized)
865  {
866  ROS_INFO("Graph has changed, updating clouds...");
867  UTimer t;
868  cv::Mat tmpGroundPts;
869  cv::Mat tmpObstaclePts;
870  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
871  {
872  if(iter->first > 0)
873  {
874  if(updateGround &&
875  (graphGroundOptimized || assembledGroundPoses_.find(iter->first) == assembledGroundPoses_.end()))
876  {
877  assembledGroundPoses_.insert(*iter);
878  std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator kter=groundClouds_.find(iter->first);
879  if(kter != groundClouds_.end() && kter->second->size())
880  {
881  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(kter->second, iter->second);
882  *assembledGround_+=*transformed;
883  if(cloudSubtractFiltering_)
884  {
885  for(unsigned int i=0; i<transformed->size(); ++i)
886  {
887  if(tmpGroundPts.empty())
888  {
889  tmpGroundPts = (cv::Mat_<float>(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z);
890  tmpGroundPts.reserve(previousIndexedGroundSize>0?previousIndexedGroundSize:100);
891  }
892  else
893  {
894  cv::Mat pt = (cv::Mat_<float>(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z);
895  tmpGroundPts.push_back(pt);
896  }
897  }
898  }
899  ++countGrounds;
900  }
901  }
902  if(updateObstacles &&
903  (graphObstacleOptimized || assembledObstaclePoses_.find(iter->first) == assembledObstaclePoses_.end()))
904  {
905  assembledObstaclePoses_.insert(*iter);
906  std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator kter=obstacleClouds_.find(iter->first);
907  if(kter != obstacleClouds_.end() && kter->second->size())
908  {
909  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(kter->second, iter->second);
910  *assembledObstacles_+=*transformed;
911  if(cloudSubtractFiltering_)
912  {
913  for(unsigned int i=0; i<transformed->size(); ++i)
914  {
915  if(tmpObstaclePts.empty())
916  {
917  tmpObstaclePts = (cv::Mat_<float>(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z);
918  tmpObstaclePts.reserve(previousIndexedObstacleSize>0?previousIndexedObstacleSize:100);
919  }
920  else
921  {
922  cv::Mat pt = (cv::Mat_<float>(1, 3) << transformed->at(i).x, transformed->at(i).y, transformed->at(i).z);
923  tmpObstaclePts.push_back(pt);
924  }
925  }
926  }
927  ++countObstacles;
928  }
929  }
930  }
931  }
932  double addingPointsTime = t.ticks();
933 
934  if(graphGroundOptimized && !tmpGroundPts.empty())
935  {
936  assembledGroundIndex_.buildKDTreeSingleIndex(tmpGroundPts, 15);
937  }
938  if(graphObstacleOptimized && !tmpObstaclePts.empty())
939  {
940  assembledObstacleIndex_.buildKDTreeSingleIndex(tmpObstaclePts, 15);
941  }
942  double indexingTime = t.ticks();
943  UINFO("Graph optimized! Time recreating clouds (%d ground, %d obstacles) = %f s (indexing %fs)", countGrounds, countObstacles, addingPointsTime+indexingTime, indexingTime);
944  }
945  else if(graphGroundChanged || graphObstacleChanged)
946  {
947  UWARN("Graph has changed! The whole cloud is regenerated.");
948  }
949 
950  for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
951  {
952  std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator jter = gridMaps_.find(iter->first);
953  if(updateGround && assembledGroundPoses_.find(iter->first) == assembledGroundPoses_.end())
954  {
955  if(iter->first > 0)
956  {
957  assembledGroundPoses_.insert(*iter);
958  }
959  if(jter!=gridMaps_.end() && jter->second.first.first.cols)
960  {
961  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.first), iter->second, 0, 255, 0);
962  pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed;
963  if(cloudSubtractFiltering_)
964  {
965  if(assembledGroundIndex_.indexedFeatures())
966  {
967  subtractedCloud = subtractFiltering(transformed, assembledGroundIndex_, occupancyGrid_->getCellSize(), cloudSubtractFilteringMinNeighbors_);
968  }
969  if(subtractedCloud->size())
970  {
971  UDEBUG("Adding ground %d pts=%d/%d (index=%d)", iter->first, subtractedCloud->size(), transformed->size(), assembledGroundIndex_.indexedFeatures());
972  cv::Mat pts(subtractedCloud->size(), 3, CV_32FC1);
973  for(unsigned int i=0; i<subtractedCloud->size(); ++i)
974  {
975  pts.at<float>(i, 0) = subtractedCloud->at(i).x;
976  pts.at<float>(i, 1) = subtractedCloud->at(i).y;
977  pts.at<float>(i, 2) = subtractedCloud->at(i).z;
978  }
979  if(!assembledGroundIndex_.isBuilt())
980  {
981  assembledGroundIndex_.buildKDTreeSingleIndex(pts, 15);
982  }
983  else
984  {
985  assembledGroundIndex_.addPoints(pts);
986  }
987  }
988  }
989  if(iter->first>0)
990  {
991  groundClouds_.insert(std::make_pair(iter->first, util3d::transformPointCloud(subtractedCloud, iter->second.inverse())));
992  }
993  if(subtractedCloud->size())
994  {
995  *assembledGround_+=*subtractedCloud;
996  }
997  ++countGrounds;
998  }
999  }
1000  if(updateObstacles && assembledObstaclePoses_.find(iter->first) == assembledObstaclePoses_.end())
1001  {
1002  if(iter->first > 0)
1003  {
1004  assembledObstaclePoses_.insert(*iter);
1005  }
1006  if(jter!=gridMaps_.end() && jter->second.first.second.cols)
1007  {
1008  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(jter->second.first.second), iter->second, 255, 0, 0);
1009  pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractedCloud = transformed;
1010  if(cloudSubtractFiltering_)
1011  {
1012  if(assembledObstacleIndex_.indexedFeatures())
1013  {
1014  subtractedCloud = subtractFiltering(transformed, assembledObstacleIndex_, occupancyGrid_->getCellSize(), cloudSubtractFilteringMinNeighbors_);
1015  }
1016  if(subtractedCloud->size())
1017  {
1018  UDEBUG("Adding obstacle %d pts=%d/%d (index=%d)", iter->first, subtractedCloud->size(), transformed->size(), assembledObstacleIndex_.indexedFeatures());
1019  cv::Mat pts(subtractedCloud->size(), 3, CV_32FC1);
1020  for(unsigned int i=0; i<subtractedCloud->size(); ++i)
1021  {
1022  pts.at<float>(i, 0) = subtractedCloud->at(i).x;
1023  pts.at<float>(i, 1) = subtractedCloud->at(i).y;
1024  pts.at<float>(i, 2) = subtractedCloud->at(i).z;
1025  }
1026  if(!assembledObstacleIndex_.isBuilt())
1027  {
1028  assembledObstacleIndex_.buildKDTreeSingleIndex(pts, 15);
1029  }
1030  else
1031  {
1032  assembledObstacleIndex_.addPoints(pts);
1033  }
1034  }
1035  }
1036  if(iter->first>0)
1037  {
1038  obstacleClouds_.insert(std::make_pair(iter->first, util3d::transformPointCloud(subtractedCloud, iter->second.inverse())));
1039  }
1040  if(subtractedCloud->size())
1041  {
1042  *assembledObstacles_+=*subtractedCloud;
1043  }
1044  ++countObstacles;
1045  }
1046  }
1047  }
1048 
1049  if(cloudOutputVoxelized_)
1050  {
1051  UASSERT(occupancyGrid_->getCellSize() > 0.0);
1052  if(countGrounds && assembledGround_->size())
1053  {
1054  assembledGround_ = util3d::voxelize(assembledGround_, occupancyGrid_->getCellSize());
1055  }
1056  if(countObstacles && assembledObstacles_->size())
1057  {
1058  assembledObstacles_ = util3d::voxelize(assembledObstacles_, occupancyGrid_->getCellSize());
1059  }
1060  }
1061 
1062  ROS_INFO("Assembled %d obstacle and %d ground clouds (%d points, %fs)",
1063  countObstacles, countGrounds, (int)(assembledGround_->size() + assembledObstacles_->size()), time.ticks());
1064 
1065  if(cloudGroundPub_.getNumSubscribers())
1066  {
1067  sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
1068  pcl::toROSMsg(*assembledGround_, *cloudMsg);
1069  cloudMsg->header.stamp = stamp;
1070  cloudMsg->header.frame_id = mapFrameId;
1071  cloudGroundPub_.publish(cloudMsg);
1072  }
1073  if(cloudObstaclesPub_.getNumSubscribers())
1074  {
1075  sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
1076  pcl::toROSMsg(*assembledObstacles_, *cloudMsg);
1077  cloudMsg->header.stamp = stamp;
1078  cloudMsg->header.frame_id = mapFrameId;
1079  cloudObstaclesPub_.publish(cloudMsg);
1080  }
1081  if(cloudMapPub_.getNumSubscribers() || scanMapPub_.getNumSubscribers())
1082  {
1083  pcl::PointCloud<pcl::PointXYZRGB> cloud = *assembledObstacles_ + *assembledGround_;
1084  sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
1085  pcl::toROSMsg(cloud, *cloudMsg);
1086  cloudMsg->header.stamp = stamp;
1087  cloudMsg->header.frame_id = mapFrameId;
1088 
1089  if(cloudMapPub_.getNumSubscribers())
1090  {
1091  cloudMapPub_.publish(cloudMsg);
1092  }
1093  if(scanMapPub_.getNumSubscribers())
1094  {
1095  scanMapPub_.publish(cloudMsg);
1096  }
1097  }
1098  }
1099  else if(mapCacheCleanup_)
1100  {
1101  assembledGround_->clear();
1102  assembledObstacles_->clear();
1103  assembledGroundPoses_.clear();
1104  assembledObstaclePoses_.clear();
1105  assembledGroundIndex_.release();
1106  assembledObstacleIndex_.release();
1107  groundClouds_.clear();
1108  obstacleClouds_.clear();
1109  }
1110 
1111 #ifdef WITH_OCTOMAP_MSGS
1112 #ifdef RTABMAP_OCTOMAP
1113  if(octoMapPubBin_.getNumSubscribers() ||
1114  octoMapPubFull_.getNumSubscribers() ||
1115  octoMapCloud_.getNumSubscribers() ||
1116  octoMapObstacleCloud_.getNumSubscribers() ||
1117  octoMapGroundCloud_.getNumSubscribers() ||
1118  octoMapEmptySpace_.getNumSubscribers() ||
1119  octoMapProj_.getNumSubscribers())
1120  {
1121  if(octoMapPubBin_.getNumSubscribers())
1122  {
1123  octomap_msgs::Octomap msg;
1124  octomap_msgs::binaryMapToMsg(*octomap_->octree(), msg);
1125  msg.header.frame_id = mapFrameId;
1126  msg.header.stamp = stamp;
1127  octoMapPubBin_.publish(msg);
1128  }
1129  if(octoMapPubFull_.getNumSubscribers())
1130  {
1131  octomap_msgs::Octomap msg;
1132  octomap_msgs::fullMapToMsg(*octomap_->octree(), msg);
1133  msg.header.frame_id = mapFrameId;
1134  msg.header.stamp = stamp;
1135  octoMapPubFull_.publish(msg);
1136  }
1137  if(octoMapCloud_.getNumSubscribers() ||
1138  octoMapObstacleCloud_.getNumSubscribers() ||
1139  octoMapGroundCloud_.getNumSubscribers() ||
1140  octoMapEmptySpace_.getNumSubscribers())
1141  {
1142  sensor_msgs::PointCloud2 msg;
1143  pcl::IndicesPtr obstacleIndices(new std::vector<int>);
1144  pcl::IndicesPtr emptyIndices(new std::vector<int>);
1145  pcl::IndicesPtr groundIndices(new std::vector<int>);
1146  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacleIndices.get(), emptyIndices.get(), groundIndices.get());
1147 
1148  if(octoMapCloud_.getNumSubscribers())
1149  {
1150  pcl::PointCloud<pcl::PointXYZRGB> cloudOccupiedSpace;
1151  pcl::IndicesPtr indices = util3d::concatenate(obstacleIndices, groundIndices);
1152  pcl::copyPointCloud(*cloud, *indices, cloudOccupiedSpace);
1153  pcl::toROSMsg(cloudOccupiedSpace, msg);
1154  msg.header.frame_id = mapFrameId;
1155  msg.header.stamp = stamp;
1156  octoMapCloud_.publish(msg);
1157  }
1158  if(octoMapObstacleCloud_.getNumSubscribers())
1159  {
1160  pcl::PointCloud<pcl::PointXYZRGB> cloudObstacles;
1161  pcl::copyPointCloud(*cloud, *obstacleIndices, cloudObstacles);
1162  pcl::toROSMsg(cloudObstacles, msg);
1163  msg.header.frame_id = mapFrameId;
1164  msg.header.stamp = stamp;
1165  octoMapObstacleCloud_.publish(msg);
1166  }
1167  if(octoMapGroundCloud_.getNumSubscribers())
1168  {
1169  pcl::PointCloud<pcl::PointXYZRGB> cloudGround;
1170  pcl::copyPointCloud(*cloud, *groundIndices, cloudGround);
1171  pcl::toROSMsg(cloudGround, msg);
1172  msg.header.frame_id = mapFrameId;
1173  msg.header.stamp = stamp;
1174  octoMapGroundCloud_.publish(msg);
1175  }
1176  if(octoMapEmptySpace_.getNumSubscribers())
1177  {
1178  pcl::PointCloud<pcl::PointXYZRGB> cloudEmptySpace;
1179  pcl::copyPointCloud(*cloud, *emptyIndices, cloudEmptySpace);
1180  pcl::toROSMsg(cloudEmptySpace, msg);
1181  msg.header.frame_id = mapFrameId;
1182  msg.header.stamp = stamp;
1183  octoMapEmptySpace_.publish(msg);
1184  }
1185  }
1186  if(octoMapProj_.getNumSubscribers())
1187  {
1188  // create the projection map
1189  float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
1190  cv::Mat pixels = octomap_->createProjectionMap(xMin, yMin, gridCellSize, occupancyGrid_->getMinMapSize(), octomapTreeDepth_);
1191 
1192  if(!pixels.empty())
1193  {
1194  //init
1195  nav_msgs::OccupancyGrid map;
1196  map.info.resolution = gridCellSize;
1197  map.info.origin.position.x = 0.0;
1198  map.info.origin.position.y = 0.0;
1199  map.info.origin.position.z = 0.0;
1200  map.info.origin.orientation.x = 0.0;
1201  map.info.origin.orientation.y = 0.0;
1202  map.info.origin.orientation.z = 0.0;
1203  map.info.origin.orientation.w = 1.0;
1204 
1205  map.info.width = pixels.cols;
1206  map.info.height = pixels.rows;
1207  map.info.origin.position.x = xMin;
1208  map.info.origin.position.y = yMin;
1209  map.data.resize(map.info.width * map.info.height);
1210 
1211  memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
1212 
1213  map.header.frame_id = mapFrameId;
1214  map.header.stamp = stamp;
1215 
1216  octoMapProj_.publish(map);
1217  }
1218  else if(poses.size())
1219  {
1220  ROS_WARN("Octomap projection map is empty! (poses=%d octomap nodes=%d). "
1221  "Make sure you activated \"%s\" and \"%s\" to true. "
1222  "See \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" for more info.",
1223  (int)poses.size(), (int)octomap_->octree()->size(),
1224  Parameters::kGrid3D().c_str(), Parameters::kGridFromDepth().c_str());
1225  }
1226  }
1227  }
1228  else if(mapCacheCleanup_)
1229  {
1230  octomap_->clear();
1231  }
1232 #endif
1233 #endif
1234 
1235  if(gridMapPub_.getNumSubscribers() || projMapPub_.getNumSubscribers() || gridProbMapPub_.getNumSubscribers())
1236  {
1237  if(projMapPub_.getNumSubscribers())
1238  {
1239  if(parameters_.find(Parameters::kGridFromDepth()) != parameters_.end() &&
1240  !uStr2Bool(parameters_.at(Parameters::kGridFromDepth())))
1241  {
1242  ROS_WARN("/proj_map topic is deprecated! Subscribe to /grid_map topic "
1243  "instead with <param name=\"%s\" type=\"string\" value=\"true\"/>. "
1244  "Do \"$ rosrun rtabmap_ros rtabmap --params | grep Grid\" to see "
1245  "all occupancy grid parameters.",
1246  Parameters::kGridFromDepth().c_str());
1247  }
1248  else
1249  {
1250  ROS_WARN("/proj_map topic is deprecated! Subscribe to /grid_map topic instead.");
1251  }
1252  }
1253 
1254  if(gridProbMapPub_.getNumSubscribers())
1255  {
1256  // create the grid map
1257  float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
1258  cv::Mat pixels = this->getGridProbMap(xMin, yMin, gridCellSize);
1259  if(!pixels.empty())
1260  {
1261  //init
1262  nav_msgs::OccupancyGrid map;
1263  map.info.resolution = gridCellSize;
1264  map.info.origin.position.x = 0.0;
1265  map.info.origin.position.y = 0.0;
1266  map.info.origin.position.z = 0.0;
1267  map.info.origin.orientation.x = 0.0;
1268  map.info.origin.orientation.y = 0.0;
1269  map.info.origin.orientation.z = 0.0;
1270  map.info.origin.orientation.w = 1.0;
1271 
1272  map.info.width = pixels.cols;
1273  map.info.height = pixels.rows;
1274  map.info.origin.position.x = xMin;
1275  map.info.origin.position.y = yMin;
1276  map.data.resize(map.info.width * map.info.height);
1277 
1278  memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
1279 
1280  map.header.frame_id = mapFrameId;
1281  map.header.stamp = stamp;
1282 
1283  if(gridProbMapPub_.getNumSubscribers())
1284  {
1285  gridProbMapPub_.publish(map);
1286  }
1287  }
1288  else if(poses.size())
1289  {
1290  ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size());
1291  }
1292  }
1293  if(gridMapPub_.getNumSubscribers() || projMapPub_.getNumSubscribers())
1294  {
1295  // create the grid map
1296  float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
1297  cv::Mat pixels = this->getGridMap(xMin, yMin, gridCellSize);
1298 
1299  if(!pixels.empty())
1300  {
1301  //init
1302  nav_msgs::OccupancyGrid map;
1303  map.info.resolution = gridCellSize;
1304  map.info.origin.position.x = 0.0;
1305  map.info.origin.position.y = 0.0;
1306  map.info.origin.position.z = 0.0;
1307  map.info.origin.orientation.x = 0.0;
1308  map.info.origin.orientation.y = 0.0;
1309  map.info.origin.orientation.z = 0.0;
1310  map.info.origin.orientation.w = 1.0;
1311 
1312  map.info.width = pixels.cols;
1313  map.info.height = pixels.rows;
1314  map.info.origin.position.x = xMin;
1315  map.info.origin.position.y = yMin;
1316  map.data.resize(map.info.width * map.info.height);
1317 
1318  memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
1319 
1320  map.header.frame_id = mapFrameId;
1321  map.header.stamp = stamp;
1322 
1323  if(gridMapPub_.getNumSubscribers())
1324  {
1325  gridMapPub_.publish(map);
1326  }
1327  if(projMapPub_.getNumSubscribers())
1328  {
1329  projMapPub_.publish(map);
1330  }
1331  }
1332  else if(poses.size())
1333  {
1334  ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size());
1335  }
1336  }
1337  }
1338 
1339  if(!this->hasSubscribers() && mapCacheCleanup_)
1340  {
1341  gridMaps_.clear();
1342  gridMapsViewpoints_.clear();
1343  }
1344 }
1345 
1346 cv::Mat MapsManager::getGridMap(
1347  float & xMin,
1348  float & yMin,
1349  float & gridCellSize)
1350 {
1351  gridCellSize = occupancyGrid_->getCellSize();
1352  return occupancyGrid_->getMap(xMin, yMin);
1353 }
1354 
1355 cv::Mat MapsManager::getGridProbMap(
1356  float & xMin,
1357  float & yMin,
1358  float & gridCellSize)
1359 {
1360  gridCellSize = occupancyGrid_->getCellSize();
1361  return occupancyGrid_->getProbMap(xMin, yMin);
1362 }
1363 
MapsManager::gridMapsViewpoints_
std::map< int, cv::Point3f > gridMapsViewpoints_
Definition: MapsManager.h:123
MapsManager::set2DMap
void set2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize, const std::map< int, rtabmap::Transform > &poses, const rtabmap::Memory *memory=0)
Definition: MapsManager.cpp:293
rtabmap::OccupancyGrid::getMinMapSize
float getMinMapSize() const
MapsManager::getFilteredPoses
std::map< int, rtabmap::Transform > getFilteredPoses(const std::map< int, rtabmap::Transform > &poses)
Definition: MapsManager.cpp:379
MapsManager::cloudSubtractFilteringMinNeighbors_
int cloudSubtractFilteringMinNeighbors_
Definition: MapsManager.h:89
MapsManager::octoMapObstacleCloud_
ros::Publisher octoMapObstacleCloud_
Definition: MapsManager.h:107
MapsManager::negativeScanEmptyRayTracing_
bool negativeScanEmptyRayTracing_
Definition: MapsManager.h:94
MapsManager::setParameters
void setParameters(const rtabmap::ParametersMap &parameters)
Definition: MapsManager.cpp:276
ros::NodeHandle
MapsManager::cloudGroundPub_
ros::Publisher cloudGroundPub_
Definition: MapsManager.h:97
rtabmap::OccupancyGrid::getCellSize
float getCellSize() const
rtabmap::graph::radiusPosesFiltering
std::map< int, Transform > RTABMAP_EXP radiusPosesFiltering(const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
uStr2Double
double uStr2Double(const std::string &str)
rtabmap::OctoMap::update
void update(const std::map< int, Transform > &poses)
MapsManager::gridMapPub_
ros::Publisher gridMapPub_
Definition: MapsManager.h:100
MapsManager::occupancyGrid_
rtabmap::OccupancyGrid * occupancyGrid_
Definition: MapsManager.h:125
uStrNumCmp
int uStrNumCmp(const std::string &a, const std::string &b)
ParametersPair
std::pair< std::string, std::string > ParametersPair
MapsManager::scanMapPub_
ros::Publisher scanMapPub_
Definition: MapsManager.h:102
octomap_msgs::fullMapToMsg
static bool fullMapToMsg(const OctomapT &octomap, Octomap &msg)
MapsManager::cloudOutputVoxelized_
bool cloudOutputVoxelized_
Definition: MapsManager.h:87
MapsManager::octoMapCloud_
ros::Publisher octoMapCloud_
Definition: MapsManager.h:105
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
MapsManager::getGridProbMap
cv::Mat getGridProbMap(float &xMin, float &yMin, float &gridCellSize)
Definition: MapsManager.cpp:1355
rtabmap::SensorData::gridCellSize
float gridCellSize() const
angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
rtabmap::OccupancyGrid::update
void update(const std::map< int, Transform > &poses)
PointCloud
sensor_msgs::PointCloud2 PointCloud
rtabmap::FlannIndex::radiusSearch
void radiusSearch(const cv::Mat &query, std::vector< std::vector< size_t > > &indices, std::vector< std::vector< float > > &dists, float radius, int maxNeighbors=0, int checks=32, float eps=0.0, bool sorted=true) const
MapsManager::assembledObstacles_
pcl::PointCloud< pcl::PointXYZRGB >::Ptr assembledObstacles_
Definition: MapsManager.h:113
MapsManager::groundClouds_
std::map< int, pcl::PointCloud< pcl::PointXYZRGB >::Ptr > groundClouds_
Definition: MapsManager.h:117
MapsManager::octomap_
rtabmap::OctoMap * octomap_
Definition: MapsManager.h:127
rtabmap::Memory::getDatabaseVersion
std::string getDatabaseVersion() const
rtabmap::FlannIndex::addPoints
unsigned int addPoints(const cv::Mat &features)
ROS_WARN
#define ROS_WARN(...)
MapsManager::cloudSubtractFiltering_
bool cloudSubtractFiltering_
Definition: MapsManager.h:88
octomap_msgs::binaryMapToMsg
static bool binaryMapToMsg(const OctomapT &octomap, Octomap &msg)
uBool2Str
std::string uBool2Str(bool boolean)
rtabmap::OccupancyGrid::setMap
void setMap(const cv::Mat &map, float xMin, float yMin, float cellSize, const std::map< int, Transform > &poses)
MapsManager::parameters_
rtabmap::ParametersMap parameters_
Definition: MapsManager.h:130
MapsManager::octoMapPubFull_
ros::Publisher octoMapPubFull_
Definition: MapsManager.h:104
util3d_mapping.h
UASSERT
#define UASSERT(condition)
MapsManager::init
void init(ros::NodeHandle &nh, ros::NodeHandle &pnh, const std::string &name, bool usePublicNamespace)
Definition: MapsManager.cpp:76
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)
rtabmap::OctoMap::addedNodes
const std::map< int, Transform > & addedNodes() const
uStr2Int
int uStr2Int(const std::string &str)
uNumber2Str
std::string uNumber2Str(unsigned int number)
rtabmap::OccupancyGrid::createLocalMap
void createLocalMap(const Signature &node, cv::Mat &groundCells, cv::Mat &obstacleCells, cv::Mat &emptyCells, cv::Point3f &viewPoint) const
true
#define true
MapsManager::cloudMapPub_
ros::Publisher cloudMapPub_
Definition: MapsManager.h:96
MapsManager::octoMapPubBin_
ros::Publisher octoMapPubBin_
Definition: MapsManager.h:103
MapsManager::updateMapCaches
std::map< int, rtabmap::Transform > updateMapCaches(const std::map< int, rtabmap::Transform > &poses, const rtabmap::Memory *memory, bool updateGrid, bool updateOctomap, const std::map< int, rtabmap::Signature > &signatures=std::map< int, rtabmap::Signature >())
Definition: MapsManager.cpp:390
rtabmap::OccupancyGrid::addToCache
void addToCache(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty)
rtabmap::FlannIndex::indexedFeatures
unsigned int indexedFeatures() const
util3d_filtering.h
ROS_INFO
#define ROS_INFO(...)
MapsManager::gridMaps_
std::map< int, std::pair< std::pair< cv::Mat, cv::Mat >, cv::Mat > > gridMaps_
Definition: MapsManager.h:122
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
pcl_conversions.h
MapsManager::assembledGroundIndex_
rtabmap::FlannIndex assembledGroundIndex_
Definition: MapsManager.h:115
rtabmap::FlannIndex::buildKDTreeSingleIndex
void buildKDTreeSingleIndex(const cv::Mat &features, int leafMaxSize=10, bool reorder=true, bool useDistanceL1=false, float rebalancingFactor=2.0f)
MapsManager::projMapPub_
ros::Publisher projMapPub_
Definition: MapsManager.h:99
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) const
MapsManager::mapFilterAngle_
double mapFilterAngle_
Definition: MapsManager.h:91
rtabmap::OctoMap::octree
const RtabmapColorOcTree * octree() const
rtabmap::OccupancyGrid::getProbMap
cv::Mat getProbMap(float &xMin, float &yMin) const
ParametersMap
QMap< QString, QVariant > ParametersMap
rtabmap::Memory::getSignatureDataConst
SensorData getSignatureDataConst(int locationId, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
MapsManager::~MapsManager
virtual ~MapsManager()
Definition: MapsManager.cpp:164
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
uStr2Bool
bool uStr2Bool(const char *str)
rtabmap::OccupancyGrid::isFullUpdate
bool isFullUpdate() const
util3d_transforms.h
uContains
bool uContains(const std::list< V > &list, const V &value)
rtabmap::Signature::setPose
void setPose(const Transform &pose)
MapsManager::negativePosesIgnored_
bool negativePosesIgnored_
Definition: MapsManager.h:93
MapsManager::assembledObstacleIndex_
rtabmap::FlannIndex assembledObstacleIndex_
Definition: MapsManager.h:116
false
#define false
MapsManager::mapCacheCleanup_
bool mapCacheCleanup_
Definition: MapsManager.h:92
MapsManager::publishMaps
void publishMaps(const std::map< int, rtabmap::Transform > &poses, const ros::Time &stamp, const std::string &mapFrameId)
Definition: MapsManager.cpp:769
parameterMoved
void parameterMoved(ros::NodeHandle &nh, const std::string &rosName, const std::string &parameterName, ParametersMap &parameters)
Definition: MapsManager.cpp:180
MapsManager::mapFilterRadius_
double mapFilterRadius_
Definition: MapsManager.h:90
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
subtractFiltering
pcl::PointCloud< pcl::PointXYZRGB >::Ptr subtractFiltering(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const rtabmap::FlannIndex &substractCloudIndex, float radiusSearch, int minNeighborsInRadius)
Definition: MapsManager.cpp:742
UDEBUG
#define UDEBUG(...)
MapsManager::hasSubscribers
bool hasSubscribers() const
Definition: MapsManager.cpp:361
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
UWARN
#define UWARN(...)
MapsManager::octomapTreeDepth_
int octomapTreeDepth_
Definition: MapsManager.h:128
pcl::toROSMsg
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
MapsManager::octoMapEmptySpace_
ros::Publisher octoMapEmptySpace_
Definition: MapsManager.h:108
UTimer::ticks
double ticks()
MapsManager::octoMapGroundCloud_
ros::Publisher octoMapGroundCloud_
Definition: MapsManager.h:106
MapsManager::obstacleClouds_
std::map< int, pcl::PointCloud< pcl::PointXYZRGB >::Ptr > obstacleClouds_
Definition: MapsManager.h:118
rtabmap::OccupancyGrid::parseParameters
void parseParameters(const ParametersMap &parameters)
rtabmap::OccupancyGrid::getUpdateError
float getUpdateError() const
MapsManager::getGridMap
cv::Mat getGridMap(float &xMin, float &yMin, float &gridCellSize)
Definition: MapsManager.cpp:1346
MapsManager::octoMapProj_
ros::Publisher octoMapProj_
Definition: MapsManager.h:109
ros::NodeHandle::hasParam
bool hasParam(const std::string &key) const
rtabmap::OctoMap::createProjectionMap
cv::Mat createProjectionMap(float &xMin, float &yMin, float &gridCellSize, float minGridSize=0.0f, unsigned int treeDepth=0)
MapsManager::assembledGround_
pcl::PointCloud< pcl::PointXYZRGB >::Ptr assembledGround_
Definition: MapsManager.h:114
ROS_ERROR
#define ROS_ERROR(...)
rtabmap::OccupancyGrid::isGridFromDepth
bool isGridFromDepth() const
MapsManager::cloudObstaclesPub_
ros::Publisher cloudObstaclesPub_
Definition: MapsManager.h:98
rtabmap::OccupancyGrid::getMap
cv::Mat getMap(float &xMin, float &yMin) const
MapsManager::gridProbMapPub_
ros::Publisher gridProbMapPub_
Definition: MapsManager.h:101
rtabmap::OccupancyGrid::addedNodes
const std::map< int, Transform > & addedNodes() const
rtabmap::SensorData::gridViewPoint
const cv::Point3f & gridViewPoint() const
MapsManager::assembledGroundPoses_
std::map< int, rtabmap::Transform > assembledGroundPoses_
Definition: MapsManager.h:111
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
MapsManager::assembledObstaclePoses_
std::map< int, rtabmap::Transform > assembledObstaclePoses_
Definition: MapsManager.h:112
MapsManager::backwardCompatibilityParameters
void backwardCompatibilityParameters(ros::NodeHandle &pnh, rtabmap::ParametersMap &parameters) const
Definition: MapsManager.cpp:233
UINFO
#define UINFO(...)

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Fri Jun 7 2019 21:55:04