Fixed #1211

corelib/src/Rtabmap.cpp

@@ -1868,9 +1868,46 @@ bool Rtabmap::process(
 	//============================================================
 	// Bayes filter update
 	//============================================================
-	int previousId = signature->getLinks().size() && signature->getLinks().begin()->first!=signature->id()?signature->getLinks().begin()->first:0;
+	bool localizationOnPreviousUpdate = false;
+	if(_memory->isIncremental())
+	{
+		localizationOnPreviousUpdate =
+			signature->getLinks().size() &&
+			signature->getLinks().begin()->first!=signature->id() &&
+			_memory->getLoopClosureLinks(signature->getLinks().begin()->first, false).size() != 0;
+	}
+	else
+	{
+		// localization mode
+
+		// Count how many localization links are in the constraints
+		int localizationLinks = 0;
+		int previousIdWithLocalizationLink = 0;
+		for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin();
+				iter!=_odomCacheConstraints.end(); ++iter)
+		{
+			if(previousIdWithLocalizationLink == iter->first)
+			{
+				// ignore links with node already counted
+				continue;
+			}
+			if(iter->second.type() == Link::kGlobalClosure ||
+			   iter->second.type() == Link::kLocalSpaceClosure ||
+			   iter->second.type() == Link::kLocalTimeClosure ||
+			   iter->second.type() == Link::kUserClosure ||
+			   iter->second.type() == Link::kNeighborMerged ||
+			   iter->second.type() == Link::kLandmark)
+			{
+				++localizationLinks;
+				previousIdWithLocalizationLink = iter->first;
+			}
+		}
+
+		localizationOnPreviousUpdate = localizationLinks > 1; // need two links in case we have delayed localization
+	}
+
 	// Not a bad signature, not an intermediate node, not a small displacement unless the previous signature didn't have a loop closure, not too fast movement
-	if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0) && !tooFastMovement)
+	if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || !localizationOnPreviousUpdate) && !tooFastMovement)
 	{
 		// If the working memory is empty, don't do the detection. It happens when it
 		// is the first time the detector is started (there needs some images to
@@ -2550,7 +2587,7 @@ bool Rtabmap::process(
 
 			// don't do it if it is a small displacement unless the previous signature didn't have a loop closure
 			// don't do it if there is a too fast movement
-			if((!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0) && !tooFastMovement)
+			if((!smallDisplacement || !localizationOnPreviousUpdate) && !tooFastMovement)
 			{
 
 				//============================================================