Localization mode: Fixed proximity detection permanently disabled whe…

…n Rtabmap/StartNewMapOnLoopClosure is true
introlab · Jun 28, 2023 · 97be9d4 · 97be9d4
1 parent a1168ba
commit 97be9d4
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 2 deletions.
diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp
@@ -2250,7 +2250,7 @@ std::map<int, Transform> findNearestPoses(
 	{
 		foundPoses.insert(*poses.find(iter->first));
 	}
-	UDEBUG("found nodes=%d", (int)foundPoses.size());
+	UDEBUG("found nodes=%d/%d (radius=%f, angle=%f, k=%d)", (int)foundPoses.size(), (int)poses.size(), radius, angle, k);
 	return foundPoses;
 }
 

diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp
@@ -2519,7 +2519,8 @@ bool Rtabmap::process(
 	{
 		if(_startNewMapOnLoopClosure &&
 			_memory->getWorkingMem().size()>=2 && // must have an old map (+1 virtual place)
-			graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in new session)
+			_localizationCovariance.empty() && // if we didn't localize yet
+			graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0) // alone in new session
 		{
 			UINFO("Proximity detection by space disabled as if we force to have a global loop "
 					"closure with previous map before doing proximity detections (%s=true).",
@@ -2695,6 +2696,15 @@ bool Rtabmap::process(
 								}
 							}
 						}
+						else if(!signature->hasLink(nearestId) && proximityFilteringRadius>0.0f)
+						{
+							UDEBUG("Skipping path %d as most likely ID %d is too far %f > %f (%s)",
+								iter->first.id,
+								nearestId,
+								_optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)),
+								proximityFilteringRadius,
+								Parameters::kRGBDProximityPathFilteringRadius().c_str());
+						}
 					}
 				}