Added Optimizer tests and discovered some bugs (fixed)

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