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.

2026-10-03 16:47:47 +08:00 · 2026-04-30 17:06:22 -07:00
parent a5af77c7b1
commit ad41df3020
10 changed files with 665 additions and 608 deletions
@@ -118,15 +118,18 @@ Transform RTABMAP_CORE_EXPORT calcRMSE(
 		float & rotational_max,
 		bool align2D = false);

-void RTABMAP_CORE_EXPORT computeMaxGraphErrors(
+struct MaxGraphErrors
+{
+	float linear=-1.0f;         // absolute error (m) of the link with maximum linear error
+	float angular=-1.0f;        // absolute error (rad)  of the link with maximum angular error
+	float linearRatio=-1.0f;    // Ratio = absolute error (m) / linear std (m), of the link with maximum linear error
+	float angularRatio=-1.0f;   // Ratio = absolute error (rad) / angular std (rad), of the link with maximum angular error
+	const Link * linearLink=0;  // link with maximum linear error
+	const Link * angularLink=0; // link with maximum angular error
+};
+MaxGraphErrors RTABMAP_CORE_EXPORT computeMaxGraphErrors(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		float & maxLinearErrorRatio,
-		float & maxAngularErrorRatio,
-		float & maxLinearError,
-		float & maxAngularError,
-		const Link ** maxLinearErrorLink = 0,
-		const Link ** maxAngularErrorLink = 0,
 		bool for3DoF = false);

 std::vector<double> RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap<int, Link> & links);
@@ -379,6 +379,7 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
    RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: 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).");
    RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 3.0,   uFormat("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 \"%s\", the disabled loop closure links will be removed.", kOptimizerRobust().c_str()));
+    RTABMAP_PARAM(RGBD, OptimizeMaxErrorRepairRadius, float, 0.0,   uFormat("If two consecutive loop closures are rejected by %s 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.", kRGBDOptimizeMaxError().c_str()));
    RTABMAP_PARAM(RGBD, MaxLoopClosureDistance,   float, 0.0,   "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled).");
    RTABMAP_PARAM(RGBD, ForceOdom3DoF,            bool, true,  uFormat("Force odometry pose to be 3DoF if %s=true.", kRegForce3DoF().c_str()));
    RTABMAP_PARAM(RGBD, StartAtOrigin,            bool, false, uFormat("If true, rtabmap will assume the robot is starting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode (%s=false).", kMemIncrementalMemory().c_str()));
@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Statistics.h"
 #include "rtabmap/core/Link.h"
 #include "rtabmap/core/ProgressState.h"
+#include "rtabmap/core/Graph.h"

 #include <opencv2/core/core.hpp>
 #include <list>
@@ -264,6 +265,13 @@ private:
 			std::multimap<int, Link> * constraints = 0,
 			double * error = 0,
 			int * iterationsDone = 0) const;
+	std::list<std::pair<int, int> > repairGraph(
+		graph::MaxGraphErrors & maxGraphErrors,
+		std::map<int, Transform> & poses,
+		std::multimap<int, Link> & constraints,
+		double & optimizationError,
+		int & optimizationIterations,
+		cv::Mat & optimizationCovariance);
 	void updateGoalIndex();
 	bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);

@@ -320,6 +328,7 @@ private:
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
 	float _optimizationMaxError;
+	float _optimizationMaxErrorRepairRadius;
 	bool _startNewMapOnLoopClosure;
 	bool _startNewMapOnGoodSignature;
 	float _goalReachedRadius; // meters
@@ -82,6 +82,7 @@ class RTABMAP_CORE_EXPORT Statistics
 	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, Optimization_max_error_removed_count, );
 	RTABMAP_STATS(Loop, Linear_variance,);
 	RTABMAP_STATS(Loop, Angular_variance,);
 	RTABMAP_STATS(Loop, Landmark_detected,);
@@ -927,21 +927,12 @@ Transform calcRMSE (
 	return t;
 }

-void computeMaxGraphErrors(
+MaxGraphErrors computeMaxGraphErrors(
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		float & maxLinearErrorRatio,
-		float & maxAngularErrorRatio,
-		float & maxLinearError,
-		float & maxAngularError,
-		const Link ** maxLinearErrorLink,
-		const Link ** maxAngularErrorLink,
 		bool force3DoF)
 {
-	maxLinearErrorRatio = -1;
-	maxAngularErrorRatio = -1;
-	maxLinearError = -1;
-	maxAngularError = -1;
+	MaxGraphErrors maxError;

 	UDEBUG("poses=%d links=%d", (int)poses.size(), (int)links.size());
 	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
@@ -963,19 +954,7 @@ void computeMaxGraphErrors(
 					iter->second.to(),
 					t2.prettyPrint().c_str());

-				if(maxLinearErrorLink)
-				{
-					*maxLinearErrorLink = 0;
-				}
-				if(maxAngularErrorLink)
-				{
-					*maxAngularErrorLink = 0;
-				}
-				maxLinearErrorRatio = -1;
-				maxAngularErrorRatio = -1;
-				maxLinearError = -1;
-				maxAngularError = -1;
-				return;
+				return MaxGraphErrors();
 			}

 			Transform t;
@@ -999,14 +978,11 @@ void computeMaxGraphErrors(
 			UASSERT(iter->second.transVariance(false)>0.0);
 			float stddevLinear = sqrt(iter->second.transVariance(false));
 			float linearErrorRatio = linearError/stddevLinear;
-			if(linearErrorRatio > maxLinearErrorRatio)
+			if(linearErrorRatio > maxError.linearRatio)
 			{
-				maxLinearError = linearError;
-				maxLinearErrorRatio = linearErrorRatio;
-				if(maxLinearErrorLink)
-				{
-					*maxLinearErrorLink = &iter->second;
-				}
+				maxError.linear = linearError;
+				maxError.linearRatio = linearErrorRatio;
+				maxError.linearLink = &iter->second;
 			}

 			// For landmark links, don't compute angular error if it doesn't estimate orientation
@@ -1031,18 +1007,16 @@ void computeMaxGraphErrors(
 				UASSERT(iter->second.rotVariance(false)>0.0);
 				float stddevAngular = sqrt(iter->second.rotVariance(false));
 				float angularErrorRatio = angularError/stddevAngular;
-				if(angularErrorRatio > maxAngularErrorRatio)
+				if(angularErrorRatio > maxError.angularRatio)
 				{
-					maxAngularError = angularError;
-					maxAngularErrorRatio = angularErrorRatio;
-					if(maxAngularErrorLink)
-					{
-						*maxAngularErrorLink = &iter->second;
-					}
+					maxError.angular = angularError;
+					maxError.angularRatio = angularErrorRatio;
+					maxError.angularLink = &iter->second;
 				}
 			}
 		}
 	}
+	return maxError;
 }

 std::vector<double> getMaxOdomInf(const std::multimap<int, Link> & links)
@@ -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)
 			{
-				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)
 			{
-				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);
 				}
@@ -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)
 													{
-														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)
 													{
-														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);
 														}
@@ -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());
@@ -63,7 +63,7 @@
          <property name="geometry">
           <rect>
            <x>0</x>
-            <y>-96</y>
+            <y>0</y>
            <width>684</width>
            <height>5218</height>
           </rect>
@@ -95,7 +95,7 @@
                <enum>QFrame::Raised</enum>
               </property>
               <property name="currentIndex">
-                <number>9</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>
@@ -22139,7 +22174,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                <double>0.010000000000000</double>
                               </property>
                               <property name="value">
-                                <double>9.805110000000000</double>
+                                <double>9.805110000000001</double>
                               </property>
                              </widget>
                             </item>