Added Optimizer tests and discovered some bugs (fixed)

corelib/include/rtabmap/core/Parameters.h

@@ -472,7 +472,8 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(g2o, Solver,            int, 0,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
 #endif
     RTABMAP_PARAM(g2o, Optimizer,         int, 0,          "0=Levenberg 1=GaussNewton");
-    RTABMAP_PARAM(g2o, PixelVariance,     double, 1.0,     "Pixel variance used for bundle adjustment.");
+    RTABMAP_PARAM(g2o, PixelVariance,     double, 1.0,     "Pixel variance used on the u/v axes of every bundle adjustment reprojection edge. Should approximate the squared 1-sigma keypoint localization error in pixels. Set higher (e.g. 4-9) if features are noisy (low texture, motion blur, low light, or large detector scale). Set lower (e.g. 0.01-0.1) if features are sub-pixel refined (Lucas-Kanade tracking, parabolic peak interpolation). Intuition: the lower the pixel variance, the more the optimizer trusts the keypoint positions.");
+    RTABMAP_PARAM(g2o, DisparityVariance, double, 1.0,     "Disparity variance used on the disparity axis (u - u_right) of stereo / RGB-D bundle adjustment edges. Defaults to the same value as PixelVariance for backward compatibility. Set higher (e.g. 2-4) if your depth source is noisier than your feature detector's u/v precision (typical for stereo block matchers / SGM at long range). Set lower (e.g. 0.01-0.1) if your depth source is more accurate than the u/v detector (typical for ToF / LiDAR-fused depth where range is measured directly rather than triangulated). Intuition: the lower the disparity variance, the more the optimizer trusts the depth measurements. Geometric note: wider baseline and/or higher image resolution improve a block matcher's effective disparity precision (larger disparity magnitudes and finer sub-pixel refinement), so wide-baseline high-resolution stereo pairs can usually afford a lower disparity variance (e.g. 0.1-0.5); narrow-baseline low-resolution pairs should keep it higher (e.g. 1-4).");
     RTABMAP_PARAM(g2o, RobustKernelDelta, double, 8,       "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
     RTABMAP_PARAM(g2o, Baseline,          double, 0.075,   "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.");
 

corelib/include/rtabmap/core/optimizer/OptimizerG2O.h

@@ -82,6 +82,7 @@ private:
 	int solver_;
 	int optimizer_;
 	double pixelVariance_;
+	double disparityVariance_;
 	double robustKernelDelta_;
 	double baseline_;
 };

corelib/src/CMakeLists.txt

@@ -912,6 +912,17 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU"
             optimizer/toro3d/treeoptimizer2.cpp
             PROPERTIES COMPILE_OPTIONS "-Wno-use-after-free")
     endif()
+
+    # GCC 12 -Wmaybe-uninitialized false positive: Eigen's SSE codepath
+    # _mm_loadu_pd's an uninitialized-by-design Eigen::Matrix<double,3,3>
+    # inside g2o::SBACam. GCC follows the SIMD load through and flags it as
+    # potentially uninitialized; Eigen relies on the caller to assign before
+    # reading. Only OptimizerG2O.cpp instantiates SBACam.
+    if(G2O_FOUND)
+        set_source_files_properties(
+            optimizer/OptimizerG2O.cpp
+            PROPERTIES COMPILE_OPTIONS "-Wno-maybe-uninitialized")
+    endif()
 endif()
 
 TARGET_LINK_LIBRARIES(rtabmap_core

corelib/src/Memory.cpp

@@ -6086,7 +6086,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						{
 							// Bearing/Range in 2D, set X as bearing and Y as range (see OptimizerGTSAM)
 							covariance(cv::Range(0,1), cv::Range(0,1)) *= _markerAngVariance;
-							covariance(cv::Range(1,3), cv::Range(1,3)) *= _markerLinVariance;
+							covariance(cv::Range(1,2), cv::Range(1,2)) *= _markerLinVariance;
 						}
 						else
 						{

corelib/src/optimizer/OptimizerG2O.cpp

@@ -161,6 +161,7 @@ OptimizerG2O::OptimizerG2O(const ParametersMap & parameters) :
 		solver_(Parameters::defaultg2oSolver()),
 		optimizer_(Parameters::defaultg2oOptimizer()),
 		pixelVariance_(Parameters::defaultg2oPixelVariance()),
+		disparityVariance_(Parameters::defaultg2oDisparityVariance()),
 		robustKernelDelta_(Parameters::defaultg2oRobustKernelDelta()),
 		baseline_(Parameters::defaultg2oBaseline())
 {
@@ -185,9 +186,11 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kg2oSolver(), solver_);
 	Parameters::parse(parameters, Parameters::kg2oOptimizer(), optimizer_);
 	Parameters::parse(parameters, Parameters::kg2oPixelVariance(), pixelVariance_);
+	Parameters::parse(parameters, Parameters::kg2oDisparityVariance(), disparityVariance_);
 	Parameters::parse(parameters, Parameters::kg2oRobustKernelDelta(), robustKernelDelta_);
 	Parameters::parse(parameters, Parameters::kg2oBaseline(), baseline_);
 	UASSERT(pixelVariance_ > 0.0);
+	UASSERT(disparityVariance_ > 0.0);
 	UASSERT(baseline_ >= 0.0);
 
 #ifdef RTABMAP_ORB_SLAM
@@ -1861,14 +1864,22 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						double variance = pixelVariance_;
 						if(uIsFinite(depth) && depth > 0.0 && baseline > 0.0)
 						{
+							// Stereo edge: per-axis info -- u, v use pixelVariance,
+							// disparity (u - u_right) uses disparityVariance.
+							// This keeps the depth measurement channel from being
+							// over-trusted relative to the u/v feature detector
+							// precision (or vice versa).
+							Eigen::Matrix3d stereoInfo = Eigen::Matrix3d::Zero();
+							stereoInfo(0, 0) = 1.0 / variance;
+							stereoInfo(1, 1) = 1.0 / variance;
+							stereoInfo(2, 2) = 1.0 / disparityVariance_;
 							// stereo edge
 #ifdef RTABMAP_ORB_SLAM
 							g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ();
 							float disparity = baseline * iterModel->second[camIndex].fx() / depth;
 							Eigen::Vector3d obs( pt.kpt.pt.x, pt.kpt.pt.y, pt.kpt.pt.x-disparity);
 							es->setMeasurement(obs);
-							//variance *= log(exp(1)+disparity);
-							es->setInformation(Eigen::Matrix3d::Identity() / variance);
+							es->setInformation(stereoInfo);
 							es->fx = iterModel->second[camIndex].fx();
 							es->fy = iterModel->second[camIndex].fy();
 							es->cx = iterModel->second[camIndex].cx();
@@ -1880,8 +1891,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 							float disparity = baseline * vcam->estimate().Kcam(0,0) / depth;
 							Eigen::Vector3d obs( pt.kpt.pt.x, pt.kpt.pt.y, pt.kpt.pt.x-disparity);
 							es->setMeasurement(obs);
-							//variance *= log(exp(1)+disparity);
-							es->setInformation(Eigen::Matrix3d::Identity() / variance);
+							es->setInformation(stereoInfo);
 							e = es;
 #endif
 						}

corelib/src/optimizer/gtsam/XYZFactor.h

@@ -59,6 +59,10 @@ public:
 #else
 		  boost::optional<gtsam::Matrix&> H = boost::none) const {
 #endif
+    if(H)
+    {
+      *H = gtsam::Matrix::Identity(3, 3);
+    }
     return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
   }
 };

corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h

@@ -41,14 +41,23 @@ namespace vertigo {
 #endif
         {
 
-          // calculate error
+          // calculate error: f(p1, p2, s) = E_raw(p1, p2) * s
           gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
+
+          // Jacobian w.r.t. the switch tangent: dE/ds = E_raw (the
+          // unscaled error). Must be captured BEFORE scaling `error` by
+          // s.value() below. Setting H3 to the scaled error (= E_raw*s)
+          // was a bug: at small s the switch gradient vanishes
+          // quadratically with s, so the optimizer stalls before driving
+          // the switch to 0 -- visibly worse outlier rejection than the
+          // g2o equivalent (EdgeSE3Switchable, which has the correct
+          // constant Jacobian-element).
+          if (H3) *H3 = error;
           error *= s.value();
 
           // handle derivatives
           if (H1) *H1 = *H1 * s.value();
           if (H2) *H2 = *H2 * s.value();
-          if (H3) *H3 = error;
 
           return error;
         };

corelib/test/CMakeLists.txt

@@ -118,6 +118,11 @@ add_executable(test_link test_link.cpp)
 target_link_libraries(test_link gtest_main rtabmap_core)
 add_test(NAME test_link COMMAND test_link)
 
+#Optimizer.h
+add_executable(test_optimizer test_optimizer.cpp)
+target_link_libraries(test_optimizer gtest_main rtabmap_core)
+add_test(NAME test_optimizer COMMAND test_optimizer)
+
 #GPS.h
 add_executable(test_gps test_gps.cpp)
 target_link_libraries(test_gps gtest_main rtabmap_core)

corelib/test/test_graph.cpp

@@ -303,21 +303,22 @@ static std::list<std::pair<int, Transform> > computeAStarPath(
 
 TEST(GraphTest, ComputePathAStarUpdateNewCostsChangesPath)
 {
-	// Detour 1→2→5→6→3 vs shortcut 1→2→4→6→3. No 5→3 edge so h(5,3) < cost(5→6→3).
-	// Node 5 is expanded before 4 (lower f-score). Node 6 is first reached from 5;
-	// expanding 4 relaxes the parent of 6 when updateNewCosts=true.
-	//
-	//        3  goal (10, 0)
-	//        |
-	//        6  (2, -5)
-	//       / \
-	//      5   4   (1,-2)  (1.5,-4)
-	//       \ /
-	//        2  (1, 0)
-	//        |
-	//        1  start (0, 0)
-	//
-	// Links: 1—2, 2—5, 2—4, 5—6, 4—6, 6—3  (no 5—3)
+	/* Detour 1→2→5→6→3 vs shortcut 1→2→4→6→3. No 5→3 edge so h(5,3) < cost(5→6→3).
+	   Node 5 is expanded before 4 (lower f-score). Node 6 is first reached from 5;
+	   expanding 4 relaxes the parent of 6 when updateNewCosts=true.
+	  
+	          3  goal (10, 0)
+	          |
+	          6  (2, -5)
+	         / \
+	        5   4   (1,-2)  (1.5,-4)
+	         \ /
+	          2  (1, 0)
+	          |
+	          1  start (0, 0)
+	  
+	   Links: 1—2, 2—5, 2—4, 5—6, 4—6, 6—3  (no 5—3)
+	*/
 	const std::map<int, Transform> poses = {
 		{1, Transform(0, 0, 0, 0, 0, 0)},
 		{2, Transform(1, 0, 0, 0, 0, 0)},
@@ -784,13 +785,13 @@ TEST(GraphTest, ComputeMaxGraphErrorsLandmarkSkipsUnconstrainedYaw)
 
 TEST(GraphTest, ComputeMaxGraphErrorsLandmarkTwoPoseObservations)
 {
-	// Same landmark -10 observed from poses 1 and 2 (two links sharing the landmark id).
-	//
-	//     -10  (1, 1)
-	//    /   \
-	//   1     2
-	// (0,0) (2,0)
-	//
+	/* Same landmark -10 observed from poses 1 and 2 (two links sharing the landmark id).
+	  
+	       -10  (1, 1)
+	      /   \
+	     1     2
+	   (0,0) (2,0)
+	*/
 	std::map<int, Transform> poses;
 	poses.insert(std::make_pair(1, Transform(0, 0, 0, 0, 0, 0)));
 	poses.insert(std::make_pair(2, Transform(2, 0, 0, 0, 0, 0)));