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

Fixed localization bug when virtual links are added

Browse Source
This commit is contained in:
matlabbe
2016-05-26 17:28:06 -04:00
parent 0fa41d317a
commit be13a9b967
3 changed files with 45 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+8 -5
View File
@@ -2035,14 +2035,17 @@ bool Rtabmap::process(
{
UASSERT(uContains(_optimizedPoses, signature->id()));
//used in localization mode: filter virtual links
std::map<int, Link> localizationLinks = graph::filterLinks(signature->getLinks(), Link::kVirtualClosure);
// Note that in localization mode, we don't re-optimize the graph
// if:
// 1- there are no signatures retrieved,
// 2- we are relocalizing on a node already in the optimized graph
if(!_memory->isIncremental() &&
signaturesRetrieved.size() == 0 &&
signature->getLinks().size() &&
uContains(_optimizedPoses, signature->getLinks().begin()->first))
localizationLinks.size() &&
uContains(_optimizedPoses, localizationLinks.begin()->first))
{
// If there are no signatures retrieved, we don't
// need to re-optimize the graph. Just update the last
@@ -2054,8 +2057,8 @@ bool Rtabmap::process(
// update all previous nodes
// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
// parameters just changed state, we should put back all poses without map correction.
Transform oldPose = _optimizedPoses.at(signature->getLinks().begin()->first);
Transform u = signature->getPose() * signature->getLinks().begin()->second.transform();
Transform oldPose = _optimizedPoses.at(localizationLinks.begin()->first);
Transform u = signature->getPose() * localizationLinks.begin()->second.transform();
Transform up = u * oldPose.inverse();
Transform mapCorrectionInv = _mapCorrection.inverse();
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
@@ -2066,7 +2069,7 @@ bool Rtabmap::process(
}
else
{
_optimizedPoses.at(signature->id()) = _optimizedPoses.at(signature->getLinks().begin()->first) * signature->getLinks().begin()->second.transform().inverse();
_optimizedPoses.at(signature->id()) = _optimizedPoses.at(localizationLinks.begin()->first) * localizationLinks.begin()->second.transform().inverse();
}
}
else