i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +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

30
corelib/src/Graph.cpp
View File

@@ -561,6 +561,36 @@ std::multimap<int, int>::const_iterator findLink(
return links.end();
}
std::multimap<int, Link> filterLinks(
const std::multimap<int, Link> & links,
Link::Type filteredType)
{
std::multimap<int, Link> output;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() != filteredType)
{
output.insert(*iter);
}
}
return output;
}
std::map<int, Link> filterLinks(
const std::map<int, Link> & links,
Link::Type filteredType)
{
std::map<int, Link> output;
for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() != filteredType)
{
output.insert(*iter);
}
}
return output;
}
std::map<int, Transform> frustumPosesFiltering(
const std::map<int, Transform> & poses,
const Transform & cameraPose,

13
corelib/src/Rtabmap.cpp
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