i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-09 03:07:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed regression #1361 added from #1352

Browse Source
This commit is contained in:
matlabbe
2025-09-25 00:50:00 -07:00
parent bda8f16bb8
commit 6e84c77d90
1 changed files with 19 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/src/MapsManager.cpp
+19 -5
View File
@@ -480,16 +480,30 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
filteredPoses.erase(0);
}
const std::map<int, rtabmap::Transform> emptyNodes;
const std::map<int, rtabmap::Transform> * addedNodes = &emptyNodes;
bool fullUpdateNeeded = true;
#if RTABMAP_VERSION_MAJOR>0 || (RTABMAP_VERSION_MAJOR==0 && RTABMAP_VERSION_MINOR>=23)
fullUpdateNeeded = (updateGrid && occupancyGrid_->fullUpdateNeeded(filteredPoses))
if(updateGrid) {
fullUpdateNeeded = occupancyGrid_->fullUpdateNeeded(filteredPoses);
addedNodes = &occupancyGrid_->addedNodes();
}
#if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
|| (updateOctomap && octomap_->fullUpdateNeeded(filteredPoses))
if(updateOctomap) {
fullUpdateNeeded = fullUpdateNeeded || octomap_->fullUpdateNeeded(filteredPoses);
if(octomap_->addedNodes().size() < addedNodes->size()) {
addedNodes = &octomap_->addedNodes();
}
}
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
|| (updateElevation && elevationMap_->fullUpdateNeeded(filteredPoses))
if(updateElevation) {
fullUpdateNeeded = fullUpdateNeeded || elevationMap_->fullUpdateNeeded(filteredPoses);
if(elevationMap_->addedNodes().size() < addedNodes->size()) {
addedNodes = &elevationMap_->addedNodes();
}
}
#endif
;
if(fullUpdateNeeded) {
UINFO("Full occupancy grid map update needed");
}
@@ -512,7 +526,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
if(!iter->second.isNull())
{
rtabmap::SensorData data;
if(iter->first == 0 || (fullUpdateNeeded && !uContains(localMaps_.localGrids(), iter->first)))
if(iter->first == 0 || (addedNodes->find(iter->first) == addedNodes->end() && !uContains(localMaps_.localGrids(), iter->first)))
{
UDEBUG("Data required for %d", iter->first);
std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);