Merge branch 'master' into qol_fixes_may3_2026

corelib/include/rtabmap/core/Graph.h

@@ -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
+	Link linearLink;  // link with maximum linear error
+	Link angularLink; // 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);

corelib/include/rtabmap/core/Parameters.h

@@ -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()));

corelib/include/rtabmap/core/Rtabmap.h

@@ -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
@@ -379,6 +388,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

@@ -78,6 +78,11 @@ 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, Optimization_max_error_removed_count, );
 	RTABMAP_STATS(Loop, Linear_variance,);
 	RTABMAP_STATS(Loop, Angular_variance,);
 	RTABMAP_STATS(Loop, Landmark_detected,);

corelib/src/Graph.cpp

@@ -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)

corelib/src/global_map/OccupancyGrid.cpp

@@ -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;