From ca4d33151cc59bf9b8bffb7231ef29e308903b64 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 18 Aug 2025 16:59:12 -0700 Subject: [PATCH 1/4] rtabmap node: added map_cache_loaded_on_init parameter (default true, like before) to avoid (when false) loading all local grids in cache on init (in case we know we are in localization mode and not using memory management) --- rtabmap_util/include/rtabmap_util/MapsManager.h | 1 + rtabmap_util/src/MapsManager.cpp | 7 ++++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/rtabmap_util/include/rtabmap_util/MapsManager.h b/rtabmap_util/include/rtabmap_util/MapsManager.h index 311ca24c..5c013f9b 100644 --- a/rtabmap_util/include/rtabmap_util/MapsManager.h +++ b/rtabmap_util/include/rtabmap_util/MapsManager.h @@ -100,6 +100,7 @@ private: bool mapCacheCleanup_; bool alwaysUpdateMap_; bool scanEmptyRayTracing_; + bool localMapsCacheLoadedOnInit_; ros::Publisher cloudMapPub_; ros::Publisher cloudGroundPub_; diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 1ee902f2..2a1d12ec 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -70,6 +70,7 @@ MapsManager::MapsManager() : mapCacheCleanup_(true), alwaysUpdateMap_(false), scanEmptyRayTracing_(true), + localMapsCacheLoadedOnInit_(true), assembledObstacles_(new pcl::PointCloud), assembledGround_(new pcl::PointCloud), occupancyGrid_(new OccupancyGrid(&localMaps_)), @@ -122,6 +123,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s } } pnh.param("map_empty_ray_tracing", scanEmptyRayTracing_, scanEmptyRayTracing_); + pnh.param("map_cache_loaded_on_init", localMapsCacheLoadedOnInit_, localMapsCacheLoadedOnInit_); if(pnh.hasParam("scan_output_voxelized")) { @@ -141,6 +143,7 @@ void MapsManager::init(ros::NodeHandle & nh, ros::NodeHandle & pnh, const std::s ROS_INFO("%s(maps): map_cleanup = %s", name.c_str(), mapCacheCleanup_?"true":"false"); ROS_INFO("%s(maps): map_always_update = %s", name.c_str(), alwaysUpdateMap_?"true":"false"); ROS_INFO("%s(maps): map_empty_ray_tracing = %s", name.c_str(), scanEmptyRayTracing_?"true":"false"); + ROS_INFO("%s(maps): map_cache_loaded_on_init = %s", name.c_str(), localMapsCacheLoadedOnInit_?"true":"false"); ROS_INFO("%s(maps): cloud_output_voxelized = %s", name.c_str(), cloudOutputVoxelized_?"true":"false"); ROS_INFO("%s(maps): cloud_subtract_filtering = %s", name.c_str(), cloudSubtractFiltering_?"true":"false"); ROS_INFO("%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_); @@ -355,7 +358,9 @@ void MapsManager::set2DMap( { occupancyGrid_->setMap(map, xMin, yMin, cellSize, poses); //update cache in case the map should be updated - if(memory) + if(memory && + uStrNumCmp(memory->getDatabaseVersion(), "0.11.10")>=0 && // versions 0.11.10+ have local grids saved in db + localMapsCacheLoadedOnInit_) { for(std::map::const_iterator iter=poses.lower_bound(1); iter!=poses.end(); ++iter) { From e9bed4a6b06a86222a28e32193c6ac83f011ea7f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 2 Sep 2025 14:28:55 -0700 Subject: [PATCH 2/4] Fixed build against rtabmap 0.23 --- rtabmap_util/src/MapsManager.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 2a1d12ec..9ad16c45 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -944,11 +944,19 @@ void MapsManager::publishMaps( if(graphGroundOptimized && !tmpGroundPts.empty()) { +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + assembledGroundIndex_.buildIndex(FlannIndex::FLANN_INDEX_KDTREE_SINGLE, tmpGroundPts); +#else assembledGroundIndex_.buildKDTreeSingleIndex(tmpGroundPts, 15); +#endif } if(graphObstacleOptimized && !tmpObstaclePts.empty()) { +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + assembledObstacleIndex_.buildIndex(FlannIndex::FLANN_INDEX_KDTREE_SINGLE, tmpObstaclePts); +#else assembledObstacleIndex_.buildKDTreeSingleIndex(tmpObstaclePts, 15); +#endif } double indexingTime = t.ticks(); ROS_INFO("Graph optimized! Time recreating clouds (%d ground, %d obstacles) = %f s (indexing %fs)", countGrounds, countObstacles, addingPointsTime+indexingTime, indexingTime); @@ -989,7 +997,11 @@ void MapsManager::publishMaps( } if(!assembledGroundIndex_.isBuilt()) { +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + assembledGroundIndex_.buildIndex(FlannIndex::FLANN_INDEX_KDTREE_SINGLE, pts); +#else assembledGroundIndex_.buildKDTreeSingleIndex(pts, 15); +#endif } else { @@ -1036,7 +1048,11 @@ void MapsManager::publishMaps( } if(!assembledObstacleIndex_.isBuilt()) { +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + assembledObstacleIndex_.buildIndex(FlannIndex::FLANN_INDEX_KDTREE_SINGLE, pts); +#else assembledObstacleIndex_.buildKDTreeSingleIndex(pts, 15); +#endif } else { From deee14e1d250eb9789355f25763321ff7870ed03 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 3 Sep 2025 16:59:24 -0700 Subject: [PATCH 3/4] Fixing map_cache_loaded_on_init loading anyway on first update (#1352) --- rtabmap_util/src/MapsManager.cpp | 37 +++++++++++++++++++------------- 1 file changed, 22 insertions(+), 15 deletions(-) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 9ad16c45..0429508f 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -551,32 +551,39 @@ std::map MapsManager::updateMapCaches( filteredPoses.erase(0); } + bool fullUpdateNeeded = true; +#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23) + fullUpdateNeeded = (updateGrid && occupancyGrid_->fullUpdateNeeded(filteredPoses)) +#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) + || (updateOctomap && octomap_->fullUpdateNeeded(filteredPoses)) +#endif +#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP) + || (updateElevation && elevationMap_->fullUpdateNeeded(filteredPoses)) +#endif + ; + if(fullUpdateNeeded) { + ROS_INFO("Full occupancy grid map update needed"); + } + else { + ROS_DEBUG("Full occupancy grid map update not needed"); + } +#endif + bool longUpdate = false; UTimer longUpdateTimer; - if(filteredPoses.size() > 20) + if(fullUpdateNeeded && filteredPoses.size() > 20 & localMaps_.size() < 5) { - if(updateGridCache && localMaps_.size() < 5) - { - ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-localMaps_.size())); - longUpdate = true; - } -#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP) - if(updateOctomap && octomap_->addedNodes().size() < 5) - { - 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())); - longUpdate = true; - } -#endif + ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-localMaps_.size())); + longUpdate = true; } bool occupancySavedInDB = memory && uStrNumCmp(memory->getDatabaseVersion(), "0.11.10")>=0?true:false; - for(std::map::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter) { if(!iter->second.isNull()) { rtabmap::SensorData data; - if(updateGridCache && (iter->first == 0 || !uContains(localMaps_.localGrids(), iter->first))) + if(iter->first == 0 || (fullUpdateNeeded && !uContains(localMaps_.localGrids(), iter->first))) { ROS_DEBUG("Data required for %d", iter->first); std::map::const_iterator findIter = signatures.find(iter->first); From 3fd34c64f47de42a53aad6cfbe2932a8d7f13b54 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 7 Sep 2025 11:49:38 -0700 Subject: [PATCH 4/4] fixed typo --- rtabmap_util/src/MapsManager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp index 0429508f..59db3fa6 100644 --- a/rtabmap_util/src/MapsManager.cpp +++ b/rtabmap_util/src/MapsManager.cpp @@ -571,7 +571,7 @@ std::map MapsManager::updateMapCaches( bool longUpdate = false; UTimer longUpdateTimer; - if(fullUpdateNeeded && filteredPoses.size() > 20 & localMaps_.size() < 5) + if(fullUpdateNeeded && filteredPoses.size() > 20 && localMaps_.size() < 5) { ROS_WARN("Many occupancy grids should be loaded (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-localMaps_.size())); longUpdate = true;