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

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40