From 99ab4051406609505070bad9c33a6fc399f40325 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Tue, 15 Mar 2016 17:04:45 -0400
Subject: [PATCH] MapCloud rviz plugin: fixed memory increasing issue on
 localization mode

---
 src/rviz/MapCloudDisplay.cpp | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/rviz/MapCloudDisplay.cpp b/src/rviz/MapCloudDisplay.cpp
index f2424976..d6206aef 100644
--- a/src/rviz/MapCloudDisplay.cpp
+++ b/src/rviz/MapCloudDisplay.cpp
@@ -256,11 +256,18 @@ void MapCloudDisplay::processMessage( const rtabmap_ros::MapDataConstPtr& msg )
 
 void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 {
+	std::map<int, rtabmap::Transform> poses;
+	for(unsigned int i=0; i<map.graph.posesId.size() && i<map.graph.poses.size(); ++i)
+	{
+		poses.insert(std::make_pair(map.graph.posesId[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i])));
+	}
+
 	// Add new clouds...
 	for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
 	{
 		int id = map.nodes[i].id;
-		if(cloud_infos_.find(id) == cloud_infos_.end())
+		if(poses.find(id) != poses.end() &&
+		   cloud_infos_.find(id) == cloud_infos_.end())
 		{
 			// Cloud not added to RVIZ, add it!
 			rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
@@ -311,12 +318,6 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 	}
 
 	// Update graph
-	std::map<int, rtabmap::Transform> poses;
-	for(unsigned int i=0; i<map.graph.posesId.size() && i<map.graph.poses.size(); ++i)
-	{
-		poses.insert(std::make_pair(map.graph.posesId[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i])));
-	}
-
 	if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
 	{
 		poses = rtabmap::graph::radiusPosesFiltering(poses,