diff --git a/rtabmap_util/src/MapsManager.cpp b/rtabmap_util/src/MapsManager.cpp
index 95228800..28209840 100644
--- a/rtabmap_util/src/MapsManager.cpp
+++ b/rtabmap_util/src/MapsManager.cpp
@@ -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);