i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Localization mode: Fixed proximity detection permanently disabled when Rtabmap/StartNewMapOnLoopClosure is true

Browse Source
This commit is contained in:
matlabbe
2023-06-27 17:25:07 -07:00
parent a1168ba8a9
commit 97be9d4ace
2 changed files with 12 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/Graph.cpp
View File

@@ -2250,7 +2250,7 @@ std::map<int, Transform> findNearestPoses(
{ {
foundPoses.insert(*poses.find(iter->first)); 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; return foundPoses;
} }

12
corelib/src/Rtabmap.cpp
View File

@@ -2519,7 +2519,8 @@ bool Rtabmap::process(
{ {
if(_startNewMapOnLoopClosure && if(_startNewMapOnLoopClosure &&
_memory->getWorkingMem().size()>=2 && // must have an old map (+1 virtual place) _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 " 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).", "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());
}
} }
} }