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

Don't optimize the graph on localization (just update last pose) if there are no retrieved signatures and the loop closure is on a node already in the optimized graph

Browse Source
This commit is contained in:
matlabbe
2015-10-07 11:38:31 -04:00
parent f40e2516c7
commit b8757f6e8b
1 changed files with 92 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Rtabmap.cpp
+45 -6
View File
@@ -1094,9 +1094,9 @@ bool Rtabmap::process(
// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
// parameters just changed state, we should put back all poses without map correction.
Transform u = guess.inverse() * t;
std::map<int, Transform>::iterator iter = _optimizedPoses.find(oldId);
UASSERT(iter!=_optimizedPoses.end());
Transform up = iter->second * u * iter->second.inverse();
std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
UASSERT(jter!=_optimizedPoses.end());
Transform up = jter->second * u * jter->second.inverse();
Transform mapCorrectionInv = _mapCorrection.inverse();
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
@@ -2103,8 +2103,44 @@ bool Rtabmap::process(
(localLoopClosuresInTimeFound>0 || // only same map of the current one
signaturesRetrieved.size())))) // can be different map of the current one
{
// Note that in localization mode, we update only
// if there is loop closure (so the last signature is linked to local map)
UASSERT(uContains(_optimizedPoses, signature->id()));
// 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))
{
// If there are no signatures retrieved, we don't
// need to re-optimize the graph. Just update the last
// position if OptimizeFromGraphEnd=false or transform the
// whole graph if OptimizeFromGraphEnd=true
UINFO("Localization without map optimization");
if(_optimizeFromGraphEnd)
{
// 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 up = u * oldPose.inverse();
Transform mapCorrectionInv = _mapCorrection.inverse();
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{
iter->second = mapCorrectionInv * up * iter->second;
}
_optimizedPoses.at(signature->id()) = signature->getPose();
}
else
{
_optimizedPoses.at(signature->id()) = _optimizedPoses.at(signature->getLinks().begin()->first) * signature->getLinks().begin()->second.transform().inverse();
}
}
else
{
UINFO("Update map correction");
std::map<int, Transform> poses = _optimizedPoses;
@@ -2115,7 +2151,9 @@ bool Rtabmap::process(
// Check added loop closures have broken the graph
// (in case of wrong loop closures).
bool updateConstraints = true;
if(_optimizationMaxLinearError > 0.0f && loopClosureLinksAdded.size())
if(_memory->isIncremental() && // FIXME: not tested in localization mode, so do it only in mapping mode
_optimizationMaxLinearError > 0.0f &&
loopClosureLinksAdded.size())
{
const Link * maxLinearLink = 0;
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
@@ -2164,6 +2202,7 @@ bool Rtabmap::process(
_optimizedPoses = poses;
_constraints = constraints;
}
}
// Update map correction, it should be identify when optimizing from the last node
_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();