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

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:42:19