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

Auto graph repairing (#1691)

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

* Added parameter RGBD/OptimizeMaxErrorRepairRadius: added approach to repair the graph (remove bad loop closures added in the past). Refactored max optimization error function, split repairGraph to its own function.

* Handling optimization from graph end in repair

* MaxGraphErrors: changed link's pointers to copy directly to avoid seg fault down the road if not used properly

* Dont publish optimization statistics if it was not computed.
This commit is contained in:
matlabbe
2026-05-04 16:02:55 -07:00
committed by GitHub
parent 0cf2e45c81
commit 37f8fa63c5
11 changed files with 746 additions and 554 deletions
Download Patch File Download Diff File Expand all files Collapse all files
guilib/src/MainWindow.cpp
+18 -30
View File
@@ -6862,10 +6862,6 @@ void MainWindow::postProcessing(
}
std::multimap<int, Link> linksIn = _currentLinksMap;
linksIn.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, information)));
const Link * maxLinearLink = 0;
const Link * maxAngularLink = 0;
float maxLinearError = 0.0f;
float maxAngularError = 0.0f;
std::map<int, Transform> poses;
std::multimap<int, Link> links;
UASSERT(_currentPosesMap.find(fromId) != _currentPosesMap.end());
@@ -6880,51 +6876,43 @@ void MainWindow::postProcessing(
std::string msg;
if(poses.size())
{
float maxLinearErrorRatio = 0.0f;
float maxAngularErrorRatio = 0.0f;
graph::computeMaxGraphErrors(
graph::MaxGraphErrors maxGraphErrors = graph::computeMaxGraphErrors(
poses,
links,
maxLinearErrorRatio,
maxAngularErrorRatio,
maxLinearError,
maxAngularError,
&maxLinearLink,
&maxAngularLink);
if(maxLinearLink)
links);
if(maxGraphErrors.linearLink.isValid())
{
UINFO("Max optimization linear error = %f m (link %d->%d)", maxLinearError, maxLinearLink->from(), maxLinearLink->to());
if(maxLinearErrorRatio > optimizeMaxError)
UINFO("Max optimization linear error = %f m (link %d->%d)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to());
if(maxGraphErrors.linearRatio > optimizeMaxError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (%f m for edge %d->%d with ratio %f > std=%f m). "
"\"%s\" is %f.",
from,
to,
maxLinearError,
maxLinearLink->from(),
maxLinearLink->to(),
maxLinearErrorRatio,
sqrt(maxLinearLink->transVariance()),
maxGraphErrors.linear,
maxGraphErrors.linearLink.from(),
maxGraphErrors.linearLink.to(),
maxGraphErrors.linearRatio,
sqrt(maxGraphErrors.linearLink.transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
optimizeMaxError);
}
}
else if(maxAngularLink)
else if(maxGraphErrors.angularLink.isValid())
{
UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
if(maxAngularErrorRatio > optimizeMaxError)
UINFO("Max optimization angular error = %f deg (link %d->%d)", maxGraphErrors.angular*180.0f/M_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to());
if(maxGraphErrors.angularRatio > optimizeMaxError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (%f deg for edge %d->%d with ratio %f > std=%f deg). "
"\"%s\" is %f m.",
from,
to,
maxAngularError*180.0f/M_PI,
maxAngularLink->from(),
maxAngularLink->to(),
maxAngularErrorRatio,
sqrt(maxAngularLink->rotVariance()),
maxGraphErrors.angular*180.0f/M_PI,
maxGraphErrors.angularLink.from(),
maxGraphErrors.angularLink.to(),
maxGraphErrors.angularRatio,
sqrt(maxGraphErrors.angularLink.rotVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
optimizeMaxError);
}