Make default config suitable for float. Implement better handling of non-finite increments.

include/basalt/utils/vio_config.h

@@ -78,8 +78,6 @@ struct VioConfig {
   double vio_lm_lambda_initial;
   double vio_lm_lambda_min;
   double vio_lm_lambda_max;
-  int vio_lm_landmark_damping_variant;  // currently unused
-  int vio_lm_pose_damping_variant;
 
   bool vio_scale_jacobian;
 

include/basalt/vi_estimator/sqrt_keypoint_vio.h

@@ -248,6 +248,8 @@ class SqrtKeypointVioEstimator : public VioEstimatorBase,
 
   VioConfig config;
 
+  constexpr static Scalar vee_factor = Scalar(2.0);
+  constexpr static Scalar initial_vee = Scalar(2.0);
   Scalar lambda, min_lambda, max_lambda, lambda_vee;
 
   std::shared_ptr<std::thread> processing_thread;

include/basalt/vi_estimator/sqrt_keypoint_vo.h

@@ -238,6 +238,8 @@ class SqrtKeypointVoEstimator : public VioEstimatorBase,
 
   VioConfig config;
 
+  constexpr static Scalar vee_factor = Scalar(2.0);
+  constexpr static Scalar initial_vee = Scalar(2.0);
   Scalar lambda, min_lambda, max_lambda, lambda_vee;
 
   std::shared_ptr<std::thread> processing_thread;

src/utils/vio_config.cpp

@@ -75,11 +75,9 @@ VioConfig::VioConfig() {
   vio_enforce_realtime = false;
 
   vio_use_lm = false;
-  vio_lm_lambda_initial = 1e-8;
+  vio_lm_lambda_initial = 1e-4;
-  vio_lm_lambda_min = 1e-32;
+  vio_lm_lambda_min = 1e-6;
   vio_lm_lambda_max = 1e2;
-  vio_lm_landmark_damping_variant = 0;  // currently unused
-  vio_lm_pose_damping_variant = 0;
 
   vio_scale_jacobian = true;
 
@@ -192,8 +190,6 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
   ar(CEREAL_NVP(config.vio_lm_lambda_initial));
   ar(CEREAL_NVP(config.vio_lm_lambda_min));
   ar(CEREAL_NVP(config.vio_lm_lambda_max));
-  ar(CEREAL_NVP(config.vio_lm_landmark_damping_variant));
-  ar(CEREAL_NVP(config.vio_lm_pose_damping_variant));
 
   ar(CEREAL_NVP(config.vio_scale_jacobian));
 

src/vi_estimator/sqrt_keypoint_vio.cpp

@@ -1202,21 +1202,32 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
 
           stats.add("get_dense_H_b", t.reset()).format("ms");
 
-          if (config.vio_lm_pose_damping_variant == 1) {
+          int iter = 0;
+          bool inc_valid = false;
+          constexpr int max_num_iter = 3;
+
+          while (iter < max_num_iter && !inc_valid) {
             VecX Hdiag_lambda = (H.diagonal() * lambda).cwiseMax(min_lambda);
-            H.diagonal() += Hdiag_lambda;
+            MatX H_copy = H;
+            H_copy.diagonal() += Hdiag_lambda;
+
+            Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H_copy);
+            inc = ldlt.solve(b);
+            stats.add("solve", t.reset()).format("ms");
+
+            if (!inc.array().isFinite().all()) {
+              lambda = lambda_vee * lambda;
+              lambda_vee *= vee_factor;
+            } else {
+              inc_valid = true;
+            }
+            iter++;
           }
 
-          Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
+          if (!inc_valid) {
-          inc = ldlt.solve(b);
+            std::cerr << "Still invalid inc after " << max_num_iter
-          stats.add("solve", t.reset()).format("ms");
+                      << " iterations." << std::endl;
-
+          }
-          // TODO: instead of crashing, backtrack and increase damping, but make
-          // sure it does not go unnoticed. (Note: right now, without further
-          // handling, Sophus would crash anyway when trying to apply and
-          // increment with NaNs or inf)
-          BASALT_ASSERT_MSG(!inc.array().isFinite().all(),
-                            "numeric failure during");
         }
 
         // backup state (then apply increment and check cost decrease)
@@ -1295,8 +1306,8 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
                              relative_decrease, step_norminf);
           }
 
-          // TODO: consider to remove assert. For now we want to test if we even
+          // TODO: consider to remove assert. For now we want to test if we
-          // run into the l_diff <= 0 case ever in practice
+          // even run into the l_diff <= 0 case ever in practice
           // BASALT_ASSERT_STREAM(l_diff > 0, "l_diff " << l_diff);
 
           // l_diff <= 0 is a theoretical possibility if the model cost change
@@ -1339,7 +1350,6 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
               1 - std::pow<Scalar>(2 * relative_decrease - 1, 3));
           lambda = std::max(min_lambda, lambda);
 
-          constexpr Scalar initial_vee = Scalar(2.0);
           lambda_vee = initial_vee;
 
           it++;
@@ -1368,7 +1378,6 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
           }
 
           lambda = lambda_vee * lambda;
-          constexpr Scalar vee_factor = Scalar(2.0);
           lambda_vee *= vee_factor;
 
           //        lambda = std::max(min_lambda, lambda);

src/vi_estimator/sqrt_keypoint_vo.cpp

@@ -1099,21 +1099,32 @@ void SqrtKeypointVoEstimator<Scalar_>::optimize() {
 
         stats.add("get_dense_H_b", t.reset()).format("ms");
 
-        if (config.vio_lm_pose_damping_variant == 1) {
+        int iter = 0;
+        bool inc_valid = false;
+        constexpr int max_num_iter = 3;
+
+        while (iter < max_num_iter && !inc_valid) {
           VecX Hdiag_lambda = (H.diagonal() * lambda).cwiseMax(min_lambda);
-          H.diagonal() += Hdiag_lambda;
+          MatX H_copy = H;
+          H_copy.diagonal() += Hdiag_lambda;
+
+          Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H_copy);
+          inc = ldlt.solve(b);
+          stats.add("solve", t.reset()).format("ms");
+
+          if (!inc.array().isFinite().all()) {
+            lambda = lambda_vee * lambda;
+            lambda_vee *= vee_factor;
+          } else {
+            inc_valid = true;
+          }
+          iter++;
         }
 
-        Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
+        if (!inc_valid) {
-        inc = ldlt.solve(b);
+          std::cerr << "Still invalid inc after " << max_num_iter
-        stats.add("solve", t.reset()).format("ms");
+                    << " iterations." << std::endl;
-
+        }
-        // TODO: instead of crashing, backtrack and increase damping, but make
-        // sure it does not go unnoticed. (Note: right now, without further
-        // handling, Sophus would crash anyway when trying to apply and
-        // increment with NaNs or inf)
-        BASALT_ASSERT_MSG(!inc.array().isFinite().all(),
-                          "numeric failure during");
       }
 
       // backup state (then apply increment and check cost decrease)
@@ -1225,7 +1236,6 @@ void SqrtKeypointVoEstimator<Scalar_>::optimize() {
             1 - std::pow<Scalar>(2 * relative_decrease - 1, 3));
         lambda = std::max(min_lambda, lambda);
 
-        constexpr Scalar initial_vee = Scalar(2.0);
         lambda_vee = initial_vee;
 
         it++;
@@ -1253,7 +1263,6 @@ void SqrtKeypointVoEstimator<Scalar_>::optimize() {
         }
 
         lambda = lambda_vee * lambda;
-        constexpr Scalar vee_factor = Scalar(2.0);
         lambda_vee *= vee_factor;
 
         //        lambda = std::max(min_lambda, lambda);