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
+92 -53
View File
@@ -1094,9 +1094,9 @@ bool Rtabmap::process(
// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd // Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
// parameters just changed state, we should put back all poses without map correction. // parameters just changed state, we should put back all poses without map correction.
Transform u = guess.inverse() * t; Transform u = guess.inverse() * t;
std::map<int, Transform>::iterator iter = _optimizedPoses.find(oldId); std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
UASSERT(iter!=_optimizedPoses.end()); UASSERT(jter!=_optimizedPoses.end());
Transform up = iter->second * u * iter->second.inverse(); Transform up = jter->second * u * jter->second.inverse();
Transform mapCorrectionInv = _mapCorrection.inverse(); Transform mapCorrectionInv = _mapCorrection.inverse();
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{ {
@@ -2103,66 +2103,105 @@ bool Rtabmap::process(
(localLoopClosuresInTimeFound>0 || // only same map of the current one (localLoopClosuresInTimeFound>0 || // only same map of the current one
signaturesRetrieved.size())))) // can be different map of the current one signaturesRetrieved.size())))) // can be different map of the current one
{ {
// Note that in localization mode, we update only UASSERT(uContains(_optimizedPoses, signature->id()));
// if there is loop closure (so the last signature is linked to local map)
UINFO("Update map correction"); // Note that in localization mode, we don't re-optimize the graph
std::map<int, Transform> poses = _optimizedPoses; // if:
std::multimap<int, Link> constraints; // 1- there are no signatures retrieved,
optimizeCurrentMap(signature->id(), false, poses, &constraints); // 2- we are relocalizing on a node already in the optimized graph
UASSERT(poses.find(signature->id()) != poses.end()); if(!_memory->isIncremental() &&
signaturesRetrieved.size() == 0 &&
// Check added loop closures have broken the graph signature->getLinks().size() &&
// (in case of wrong loop closures). uContains(_optimizedPoses, signature->getLinks().begin()->first))
bool updateConstraints = true;
if(_optimizationMaxLinearError > 0.0f && loopClosureLinksAdded.size())
{ {
const Link * maxLinearLink = 0; // If there are no signatures retrieved, we don't
for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter) // 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)
{ {
Transform t1 = uValue(poses, iter->second.from(), Transform()); // update all previous nodes
Transform t2 = uValue(poses, iter->second.to(), Transform()); // Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
Transform t = t1.inverse()*t2; // parameters just changed state, we should put back all poses without map correction.
float linearError = uMax3( Transform oldPose = _optimizedPoses.at(signature->getLinks().begin()->first);
fabs(iter->second.transform().x() - t.x()), Transform u = signature->getPose() * signature->getLinks().begin()->second.transform();
fabs(iter->second.transform().y() - t.y()), Transform up = u * oldPose.inverse();
fabs(iter->second.transform().z() - t.z())); Transform mapCorrectionInv = _mapCorrection.inverse();
if(linearError > maxLinearError) for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
{ {
maxLinearError = linearError; iter->second = mapCorrectionInv * up * iter->second;
maxLinearLink = &iter->second;
} }
_optimizedPoses.at(signature->id()) = signature->getPose();
} }
else
if(maxLinearError > _optimizationMaxLinearError)
{ {
UWARN("Rejecting all added loop closures (%d) in this " _optimizedPoses.at(signature->id()) = _optimizedPoses.at(signature->getLinks().begin()->first) * signature->getLinks().begin()->second.transform().inverse();
"iteration because a wrong loop closure has been "
"detected after graph optimization, resulting in "
"a maximum graph error of %f m (edge %d->%d). The "
"maximum error parameter is %f m.",
(int)loopClosureLinksAdded.size(),
maxLinearError,
maxLinearLink->from(),
maxLinearLink->to(),
_optimizationMaxLinearError);
for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
{
_memory->removeLink(iter->first, iter->second);
UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
}
updateConstraints = false;
_loopClosureHypothesis.first = 0;
lastLocalSpaceClosureId = 0;
rejectedHypothesis = true;
} }
} }
else
if(updateConstraints)
{ {
UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
_optimizedPoses = poses; UINFO("Update map correction");
_constraints = constraints; std::map<int, Transform> poses = _optimizedPoses;
std::multimap<int, Link> constraints;
optimizeCurrentMap(signature->id(), false, poses, &constraints);
UASSERT(poses.find(signature->id()) != poses.end());
// Check added loop closures have broken the graph
// (in case of wrong loop closures).
bool updateConstraints = true;
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)
{
Transform t1 = uValue(poses, iter->second.from(), Transform());
Transform t2 = uValue(poses, iter->second.to(), Transform());
Transform t = t1.inverse()*t2;
float linearError = uMax3(
fabs(iter->second.transform().x() - t.x()),
fabs(iter->second.transform().y() - t.y()),
fabs(iter->second.transform().z() - t.z()));
if(linearError > maxLinearError)
{
maxLinearError = linearError;
maxLinearLink = &iter->second;
}
}
if(maxLinearError > _optimizationMaxLinearError)
{
UWARN("Rejecting all added loop closures (%d) in this "
"iteration because a wrong loop closure has been "
"detected after graph optimization, resulting in "
"a maximum graph error of %f m (edge %d->%d). The "
"maximum error parameter is %f m.",
(int)loopClosureLinksAdded.size(),
maxLinearError,
maxLinearLink->from(),
maxLinearLink->to(),
_optimizationMaxLinearError);
for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
{
_memory->removeLink(iter->first, iter->second);
UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
}
updateConstraints = false;
_loopClosureHypothesis.first = 0;
lastLocalSpaceClosureId = 0;
rejectedHypothesis = true;
}
}
if(updateConstraints)
{
UINFO("Updated local map (old size=%d, new size=%d)", (int)_optimizedPoses.size(), (int)poses.size());
_optimizedPoses = poses;
_constraints = constraints;
}
} }
// Update map correction, it should be identify when optimizing from the last node // Update map correction, it should be identify when optimizing from the last node