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

Auto graph repairing: detect and remove bad loop closures accepted in the past that block new good loop closures to be accepted

Browse Source
This commit is contained in:
matlabbe
2026-04-20 15:21:40 -07:00
parent 9cbe84e445
commit a5af77c7b1
5 changed files with 134 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Rtabmap.h
+1
View File
@@ -379,6 +379,7 @@ private:
std::map<int, Transform> _odomCachePoses; // used in localization mode to reject loop closures
std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
std::map<int, Transform> _markerPriors;
std::pair<int, int> _lastRejectedLoopClosureIds;
std::set<int> _nodesToRepublish;
corelib/include/rtabmap/core/Statistics.h
+4
View File
@@ -78,6 +78,10 @@ class RTABMAP_CORE_EXPORT Statistics
RTABMAP_STATS(Loop, Optimization_iterations, );
RTABMAP_STATS(Loop, Optimization_max_error_from_id, );
RTABMAP_STATS(Loop, Optimization_max_error_to_id, );
RTABMAP_STATS(Loop, Optimization_max_ang_error_from_id, );
RTABMAP_STATS(Loop, Optimization_max_ang_error_to_id, );
RTABMAP_STATS(Loop, Optimization_max_error_removed_from_id, );
RTABMAP_STATS(Loop, Optimization_max_error_removed_to_id, );
RTABMAP_STATS(Loop, Linear_variance,);
RTABMAP_STATS(Loop, Angular_variance,);
RTABMAP_STATS(Loop, Landmark_detected,);
corelib/src/Rtabmap.cpp
+123 -27
View File
@@ -173,6 +173,7 @@ Rtabmap::Rtabmap() :
_mapCorrection(Transform::getIdentity()),
_lastLocalizationNodeId(0),
_currentSessionHasGPS(false),
_lastRejectedLoopClosureIds(0,0),
_pathStatus(0),
_pathCurrentIndex(0),
_pathGoalIndex(0),
@@ -936,6 +937,7 @@ int Rtabmap::triggerNewMap()
UINFO("New map triggered, new map = %d", mapId);
_optimizedPoses.clear();
_constraints.clear();
_lastRejectedLoopClosureIds = std::make_pair(0,0);
if(_bayesFilter)
{
@@ -1108,6 +1110,7 @@ void Rtabmap::resetMemory()
_globalScanMap.clear();
_globalScanMapPoses.clear();
_nodesToRepublish.clear();
_lastRejectedLoopClosureIds = std::make_pair(0,0);
this->clearPath(0);
if(_memory)
@@ -3217,8 +3220,9 @@ bool Rtabmap::process(
float maxLinearErrorRatio = 0.0f;
float maxAngularError = 0.0f;
float maxAngularErrorRatio = 0.0f;
int maxLinearErrorFromId = 0;
int maxLinearErrorToId = 0;
std::pair<int, int> maxLinearErrorIds(0,0);
std::pair<int, int> maxAngularErrorIds(0,0);
std::pair<int, int> maxLinearErrorRemovedIds(0,0);
double optimizationError = 0.0;
int optimizationIterations = 0;
Transform previousMapCorrection;
@@ -3389,8 +3393,7 @@ bool Rtabmap::process(
if(maxLinearLink)
{
maxLinearErrorFromId = maxLinearLink->from();
maxLinearErrorToId = maxLinearLink->to();
maxLinearErrorIds = std::make_pair(maxLinearLink->from(), maxLinearLink->to());
UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
maxLinearError,
maxLinearLink->from(),
@@ -3433,6 +3436,7 @@ bool Rtabmap::process(
}
if(maxAngularLink)
{
maxAngularErrorIds = std::make_pair(maxAngularLink->from(), maxAngularLink->to());
UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
maxAngularError*180.0f/CV_PI,
maxAngularLink->from(),
@@ -3541,8 +3545,7 @@ bool Rtabmap::process(
if(maxLinearLink)
{
maxLinearErrorFromId = maxLinearLink->from();
maxLinearErrorToId = maxLinearLink->to();
maxLinearErrorIds = std::make_pair(maxLinearLink->from(), maxLinearLink->to());
UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
maxLinearError,
maxLinearLink->from(),
@@ -3585,6 +3588,7 @@ bool Rtabmap::process(
}
if(maxAngularLink)
{
maxAngularErrorIds = std::make_pair(maxAngularLink->from(), maxAngularLink->to());
UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f, thr=%f)",
maxAngularError*180.0f/CV_PI,
maxAngularLink->from(),
@@ -3893,28 +3897,106 @@ bool Rtabmap::process(
bool reject = false;
if(maxLinearLink)
{
maxLinearErrorFromId = maxLinearLink->from();
maxLinearErrorToId = maxLinearLink->to();
maxLinearErrorIds = std::make_pair(maxLinearLink->from(), maxLinearLink->to());
UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
if(_optimizationMaxError > 0.0f && maxLinearErrorRatio > _optimizationMaxError)
{
UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
"iteration because a wrong loop closure has been "
"detected after graph optimization, resulting in "
"a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
"maximum error ratio parameter \"%s\" is %f of std deviation.",
(int)loopClosureLinksAdded.size(),
loopClosureLinksAdded.front().first,
loopClosureLinksAdded.front().second,
maxLinearErrorRatio,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearLink->type(),
maxLinearError,
sqrt(maxLinearLink->transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
_optimizationMaxError);
reject = true;
if( maxLinearErrorIds == _lastRejectedLoopClosureIds &&
graph::findLink(constraints, maxLinearErrorIds.first, maxLinearErrorIds.second) != constraints.end())
{
UWARN("We detected 2 consecutive loop closure rejections because of the same loop closure link (%d->%d), trying optimization again without that link...",
maxLinearErrorIds.first, maxLinearErrorIds.second);
std::map<int, Transform> posesOut;
std::multimap<int, Link> edgeConstraintsOut;
int fromId = poses.lower_bound(1)->first;
_graphOptimizer->getConnectedGraph(fromId, poses, constraints, posesOut, edgeConstraintsOut);
cv::Mat subCovariance;
double subOptimizationError = 0.0;
int subOptimizationIterations = 0;
std::map<int, Transform> subPoses = _graphOptimizer->optimize(fromId, posesOut, edgeConstraintsOut, subCovariance, 0, &subOptimizationError, &subOptimizationIterations);
if(subPoses.empty())
{
UWARN("Optimization failed when trying to repair graph.");
reject = true;
}
else
{
const Link * subMaxLinearLink = 0;
float subMaxLinearErrorRatio = 0;
float subMaxAngularErrorRatio = 0;
float subMaxLinearError = 0;
float subMaxAngularError = 0;
graph::computeMaxGraphErrors(
subPoses,
edgeConstraintsOut,
subMaxLinearErrorRatio,
subMaxAngularErrorRatio,
subMaxLinearError,
subMaxAngularError,
&subMaxLinearLink,
0);
if(subMaxLinearLink == 0)
{
UWARN("Could not compute graph errors! Wrong loop closures could be accepted!");
reject = true;
}
else if(subMaxLinearErrorRatio > _optimizationMaxError)
{
UWARN("Optimization error is still high (%f, on link %d->%d) after removing loop closure with highest error.",
subMaxLinearErrorRatio, subMaxLinearLink->from(), subMaxLinearLink->to());
reject = true;
}
else
{
UWARN("Optimization error is lower (%f, on link %d->%d) after removing loop "
"closure with highest error. We will remove the old link (%d->%d) and accept the new one (%d->%d).",
subMaxLinearErrorRatio, subMaxLinearLink->from(), subMaxLinearLink->to(),
maxLinearLink->from(), maxLinearLink->to(),
loopClosureLinksAdded.front().first, loopClosureLinksAdded.front().second);
_memory->removeLink(maxLinearLink->from(), maxLinearLink->to());
maxLinearErrorRemovedIds = std::make_pair(maxLinearLink->from(), maxLinearLink->to()),
poses = subPoses;
constraints = edgeConstraintsOut;
maxLinearLink = subMaxLinearLink;
maxLinearErrorRatio = subMaxLinearErrorRatio;
maxAngularErrorRatio = subMaxAngularErrorRatio;
maxLinearError = subMaxLinearError;
maxAngularError = subMaxAngularError;
covariance = subCovariance;
optimizationError = subOptimizationError;
optimizationIterations = subOptimizationIterations;
maxLinearErrorIds = std::make_pair(maxLinearLink->from(), maxLinearLink->to());
}
}
}
else {
reject = true;
}
if(reject)
{
UWARN("Rejecting all added loop closures (%d, first is %d <-> %d) in this "
"iteration because a wrong loop closure has been "
"detected after graph optimization, resulting in "
"a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
"maximum error ratio parameter \"%s\" is %f of std deviation.",
(int)loopClosureLinksAdded.size(),
loopClosureLinksAdded.front().first,
loopClosureLinksAdded.front().second,
maxLinearErrorRatio,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearLink->type(),
maxLinearError,
sqrt(maxLinearLink->transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
_optimizationMaxError);
}
if(reject && maxLinearLink->type() != Link::kNeighbor)
{
_lastRejectedLoopClosureIds = maxLinearErrorIds;
}
}
else if(_optimizationMaxError == 0.0f && maxLinearErrorRatio>100 && !_graphOptimizer->isRobust())
{
@@ -3932,6 +4014,7 @@ bool Rtabmap::process(
}
if(maxAngularLink)
{
maxAngularErrorIds = std::make_pair(maxAngularLink->from(), maxAngularLink->to());
UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
if(_optimizationMaxError > 0.0f && maxAngularErrorRatio > _optimizationMaxError)
{
@@ -4116,8 +4199,21 @@ bool Rtabmap::process(
statistics_.addStatistic(Statistics::kLoopOptimization_max_error_ratio(), maxLinearErrorRatio);
statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error(), maxAngularError*180.0f/M_PI);
statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error_ratio(), maxAngularErrorRatio);
statistics_.addStatistic(Statistics::kLoopOptimization_max_error_from_id(), maxLinearErrorFromId);
statistics_.addStatistic(Statistics::kLoopOptimization_max_error_to_id(), maxLinearErrorToId);
if(maxLinearErrorIds.first>0)
{
statistics_.addStatistic(Statistics::kLoopOptimization_max_error_from_id(), maxLinearErrorIds.first);
statistics_.addStatistic(Statistics::kLoopOptimization_max_error_to_id(), maxLinearErrorIds.second);
}
if(maxAngularErrorIds.first>0)
{
statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error_from_id(), maxAngularErrorIds.first);
statistics_.addStatistic(Statistics::kLoopOptimization_max_ang_error_to_id(), maxAngularErrorIds.second);
}
if(maxLinearErrorRemovedIds.first > 0)
{
statistics_.addStatistic(Statistics::kLoopOptimization_max_error_removed_from_id(), maxLinearErrorRemovedIds.first);
statistics_.addStatistic(Statistics::kLoopOptimization_max_error_removed_to_id(), maxLinearErrorRemovedIds.second);
}
statistics_.addStatistic(Statistics::kLoopOptimization_error(), optimizationError);
statistics_.addStatistic(Statistics::kLoopOptimization_iterations(), optimizationIterations);
statistics_.addStatistic(Statistics::kLoopLandmark_detected(), landmarksDetected.empty()?0:-landmarksDetected.begin()->first);
corelib/src/global_map/OccupancyGrid.cpp
+2 -2
View File
@@ -222,14 +222,14 @@ void OccupancyGrid::assemble(const std::list<std::pair<int, Transform> > & newPo
if(!cache().empty())
{
UDEBUG("Updating from cache");
UDEBUG("Updating %ld poses from cache", newPoses.size());
for(std::list<std::pair<int, Transform> >::const_iterator iter = newPoses.begin(); iter!=newPoses.end(); ++iter)
{
if(uContains(cache(), iter->first))
{
const LocalGrid & localGrid = cache().at(iter->first);
UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
//UDEBUG("Adding grid %d: ground=%d obstacles=%d empty=%d", iter->first, localGrid.groundCells.cols, localGrid.obstacleCells.cols, localGrid.emptyCells.cols);
//ground
cv::Mat ground;
guilib/src/DatabaseViewer.cpp
+4 -4
View File
@@ -9450,7 +9450,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to());
if(findIter != linksRemoved_.end())
{
UDEBUG("Removed link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
//UDEBUG("Removed link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
continue; // don't add this link
}
@@ -9467,7 +9467,7 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
{
links.insert(*findIter);
}
UDEBUG("Updated link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
//UDEBUG("Updated link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
continue;
}
@@ -9486,11 +9486,11 @@ std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
if(findIter->second.from() != findIter->second.to()) {
links.insert(std::make_pair(findIter->second.to(), findIter->second.inverse())); // return both ways
}
UDEBUG("Added refined link (%d->%d, %d)", findIter->second.from(), findIter->second.to(), findIter->second.type());
//UDEBUG("Added refined link (%d->%d, %d)", findIter->second.from(), findIter->second.to(), findIter->second.type());
continue;
}
UDEBUG("Added link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
//UDEBUG("Added link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
links.insert(*iter);
if(iter->second.from() != iter->second.to()) {
links.insert(std::make_pair(iter->second.to(), iter->second.inverse())); // return both ways