Added path immunization between the nearest local location and the current pose. New parameter: "RGBD/LocalImmunizationRatio"

corelib/include/rtabmap/core/Memory.h

@@ -160,7 +160,7 @@ public:
 
 	// RGB-D stuff
 	void getMetricConstraints(
-			const std::vector<int> & ids,
+			const std::set<int> & ids,
 			std::map<int, Transform> & poses,
 			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);

corelib/include/rtabmap/core/Parameters.h

@@ -191,7 +191,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, RecentWmRatio,           float, 0.2, 	"Ratio of locations after the last loop closure in WM that cannot be transferred.");
 	RTABMAP_PARAM(Mem, TransferSortingByWeightId, bool, false,  "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
 	RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
-	RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, true, "When the robot is moving, weights are not updated on rehearsal.");
+	RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, false, "When the robot is moving, weights are not updated on rehearsal.");
 	RTABMAP_PARAM(Mem, GenerateIds,             bool, true,     "True=Generate location IDs, False=use input image IDs.");
 	RTABMAP_PARAM(Mem, BadSignaturesIgnored,    bool, false,     "Bad signatures are ignored.");
 	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
@@ -290,15 +290,15 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, false,    "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
 	RTABMAP_PARAM(RGBD, GoalReachedRadius,    float, 0.5, "Goal reached radius (m).");
 	RTABMAP_PARAM(RGBD, PlanVirtualLinks,  bool, true, "Before planning in the graph, close nodes are linked together. Radius is defined by \"RGBD/GoalReachedRadius\" parameter.");
-	RTABMAP_PARAM(RGBD, PlanVirtualLinksMaxDiffID,  int, 50, "Max difference ID to add virtual links before planning.");
 	RTABMAP_PARAM(RGBD, GoalsSavedInUserData,     bool, true, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
 	RTABMAP_PARAM(RGBD, MaxLocalRetrieved, unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
 	RTABMAP_PARAM(RGBD, LocalRadius, float, 10, "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
+	RTABMAP_PARAM(RGBD, LocalImmunizationRatio, float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
 
 	// Local loop closure detection
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionTime,     bool, false, 	"Detection over all locations in STM.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionSpace,    bool, false, 	"Detection over locations (in Working Memory or STM) near in space.");
-	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxGraphDepth,   int, 20,      "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
+	RTABMAP_PARAM(RGBD, LocalLoopDetectionMaxGraphDepth,   int, 50,      "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathFilteringRadius,   float, 0.5, "Path filtering radius.");
 	RTABMAP_PARAM(RGBD, LocalLoopDetectionPathOdomPosesUsed,   bool, true, "When comparing to a local path, merge the scan using the odometry poses instead of the ones in the optimized local graph.");
 

corelib/include/rtabmap/core/Rtabmap.h

@@ -151,7 +151,7 @@ private:
 			std::multimap<int, Link> * constraints = 0) const;
 	std::map<int, Transform> optimizeGraph(
 			int fromId,
-			const std::vector<int> & ids,
+			const std::set<int> & ids,
 			bool lookInDatabase,
 			std::multimap<int, Link> * constraints = 0) const;
 	void updateGoalIndex();
@@ -184,6 +184,7 @@ private:
 	bool _localLoopClosureDetectionTime;
 	bool _localLoopClosureDetectionSpace;
 	float _localRadius;
+	float _localImmunizationRatio;
 	int _localDetectMaxGraphDepth;
 	float _localPathFilteringRadius;
 	bool _localPathOdomPosesUsed;
@@ -197,7 +198,6 @@ private:
 	bool _startNewMapOnLoopClosure;
 	float _goalReachedRadius; // meters
 	bool _planVirtualLinks;
-	int _planVirtualLinksMaxDiffID;
 	bool _goalsSavedInUserData;
 
 	std::pair<int, float> _loopClosureHypothesis;

corelib/include/rtabmap/core/Statistics.h

@@ -77,6 +77,9 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Working_memory_size,);
 	RTABMAP_STATS(Memory, Short_time_memory_size,);
 	RTABMAP_STATS(Memory, Signatures_removed,);
+	RTABMAP_STATS(Memory, Immunized_globally,);
+	RTABMAP_STATS(Memory, Immunized_locally,);
+	RTABMAP_STATS(Memory, Immunized_locally_max,);
 	RTABMAP_STATS(Memory, Signatures_retrieved,);
 	RTABMAP_STATS(Memory, Images_buffered,);
 	RTABMAP_STATS(Memory, Rehearsal_sim,);

corelib/src/Memory.cpp

@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/Statistics.h"
 #include "rtabmap/core/Compression.h"
+#include "rtabmap/core/Graph.h"
 
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/common.h>
@@ -4256,56 +4257,47 @@ std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned
 	return std::set<int>(idsToLoad.begin(), idsToLoad.end());
 }
 
+// return all non-null poses
+// return unique links between nodes (for neighbors: old->new, for loops: parent->child)
 void Memory::getMetricConstraints(
-		const std::vector<int> & ids,
+		const std::set<int> & ids,
 		std::map<int, Transform> & poses,
 		std::multimap<int, Link> & links,
 		bool lookInDatabase)
 {
 	UDEBUG("");
-	for(unsigned int i=0; i<ids.size(); ++i)
+	for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
 	{
-		Transform pose = getOdomPose(ids[i], lookInDatabase);
+		Transform pose = getOdomPose(*iter, lookInDatabase);
 		if(!pose.isNull())
 		{
-			poses.insert(std::make_pair(ids[i], pose));
+			poses.insert(std::make_pair(*iter, pose));
 		}
 	}
 
-	for(unsigned int i=0; i<ids.size(); ++i)
+	for(std::set<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
 	{
-		if(uContains(poses, ids[i]))
+		if(uContains(poses, *iter))
 		{
-			std::map<int, Link> neighbors = this->getNeighborLinks(ids[i], lookInDatabase); // only direct neighbors
+			std::map<int, Link> neighbors = this->getNeighborLinks(*iter, lookInDatabase); // only direct neighbors
 			for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
 			{
-				if(uContains(poses, jter->first) && jter->second.isValid())
+				if(	jter->second.isValid() &&
+					uContains(poses, jter->first) &&
+					graph::findLink(links, *iter, jter->first) == links.end())
 				{
-					bool edgeAlreadyAdded = false;
-					for(std::multimap<int, Link>::iterator iter = links.lower_bound(jter->first);
-							iter != links.end() && iter->first == jter->first;
-							++iter)
-					{
-						if(iter->second.to() == ids[i])
-						{
-							edgeAlreadyAdded = true;
-						}
-					}
-					if(!edgeAlreadyAdded)
-					{
-						links.insert(std::make_pair(ids[i], jter->second));
-					}
+					links.insert(std::make_pair(*iter, jter->second));
 				}
 			}
 
-			std::map<int, Link> loops = this->getLoopClosureLinks(ids[i], lookInDatabase);
+			std::map<int, Link> loops = this->getLoopClosureLinks(*iter, lookInDatabase);
 			for(std::map<int, Link>::iterator jter=loops.begin(); jter!=loops.end(); ++jter)
 			{
-				if(jter->first < ids[i] &&
-					uContains(poses, jter->first) &&
-					jter->second.isValid()) // null transform means a child (rehearsed location)
+				if( jter->second.isValid() && // null transform means a rehearsed location
+					jter->first < *iter && // Loop parent to child
+					uContains(poses, jter->first))
 				{
-					links.insert(std::make_pair(ids[i],jter->second));
+					links.insert(std::make_pair(*iter, jter->second));
 				}
 			}
 		}

corelib/src/Rtabmap.cpp

@@ -96,6 +96,7 @@ Rtabmap::Rtabmap() :
 	_localLoopClosureDetectionTime(Parameters::defaultRGBDLocalLoopDetectionTime()),
 	_localLoopClosureDetectionSpace(Parameters::defaultRGBDLocalLoopDetectionSpace()),
 	_localRadius(Parameters::defaultRGBDLocalRadius()),
+	_localImmunizationRatio(Parameters::defaultRGBDLocalImmunizationRatio()),
 	_localDetectMaxGraphDepth(Parameters::defaultRGBDLocalLoopDetectionMaxGraphDepth()),
 	_localPathFilteringRadius(Parameters::defaultRGBDLocalLoopDetectionPathFilteringRadius()),
 	_localPathOdomPosesUsed(Parameters::defaultRGBDLocalLoopDetectionPathOdomPosesUsed()),
@@ -109,7 +110,6 @@ Rtabmap::Rtabmap() :
 	_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
 	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
 	_planVirtualLinks(Parameters::defaultRGBDPlanVirtualLinks()),
-	_planVirtualLinksMaxDiffID(Parameters::defaultRGBDPlanVirtualLinksMaxDiffID()),
 	_goalsSavedInUserData(Parameters::defaultRGBDGoalsSavedInUserData()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
@@ -385,6 +385,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionTime(), _localLoopClosureDetectionTime);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionSpace(), _localLoopClosureDetectionSpace);
 	Parameters::parse(parameters, Parameters::kRGBDLocalRadius(), _localRadius);
+	Parameters::parse(parameters, Parameters::kRGBDLocalImmunizationRatio(), _localImmunizationRatio);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxGraphDepth(), _localDetectMaxGraphDepth);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathFilteringRadius(), _localPathFilteringRadius);
 	Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionPathOdomPosesUsed(), _localPathOdomPosesUsed);
@@ -397,7 +398,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
 	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
 	Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinks(), _planVirtualLinks);
-	Parameters::parse(parameters, Parameters::kRGBDPlanVirtualLinksMaxDiffID(), _planVirtualLinksMaxDiffID);
 	Parameters::parse(parameters, Parameters::kRGBDGoalsSavedInUserData(), _goalsSavedInUserData);
 
 	// RGB-D SLAM stuff
@@ -723,7 +723,7 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
 		else
 		{
 			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
-			_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
+			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 		}
 
 		graph::TOROOptimizer::saveGraph(path, poses, constraints);
@@ -981,7 +981,19 @@ bool Rtabmap::process(const SensorData & data)
 
 		if(signature->getLinks().size() == 1)
 		{
-			_constraints.insert(std::make_pair(signature->id(), signature->getLinks().begin()->second));
+			// link should be old to new
+			if(signature->id() > signature->getLinks().begin()->second.to())
+			{
+				Link tmp = signature->getLinks().begin()->second;
+				tmp.setFrom(tmp.to());
+				tmp.setTo(signature->id());
+				tmp.setTransform(tmp.transform().inverse());
+				_constraints.insert(std::make_pair(tmp.from(), tmp));
+			}
+			else
+			{
+				_constraints.insert(std::make_pair(signature->id(), signature->getLinks().begin()->second));
+			}
 		}
 
 		//============================================================
@@ -1171,35 +1183,36 @@ bool Rtabmap::process(const SensorData & data)
 	std::list<int> reactivatedIds;
 	double timeGetNeighborsTimeDb = 0.0;
 	double timeGetNeighborsSpaceDb = 0.0;
+	int immunizedGlobally = 0;
+	int immunizedLocally = 0;
 	if(retrievalId > 0 )
 	{
 		//Load neighbors
 		ULOGGER_INFO("Retrieving locations... around id=%d", retrievalId);
 		int neighborhoodSize = (int)_bayesFilter->getPredictionLC().size()-1;
 		UASSERT(neighborhoodSize >= 0);
-		int margin = neighborhoodSize;
-		ULOGGER_DEBUG("margin=%d maxRetieved=%d", margin, _maxRetrieved);
+		ULOGGER_DEBUG("margin=%d maxRetieved=%d", neighborhoodSize, _maxRetrieved);
 
 		UTimer timeGetN;
 		unsigned int nbLoadedFromDb = 0;
 		std::set<int> reactivatedIdsSet;
 		std::map<int, int> neighbors;
-		bool firstPassDone = false;
-		int m = 0;
 		int nbDirectNeighborsInDb = 0;
 
 		// priority in time
 		// Direct neighbors TIME
 		ULOGGER_DEBUG("In TIME");
 		neighbors = _memory->getNeighborsId(retrievalId,
-				margin,
+				neighborhoodSize,
 				_maxRetrieved,
 				true,
 				true,
 				&timeGetNeighborsTimeDb);
 		ULOGGER_DEBUG("neighbors of %d in time = %d", retrievalId, (int)neighbors.size());
 		//Priority to locations near in time (direct neighbor) then by space (loop closure)
-		while(m < margin)
+		bool firstPassDone = false; // just to avoid checking to STM after the first pass
+		int m = 0;
+		while(m < neighborhoodSize)
 		{
 			std::set<int> idsSorted;
 			for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
@@ -1220,15 +1233,15 @@ bool Rtabmap::process(const SensorData & data)
 							++nbDirectNeighborsInDb;
 						}
 
-						if(m<neighborhoodSize)
+						//immunized locations in the neighborhood from being transferred
+						if(immunizedLocations.insert(iter->first).second)
 						{
-							//immunized locations in the neighborhood from being transferred
-							immunizedLocations.insert(iter->first);
+							++immunizedGlobally;
 						}
+
 						UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second);
 					}
-					std::map<int, int>::iterator tmp = iter++;
-					neighbors.erase(tmp);
+					neighbors.erase(iter++);
 				}
 				else
 				{
@@ -1243,15 +1256,15 @@ bool Rtabmap::process(const SensorData & data)
 		// neighbors SPACE, already added direct neighbors will be ignored
 		ULOGGER_DEBUG("In SPACE");
 		neighbors = _memory->getNeighborsId(retrievalId,
-				margin,
+				neighborhoodSize,
 				_maxRetrieved,
 				true,
 				false,
 				&timeGetNeighborsSpaceDb);
 		ULOGGER_DEBUG("neighbors of %d in space = %d", retrievalId, (int)neighbors.size());
-		m = 0;
 		firstPassDone = false;
-		while(m < margin)
+		m = 0;
+		while(m < neighborhoodSize)
 		{
 			std::set<int> idsSorted;
 			for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
@@ -1273,8 +1286,7 @@ bool Rtabmap::process(const SensorData & data)
 						}
 						UDEBUG("nt=%d m=%d", iter->first, iter->second);
 					}
-					std::map<int, int>::iterator tmp = iter++;
-					neighbors.erase(tmp);
+					neighbors.erase(iter++);
 				}
 				else
 				{
@@ -1285,13 +1297,11 @@ bool Rtabmap::process(const SensorData & data)
 			reactivatedIds.insert(reactivatedIds.end(), idsSorted.rbegin(), idsSorted.rend());
 			++m;
 		}
-		ULOGGER_INFO("margin=%d, "
-				"neighborhoodSize=%d, "
+		ULOGGER_INFO("neighborhoodSize=%d, "
 				"reactivatedIds.size=%d, "
 				"nbLoadedFromDb=%d, "
 				"nbDirectNeighborsInDb=%d, "
 				"time=%fs (%fs %fs)",
-				margin,
 				neighborhoodSize,
 				reactivatedIds.size(),
 				(int)nbLoadedFromDb,
@@ -1306,6 +1316,7 @@ bool Rtabmap::process(const SensorData & data)
 	// RETRIEVAL 2/3 : Update planned path and get next nodes to retrieve
 	//============================================================
 	std::list<int> retrievalLocalIds;
+	int maxLocalLocationsImmunized = _localImmunizationRatio * float(_memory->getWorkingMem().size());
 	if(_rgbdSlamMode)
 	{
 		// Priority on locations on the planned path
@@ -1327,7 +1338,10 @@ bool Rtabmap::process(const SensorData & data)
 				{
 					if(_memory->getSignature(_path[i].first) != 0)
 					{
-						immunizedLocations.insert(_path[i].first);
+						if(immunizedLocations.insert(_path[i].first).second)
+						{
+							++immunizedLocally;
+						}
 						UDEBUG("Path immunization: node %d (dist=%fm)", _path[i].first, distanceSoFar);
 					}
 					else if(retrievalLocalIds.size() < _maxLocalRetrieved)
@@ -1345,65 +1359,144 @@ bool Rtabmap::process(const SensorData & data)
 				}
 			}
 		}
-		if(retrievalLocalIds.size() < _maxLocalRetrieved)
+
+		// immunize the path from the nearest local location to the current location
+		if(immunizedLocally < maxLocalLocationsImmunized)
 		{
-			// retrieval based on the nodes near the current pose
-			std::map<int, float> nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
-			// sort by distance
-			std::multimap<float, int> nearNodesByDist;
-			for(std::map<int, float>::iterator iter=nearNodes.begin(); iter!=nearNodes.end(); ++iter)
+			std::map<int ,Transform> poses;
+			// remove poses from STM
+			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 			{
-				nearNodesByDist.insert(std::make_pair(iter->second, iter->first));
-			}
-			for(std::multimap<float, int>::iterator iter=nearNodesByDist.begin();
-				iter!=nearNodesByDist.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
-				++iter)
-			{
-				const Signature * s = _memory->getSignature(iter->second);
-				UASSERT(s!=0);
-				// If there is a change of direction, better to be retrieving
-				// ALL nearest signatures than only newest neighbors
-				const std::map<int, Link> & links = s->getLinks();
-				for(std::map<int, Link>::const_reverse_iterator jter=links.rbegin();
-					jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
-					++jter)
+				if(!_memory->isInSTM(iter->first))
 				{
-					if(_memory->getSignature(jter->first) == 0)
-					{
-						UINFO("retrieval of node %d on local map", jter->first);
-						retrievalLocalIds.push_back(jter->first);
-					}
+					poses.insert(*iter);
 				}
 			}
-			// well, if the maximum retrieved is not reached, look for neighbors in database
-			if(retrievalLocalIds.size() < _maxLocalRetrieved)
+			int nearestId = graph::findNearestNode(poses, _optimizedPoses.at(signature->id()));
+
+			if(nearestId > 0 &&
+				(_localRadius==0 ||
+				 _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)) < _localRadius))
 			{
-				std::set<int> retrievalLocalIdsSet(retrievalLocalIds.begin(), retrievalLocalIds.end());
-				for(std::list<int>::iterator iter=retrievalLocalIds.begin();
-					iter!=retrievalLocalIds.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
-					++iter)
+				std::multimap<int, int> links;
+				for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
 				{
-					std::map<int, int> ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - retrievalLocalIds.size() + 1, true, false);
-					for(std::map<int, int>::reverse_iterator jter=ids.rbegin();
-						jter!=ids.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
-						++jter)
+					if(uContains(_optimizedPoses, iter->second.from()) && uContains(_optimizedPoses, iter->second.to()))
 					{
-						if(_memory->getSignature(jter->first) == 0 &&
-						   retrievalLocalIdsSet.find(jter->first) == retrievalLocalIdsSet.end())
+						links.insert(std::make_pair(iter->second.from(), iter->second.to()));
+						links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <->
+					}
+				}
+
+				std::list<std::pair<int, Transform> > path = graph::computePath(_optimizedPoses, links, nearestId, signature->id());
+				if(path.size() == 0)
+				{
+					UWARN("Could not compute a path between %d and %d", nearestId, signature->id());
+				}
+				else
+				{
+					for(std::list<std::pair<int, Transform> >::iterator iter=path.begin();
+						iter!=path.end();
+						++iter)
+					{
+						if(immunizedLocally >= maxLocalLocationsImmunized)
 						{
-							UINFO("retrieval of node %d on local map", jter->first);
-							retrievalLocalIds.push_back(jter->first);
-							retrievalLocalIdsSet.insert(jter->first);
+							UWARN("Could not immunize the whole local path (%d) between "
+								  "%d and %d (max location immunized=%d). You may want "
+								  "to increase RGBD/LocalImmunizationRatio (current=%f (%d of WM=%d)) "
+								  "to be able to immunize longer paths.",
+									(int)path.size(),
+									nearestId,
+									signature->id(),
+									maxLocalLocationsImmunized,
+									_localImmunizationRatio,
+									maxLocalLocationsImmunized,
+									(int)_memory->getWorkingMem().size());
+							break;
+						}
+						else if(!_memory->isInSTM(iter->first))
+						{
+							if(immunizedLocations.insert(iter->first).second)
+							{
+								++immunizedLocally;
+							}
+							UDEBUG("local node %d on path immunized=1", iter->first);
 						}
 					}
 				}
 			}
+		}
 
-			// update Age of the close signatures (oldest the farthest)
-			for(std::multimap<float, int>::reverse_iterator iter=nearNodesByDist.rbegin(); iter!=nearNodesByDist.rend(); ++iter)
+		// retrieval based on the nodes close the the nearest pose in WM
+		// immunize closest nodes
+		std::map<int, float> nearNodes = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
+		// sort by distance
+		std::multimap<float, int> nearNodesByDist;
+		for(std::map<int, float>::iterator iter=nearNodes.begin(); iter!=nearNodes.end(); ++iter)
+		{
+			nearNodesByDist.insert(std::make_pair(iter->second, iter->first));
+		}
+		UINFO("near nodes=%d, max local immunized=%d, ratio=%f WM=%d",
+				(int)nearNodesByDist.size(),
+				maxLocalLocationsImmunized,
+				_localImmunizationRatio,
+				(int)_memory->getWorkingMem().size());
+		for(std::multimap<float, int>::iterator iter=nearNodesByDist.begin();
+			iter!=nearNodesByDist.end() && (retrievalLocalIds.size() < _maxLocalRetrieved || immunizedLocally < maxLocalLocationsImmunized);
+			++iter)
+		{
+			const Signature * s = _memory->getSignature(iter->second);
+			UASSERT(s!=0);
+			// If there is a change of direction, better to be retrieving
+			// ALL nearest signatures than only newest neighbors
+			const std::map<int, Link> & links = s->getLinks();
+			for(std::map<int, Link>::const_reverse_iterator jter=links.rbegin();
+				jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
+				++jter)
 			{
-				_memory->updateAge(iter->second);
+				if(_memory->getSignature(jter->first) == 0)
+				{
+					UINFO("retrieval of node %d on local map", jter->first);
+					retrievalLocalIds.push_back(jter->first);
+				}
 			}
+			if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized)
+			{
+				if(immunizedLocations.insert(s->id()).second)
+				{
+					++immunizedLocally;
+				}
+				UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
+			}
+		}
+		// well, if the maximum retrieved is not reached, look for neighbors in database
+		if(retrievalLocalIds.size() < _maxLocalRetrieved)
+		{
+			std::set<int> retrievalLocalIdsSet(retrievalLocalIds.begin(), retrievalLocalIds.end());
+			for(std::list<int>::iterator iter=retrievalLocalIds.begin();
+				iter!=retrievalLocalIds.end() && retrievalLocalIds.size() < _maxLocalRetrieved;
+				++iter)
+			{
+				std::map<int, int> ids = _memory->getNeighborsId(*iter, 2, _maxLocalRetrieved - retrievalLocalIds.size() + 1, true, false);
+				for(std::map<int, int>::reverse_iterator jter=ids.rbegin();
+					jter!=ids.rend() && retrievalLocalIds.size() < _maxLocalRetrieved;
+					++jter)
+				{
+					if(_memory->getSignature(jter->first) == 0 &&
+					   retrievalLocalIdsSet.find(jter->first) == retrievalLocalIdsSet.end())
+					{
+						UINFO("retrieval of node %d on local map", jter->first);
+						retrievalLocalIds.push_back(jter->first);
+						retrievalLocalIdsSet.insert(jter->first);
+					}
+				}
+			}
+		}
+
+		// update Age of the close signatures (oldest the farthest)
+		for(std::multimap<float, int>::reverse_iterator iter=nearNodesByDist.rbegin(); iter!=nearNodesByDist.rend(); ++iter)
+		{
+			_memory->updateAge(iter->second);
 		}
 
 		// insert them first to make sure they are loaded.
@@ -1703,7 +1796,7 @@ bool Rtabmap::process(const SensorData & data)
 						if(_localPathOdomPosesUsed)
 						{
 							//optimize the path's poses locally
-							path = optimizeGraph(nearestId, uKeys(path), false);
+							path = optimizeGraph(nearestId, uKeysSet(path), false);
 							// transform local poses in optimized graph referential
 							Transform t = _optimizedPoses.at(nearestId) * path.at(nearestId).inverse();
 							for(std::map<int, Transform>::iterator jter=path.begin(); jter!=path.end(); ++jter)
@@ -1904,7 +1997,7 @@ bool Rtabmap::process(const SensorData & data)
 				std::map<int, double> stamps;
 				std::map<int, std::vector<unsigned char> > userDatas;
 				std::multimap<int, Link> constraints;
-				_memory->getMetricConstraints(uKeys(ids), poses, constraints, false);
+				_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, false);
 				for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 				{
 					Transform odomPose;
@@ -2090,7 +2183,12 @@ bool Rtabmap::process(const SensorData & data)
 		statistics_.addStatistic(Statistics::kTimingJoining_trash(), timeJoiningTrash*1000);
 		statistics_.addStatistic(Statistics::kTimingEmptying_trash(), timeEmptyingTrash*1000);
 		statistics_.addStatistic(Statistics::kTimingMemory_cleanup(), timeMemoryCleanup*1000);
+
+		// Transfer
 		statistics_.addStatistic(Statistics::kMemorySignatures_removed(), signaturesRemoved.size());
+		statistics_.addStatistic(Statistics::kMemoryImmunized_globally(), immunizedGlobally);
+		statistics_.addStatistic(Statistics::kMemoryImmunized_locally(), immunizedLocally);
+		statistics_.addStatistic(Statistics::kMemoryImmunized_locally_max(), maxLocalLocationsImmunized);
 
 		// place after transfer because the memory/local graph may have changed
 		statistics_.addStatistic(Statistics::kMemoryWorking_memory_size(), _memory->getWorkingMem().size());
@@ -2433,7 +2531,7 @@ void Rtabmap::optimizeCurrentMap(
 		}
 		UINFO("get ids time %f s", timer.ticks());
 
-		optimizedPoses = Rtabmap::optimizeGraph(id, uKeys(ids), lookInDatabase, constraints);
+		optimizedPoses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints);
 
 		if(_memory->getSignature(id) && uContains(optimizedPoses, id))
 		{
@@ -2446,7 +2544,7 @@ void Rtabmap::optimizeCurrentMap(
 
 std::map<int, Transform> Rtabmap::optimizeGraph(
 		int fromId,
-		const std::vector<int> & ids,
+		const std::set<int> & ids,
 		bool lookInDatabase,
 		std::multimap<int, Link> * constraints) const
 {
@@ -2603,14 +2701,14 @@ void Rtabmap::get3DMap(std::map<int, Signature> & signatures,
 			else
 			{
 				std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
-				_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
+				_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 			}
 		}
 		else
 		{
 			// no optimization on appearance-only mode
 			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
-			_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
+			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 		}
 
 		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
@@ -2681,14 +2779,14 @@ void Rtabmap::getGraph(
 			else
 			{
 				std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
-				_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
+				_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 			}
 		}
 		else
 		{
 			// no optimization on appearance-only mode
 			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
-			_memory->getMetricConstraints(uKeys(ids), poses, constraints, global);
+			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
 		}
 
 		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
@@ -2766,13 +2864,10 @@ bool Rtabmap::computePath(
 			std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(nodes, _goalReachedRadius, CV_PI);
 			for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
 			{
-				if(graph::findLink(links, iter->first, iter->second) != links.end())
+				if(graph::findLink(links, iter->first, iter->second) == links.end())
 				{
-					if(_planVirtualLinksMaxDiffID <= 0 ||
-					   abs(iter->first - iter->second) < _planVirtualLinksMaxDiffID)
-					{
-						links.insert(*iter);
-					}
+					links.insert(*iter);
+					links.insert(std::make_pair(iter->second, iter->first)); // <->
 				}
 			}
 		}