diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 2f4c482b9e..1b7a4477d0 100644 --- a/corelib/src/Rtabmap.cpp +++ b/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::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) { //============================================================