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

Fixed #1211

Browse Source
This commit is contained in:
matlabbe
2024-02-10 18:27:02 -08:00
parent cd615c6e52
commit a11ea291d8
1 changed files with 40 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
corelib/src/Rtabmap.cpp
View File

@@ -1868,9 +1868,46 @@ bool Rtabmap::process(
//============================================================ //============================================================
// Bayes filter update // 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 // 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 // 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
@@ -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 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 // don't do it if there is a too fast movement
if((!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0) && !tooFastMovement) if((!smallDisplacement || !localizationOnPreviousUpdate) && !tooFastMovement)
{ {
//============================================================ //============================================================