i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 00:57:46 +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/DatabaseViewer.cpp
+22 -34
View File
@@ -9128,10 +9128,6 @@ bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool si
int fromId = newLink.from();
std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
linksIn.insert(std::make_pair(newLink.from(), newLink));
const Link * maxLinearLink = 0;
const Link * maxAngularLink = 0;
float maxLinearErrorRatio = 0.0f;
float maxAngularErrorRatio = 0.0f;
Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
std::map<int, Transform> poses;
std::multimap<int, Link> links;
@@ -9164,51 +9160,43 @@ bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool si
std::string msg;
if(poses.size())
{
float maxLinearError = 0.0f;
float maxAngularError = 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, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
if(maxLinearErrorRatio > maxOptimizationError)
UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.linear, maxGraphErrors.linearLink.from(), maxGraphErrors.linearLink.to(), maxGraphErrors.linearLink.transVariance(), maxGraphErrors.linear/sqrt(maxGraphErrors.linearLink.transVariance()));
if(maxGraphErrors.linearRatio > maxOptimizationError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large (abs=%f m) after optimization (ratio %f for edge %d->%d, stddev=%f m). "
"\"%s\" is %f.",
newLink.from(),
newLink.to(),
maxLinearError,
maxLinearErrorRatio,
maxLinearLink->from(),
maxLinearLink->to(),
sqrt(maxLinearLink->transVariance()),
maxGraphErrors.linear,
maxGraphErrors.linearRatio,
maxGraphErrors.linearLink.from(),
maxGraphErrors.linearLink.to(),
sqrt(maxGraphErrors.linearLink.transVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
maxOptimizationError);
}
}
if(maxAngularLink)
if(maxGraphErrors.angularLink.isValid())
{
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(maxAngularErrorRatio > maxOptimizationError)
UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxGraphErrors.angular*180.0f/CV_PI, maxGraphErrors.angularLink.from(), maxGraphErrors.angularLink.to(), maxGraphErrors.angularLink.rotVariance(), maxGraphErrors.angular/sqrt(maxGraphErrors.angularLink.rotVariance()));
if(maxGraphErrors.angularRatio > maxOptimizationError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large (abs=%f deg) after optimization (ratio %f for edge %d->%d, stddev=%f deg). "
"\"%s\" is %f.",
newLink.from(),
newLink.to(),
maxAngularError*180.0f/CV_PI,
maxAngularErrorRatio,
maxAngularLink->from(),
maxAngularLink->to(),
sqrt(maxAngularLink->rotVariance()),
maxGraphErrors.angular*180.0f/CV_PI,
maxGraphErrors.angularRatio,
maxGraphErrors.angularLink.from(),
maxGraphErrors.angularLink.to(),
sqrt(maxGraphErrors.angularLink.rotVariance()),
Parameters::kRGBDOptimizeMaxError().c_str(),
maxOptimizationError);
}
@@ -9450,7 +9438,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 +9455,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 +9474,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
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);
}
guilib/src/PreferencesDialog.cpp
+1
View File
@@ -1272,6 +1272,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kOptimizerVarianceIgnored().c_str());
_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
_ui->graphOptimization_maxError->setObjectName(Parameters::kRGBDOptimizeMaxError().c_str());
_ui->graphOptimization_maxErrorRepairRadius->setObjectName(Parameters::kRGBDOptimizeMaxErrorRepairRadius().c_str());
_ui->graphOptimization_gravitySigma->setObjectName(Parameters::kOptimizerGravitySigma().c_str());
_ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kOptimizerEpsilon().c_str());
_ui->graphOptimization_robust->setObjectName(Parameters::kOptimizerRobust().c_str());
guilib/src/ui/preferencesDialog.ui
+161 -126
View File
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>28</number>
<number>14</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -13813,6 +13813,152 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<string>Graph Optimization</string>
</property>
<layout class="QGridLayout" name="gridLayout_81" columnstretch="0,1">
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="graphOptimization_maxError">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="Line" name="line">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_521">
<property name="text">
<string>Ignore landmarks.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_527">
<property name="text">
<string>Gravity sigma value (&gt;=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with g2o and GTSAM optimization strategies.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="graphOptimization_priorsIgnored">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_151">
<property name="text">
<string>Optimize graph from the newest node.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_259">
<property name="text">
<string>Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. If used with robust graph optimization, the disabled loop closure links will be removed.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QDoubleSpinBox" name="graphOptimization_gravitySigma">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>10.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_45">
<property name="text">
<string>Graph optimization algorithm. See Optimizer panel.</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="9" column="1">
<widget class="QLabel" name="label_211">
<property name="text">
<string>-If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_431">
<property name="text">
<string>Ignore pose priors.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QComboBox" name="graphOptimization_type">
<item>
@@ -13837,18 +13983,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</item>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_45">
<property name="text">
<string>Graph optimization algorithm. See Optimizer panel.</string>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="graphOptimization_robust">
<item row="6" column="0">
<widget class="QCheckBox" name="graphOptimization_landmarksIgnored">
<property name="text">
<string/>
</property>
@@ -13867,82 +14003,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="graphOptimization_maxError">
<property name="suffix">
<item row="1" column="0">
<widget class="QCheckBox" name="graphOptimization_robust">
<property name="text">
<string/>
</property>
<property name="decimals">
<number>2</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>100.000000000000000</double>
</property>
<property name="singleStep">
<double>1.000000000000000</double>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_259">
<property name="text">
<string>Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. If used with robust graph optimization, the disabled loop closure links will be removed.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="0" rowspan="2">
<item row="7" column="0" rowspan="2">
<widget class="QCheckBox" name="graphOptimization_fromGraphEnd">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="Line" name="line">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_151">
<property name="text">
<string>Optimize graph from the newest node.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="8" column="1">
<widget class="QLabel" name="label_211">
<property name="text">
<string>-If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="9" column="1">
<item row="10" column="1">
<widget class="QLabel" name="label_183">
<property name="text">
<string>-If false, the graph is optimized from the oldest node of the current graph. It can be useful to preserve the map referential from the oldest node. An odometry correction between frames /map to /odom is computed. Warning: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
@@ -13955,50 +14030,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_431">
<property name="text">
<string>Ignore pose priors.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="graphOptimization_priorsIgnored">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="graphOptimization_landmarksIgnored">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_521">
<property name="text">
<string>Ignore landmarks.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_527">
<widget class="QLabel" name="label_795">
<property name="text">
<string>Gravity sigma value (&gt;=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with g2o and GTSAM optimization strategies.</string>
<string>Repairing graph radius. If two consecutive loop closures are rejected by the optimization error ratio on the same old loop closure link, we will remove that old link, and other old links under that radius if necessary, until optimization is accepted. When optimization is accepted, the old loop closure links are removed from the graph. This feature is useful to reject bad loop closures that were accepted previously. Set to 0 to disable this feature.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
@@ -14009,24 +14044,24 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="graphOptimization_gravitySigma">
<widget class="QDoubleSpinBox" name="graphOptimization_maxErrorRepairRadius">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>4</number>
<number>1</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>10.000000000000000</double>
<double>999.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
<double>1.000000000000000</double>
</property>
<property name="value">
<double>0.000000000000000</double>
<double>1.000000000000000</double>
</property>
</widget>
</item>