Fixed localization bug when virtual links are added

corelib/include/rtabmap/core/Graph.h

@@ -79,6 +79,13 @@ std::multimap<int, int>::const_iterator RTABMAP_EXP findLink(
 		int to,
 		bool checkBothWays = true);
 
+std::multimap<int, Link> RTABMAP_EXP filterLinks(
+		const std::multimap<int, Link> & links,
+		Link::Type filteredType);
+std::map<int, Link> RTABMAP_EXP filterLinks(
+		const std::map<int, Link> & links,
+		Link::Type filteredType);
+
 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
 std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
 		const std::map<int, Transform> & poses,

corelib/src/Graph.cpp

@@ -561,6 +561,36 @@ std::multimap<int, int>::const_iterator findLink(
 	return links.end();
 }
 
+std::multimap<int, Link> filterLinks(
+		const std::multimap<int, Link> & links,
+		Link::Type filteredType)
+{
+	std::multimap<int, Link> output;
+	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		if(iter->second.type() != filteredType)
+		{
+			output.insert(*iter);
+		}
+	}
+	return output;
+}
+
+std::map<int, Link> filterLinks(
+		const std::map<int, Link> & links,
+		Link::Type filteredType)
+{
+	std::map<int, Link> output;
+	for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
+	{
+		if(iter->second.type() != filteredType)
+		{
+			output.insert(*iter);
+		}
+	}
+	return output;
+}
+
 std::map<int, Transform> frustumPosesFiltering(
 		const std::map<int, Transform> & poses,
 		const Transform & cameraPose,

corelib/src/Rtabmap.cpp

@@ -2035,14 +2035,17 @@ bool Rtabmap::process(
 	{
 		UASSERT(uContains(_optimizedPoses, signature->id()));
 
+		//used in localization mode: filter virtual links
+		std::map<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
+
 		// Note that in localization mode, we don't re-optimize the graph
 		// if:
 		//  1- there are no signatures retrieved,
 		//  2- we are relocalizing on a node already in the optimized graph
 		if(!_memory->isIncremental() &&
 		   signaturesRetrieved.size() == 0 &&
-		   signature->getLinks().size() &&
+		   localizationLinks.size() &&
-		   uContains(_optimizedPoses, signature->getLinks().begin()->first))
+		   uContains(_optimizedPoses, localizationLinks.begin()->first))
 		{
 			// If there are no signatures retrieved, we don't
 			// need to re-optimize the graph. Just update the last
@@ -2054,8 +2057,8 @@ bool Rtabmap::process(
 				// update all previous nodes
 				// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
 				// parameters just changed state, we should put back all poses without map correction.
-				Transform oldPose = _optimizedPoses.at(signature->getLinks().begin()->first);
+				Transform oldPose = _optimizedPoses.at(localizationLinks.begin()->first);
-				Transform u = signature->getPose() * signature->getLinks().begin()->second.transform();
+				Transform u = signature->getPose() * localizationLinks.begin()->second.transform();
 				Transform up = u * oldPose.inverse();
 				Transform mapCorrectionInv = _mapCorrection.inverse();
 				for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
@@ -2066,7 +2069,7 @@ bool Rtabmap::process(
 			}
 			else
 			{
-				_optimizedPoses.at(signature->id()) = _optimizedPoses.at(signature->getLinks().begin()->first) * signature->getLinks().begin()->second.transform().inverse();
+				_optimizedPoses.at(signature->id()) = _optimizedPoses.at(localizationLinks.begin()->first) * localizationLinks.begin()->second.transform().inverse();
 			}
 		}
 		else