i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added estimation loop closure hypothesis on small displacements, but only if the previous signature doesn't have a loop closure

Browse Source
This commit is contained in:
matlabbe
2015-05-08 11:00:50 -04:00
parent dd7d28898b
commit 7aaa4698d5
1 changed files with 202 additions and 194 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+10 -2
View File
@@ -1058,7 +1058,9 @@ bool Rtabmap::process(const SensorData & data)
//============================================================ //============================================================
// Bayes filter update // Bayes filter update
//============================================================ //============================================================
if(!signature->isBadSignature() && !smallDisplacement) int previousId = signature->getLinks().size() == 1?signature->getLinks().begin()->first:0;
// Not a bad signature, not a small displacemnt unless the previous signature didn't have a loop closure
if(!signature->isBadSignature() && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0))
{ {
// If the working memory is empty, don't do the detection. It happens when it // 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 // is the first time the detector is started (there needs some images to
@@ -1653,6 +1655,10 @@ bool Rtabmap::process(const SensorData & data)
// In localization mode, no need to check local loop // In localization mode, no need to check local loop
// closures if we are already localized by a global closure. // closures if we are already localized by a global closure.
// don't do it if it is a small displacement unless the previous signature didn't have a loop closure
if(!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0)
{
//============================================================ //============================================================
// LOCAL LOOP CLOSURE SPACE // LOCAL LOOP CLOSURE SPACE
//============================================================ //============================================================
@@ -1879,6 +1885,7 @@ bool Rtabmap::process(const SensorData & data)
} }
} }
} }
}
timeLocalSpaceDetection = timer.ticks(); timeLocalSpaceDetection = timer.ticks();
ULOGGER_INFO("timeLocalSpaceDetection=%fs", timeLocalSpaceDetection); ULOGGER_INFO("timeLocalSpaceDetection=%fs", timeLocalSpaceDetection);
@@ -2128,8 +2135,9 @@ bool Rtabmap::process(const SensorData & data)
signaturesRemoved.push_back(signature->id()); signaturesRemoved.push_back(signature->id());
_memory->deleteLocation(signature->id()); _memory->deleteLocation(signature->id());
} }
else if(smallDisplacement) else if(smallDisplacement && _loopClosureHypothesis.first == 0 && lastLocalSpaceClosureId == 0)
{ {
// Don't delete the location if a loop closure is detected
UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)", UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate); signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
// If there is a too small displacement, remove the node // If there is a too small displacement, remove the node