Added LM options

data/euroc_config.json

@@ -20,6 +20,9 @@
         "config.vio_filter_iteration": 4,
         "config.vio_max_iterations": 7,
         "config.vio_enforce_realtime": false,
+        "config.vio_use_lm": false,
+        "config.vio_lm_lambda_min": 1e-32,
+        "config.vio_lm_lambda_max": 1e2,
 
         "config.mapper_obs_std_dev": 0.25,
         "config.mapper_obs_huber_thresh": 1.5,
@@ -33,6 +36,9 @@
         "config.mapper_second_best_test_ratio": 1.2,
         "config.mapper_bow_num_bits": 16,
         "config.mapper_min_triangulation_dist": 0.07,
-        "config.mapper_no_factor_weights": false
+        "config.mapper_no_factor_weights": false,
+        "config.mapper_use_lm": false,
+        "config.mapper_lm_lambda_min": 1e-32,
+        "config.mapper_lm_lambda_max": 1e3
     }
 }

data/tumvi_512_config.json

@@ -21,6 +21,9 @@
         "config.vio_filter_iteration": 4,
         "config.vio_max_iterations": 7,
         "config.vio_enforce_realtime": false,
+        "config.vio_use_lm": false,
+        "config.vio_lm_lambda_min": 1e-32,
+        "config.vio_lm_lambda_max": 1e2,
 
         "config.mapper_obs_std_dev": 0.25,
         "config.mapper_obs_huber_thresh": 1.5,
@@ -34,6 +37,9 @@
         "config.mapper_second_best_test_ratio": 1.2,
         "config.mapper_bow_num_bits": 16,
         "config.mapper_min_triangulation_dist": 0.07,
-        "config.mapper_no_factor_weights": false
+        "config.mapper_no_factor_weights": false,
+        "config.mapper_use_lm": false,
+        "config.mapper_lm_lambda_min": 1e-32,
+        "config.mapper_lm_lambda_max": 1e3
     }
 }

include/basalt/utils/vio_config.h

@@ -68,6 +68,10 @@ struct VioConfig {
 
   bool vio_enforce_realtime;
 
+  bool vio_use_lm;
+  double vio_lm_lambda_min;
+  double vio_lm_lambda_max;
+
   double mapper_obs_std_dev;
   double mapper_obs_huber_thresh;
   int mapper_detection_num_points;
@@ -81,5 +85,9 @@ struct VioConfig {
   int mapper_bow_num_bits;
   double mapper_min_triangulation_dist;
   bool mapper_no_factor_weights;
+
+  bool mapper_use_lm;
+  double mapper_lm_lambda_min;
+  double mapper_lm_lambda_max;
 };
 }  // namespace basalt

src/utils/vio_config.cpp

@@ -68,6 +68,10 @@ VioConfig::VioConfig() {
 
   vio_enforce_realtime = false;
 
+  vio_use_lm = false;
+  vio_lm_lambda_min = 1e-32;
+  vio_lm_lambda_max = 1e2;
+
   mapper_obs_std_dev = 0.25;
   mapper_obs_huber_thresh = 1.5;
   mapper_detection_num_points = 800;
@@ -81,6 +85,10 @@ VioConfig::VioConfig() {
   mapper_bow_num_bits = 16;
   mapper_min_triangulation_dist = 0.07;
   mapper_no_factor_weights = false;
+
+  mapper_use_lm = false;
+  mapper_lm_lambda_min = 1e-32;
+  mapper_lm_lambda_max = 1e2;
 }
 
 void VioConfig::save(const std::string& filename) {
@@ -132,6 +140,10 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
 
   ar(CEREAL_NVP(config.vio_enforce_realtime));
 
+  ar(CEREAL_NVP(config.vio_use_lm));
+  ar(CEREAL_NVP(config.vio_lm_lambda_min));
+  ar(CEREAL_NVP(config.vio_lm_lambda_max));
+
   ar(CEREAL_NVP(config.mapper_obs_std_dev));
   ar(CEREAL_NVP(config.mapper_obs_huber_thresh));
   ar(CEREAL_NVP(config.mapper_detection_num_points));
@@ -145,5 +157,9 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
   ar(CEREAL_NVP(config.mapper_bow_num_bits));
   ar(CEREAL_NVP(config.mapper_min_triangulation_dist));
   ar(CEREAL_NVP(config.mapper_no_factor_weights));
+
+  ar(CEREAL_NVP(config.mapper_use_lm));
+  ar(CEREAL_NVP(config.mapper_lm_lambda_min));
+  ar(CEREAL_NVP(config.mapper_lm_lambda_max));
 }
 }  // namespace cereal

src/vi_estimator/keypoint_vio.cpp

@@ -54,9 +54,9 @@ KeypointVioEstimator::KeypointVioEstimator(
       g(g),
       initialized(false),
       config(config),
-      lambda(1e-10),
+      lambda(config.vio_lm_lambda_min),
-      min_lambda(1e-32),
+      min_lambda(config.vio_lm_lambda_min),
-      max_lambda(100),
+      max_lambda(config.vio_lm_lambda_max),
       lambda_vee(2) {
   this->obs_std_dev = config.vio_obs_std_dev;
   this->huber_thresh = config.vio_obs_huber_thresh;
@@ -945,18 +945,107 @@ void KeypointVioEstimator::optimize() {
       double error_total =
           rld_error + imu_error + marg_prior_error + ba_error + bg_error;
 
-      std::cout << "[LINEARIZE] Error: " << error_total << " num points "
+      if (config.vio_debug)
-                << std::endl;
+        std::cout << "[LINEARIZE] Error: " << error_total << " num points "
+                  << std::endl;
 
       lopt.accum.setup_solver();
       Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
 
       bool converged = false;
-      bool step = false;
-      int max_iter = 10;
 
-      while (!step && max_iter > 0 && !converged) {
+      if (config.vio_use_lm) {  // Use Levenberg–Marquardt
-        Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
+        bool step = false;
+        int max_iter = 10;
+
+        while (!step && max_iter > 0 && !converged) {
+          Eigen::VectorXd Hdiag_lambda = Hdiag * lambda;
+          for (int i = 0; i < Hdiag_lambda.size(); i++)
+            Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
+
+          const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
+          double max_inc = inc.array().abs().maxCoeff();
+          if (max_inc < 1e-4) converged = true;
+
+          backup();
+
+          // apply increment to poses
+          for (auto& kv : frame_poses) {
+            int idx = aom.abs_order_map.at(kv.first).first;
+            kv.second.applyInc(-inc.segment<POSE_SIZE>(idx));
+          }
+
+          // apply increment to states
+          for (auto& kv : frame_states) {
+            int idx = aom.abs_order_map.at(kv.first).first;
+            kv.second.applyInc(-inc.segment<POSE_VEL_BIAS_SIZE>(idx));
+          }
+
+          // Update points
+          tbb::blocked_range<size_t> keys_range(0, rld_vec.size());
+          auto update_points_func = [&](const tbb::blocked_range<size_t>& r) {
+            for (size_t i = r.begin(); i != r.end(); ++i) {
+              const auto& rld = rld_vec[i];
+              updatePoints(aom, rld, inc);
+            }
+          };
+          tbb::parallel_for(keys_range, update_points_func);
+
+          double after_update_marg_prior_error = 0;
+          double after_update_vision_error = 0, after_update_imu_error = 0,
+                 after_bg_error = 0, after_ba_error = 0;
+
+          computeError(after_update_vision_error);
+          computeImuError(aom, after_update_imu_error, after_bg_error,
+                          after_ba_error, frame_states, imu_meas,
+                          gyro_bias_weight, accel_bias_weight, g);
+          computeMargPriorError(after_update_marg_prior_error);
+
+          double after_error_total =
+              after_update_vision_error + after_update_imu_error +
+              after_update_marg_prior_error + after_bg_error + after_ba_error;
+
+          double f_diff = (error_total - after_error_total);
+          double l_diff = 0.5 * inc.dot(inc * lambda + lopt.accum.getB());
+
+          // std::cout << "f_diff " << f_diff << " l_diff " << l_diff <<
+          // std::endl;
+
+          double step_quality = f_diff / l_diff;
+
+          if (step_quality < 0) {
+            if (config.vio_debug)
+              std::cout << "\t[REJECTED] lambda:" << lambda
+                        << " step_quality: " << step_quality
+                        << " max_inc: " << max_inc
+                        << " Error: " << after_error_total << std::endl;
+            lambda = std::min(max_lambda, lambda_vee * lambda);
+            lambda_vee *= 2;
+
+            restore();
+          } else {
+            if (config.vio_debug)
+              std::cout << "\t[ACCEPTED] lambda:" << lambda
+                        << " step_quality: " << step_quality
+                        << " max_inc: " << max_inc
+                        << " Error: " << after_error_total << std::endl;
+
+            lambda = std::max(
+                min_lambda,
+                lambda *
+                    std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
+            lambda_vee = 2;
+
+            step = true;
+          }
+          max_iter--;
+        }
+
+        if (config.vio_debug && converged) {
+          std::cout << "[CONVERGED]" << std::endl;
+        }
+      } else {  // Use Gauss-Newton
+        Eigen::VectorXd Hdiag_lambda = Hdiag * min_lambda;
         for (int i = 0; i < Hdiag_lambda.size(); i++)
           Hdiag_lambda[i] = std::max(Hdiag_lambda[i], min_lambda);
 
@@ -964,8 +1053,6 @@ void KeypointVioEstimator::optimize() {
         double max_inc = inc.array().abs().maxCoeff();
         if (max_inc < 1e-4) converged = true;
 
-        backup();
-
         // apply increment to poses
         for (auto& kv : frame_poses) {
           int idx = aom.abs_order_map.at(kv.first).first;
@@ -987,56 +1074,6 @@ void KeypointVioEstimator::optimize() {
           }
         };
         tbb::parallel_for(keys_range, update_points_func);
-
-        double after_update_marg_prior_error = 0;
-        double after_update_vision_error = 0, after_update_imu_error = 0,
-               after_bg_error = 0, after_ba_error = 0;
-
-        computeError(after_update_vision_error);
-        computeImuError(aom, after_update_imu_error, after_bg_error,
-                        after_ba_error, frame_states, imu_meas,
-                        gyro_bias_weight, accel_bias_weight, g);
-        computeMargPriorError(after_update_marg_prior_error);
-
-        double after_error_total =
-            after_update_vision_error + after_update_imu_error +
-            after_update_marg_prior_error + after_bg_error + after_ba_error;
-
-        double f_diff = (error_total - after_error_total);
-        double l_diff = 0.5 * inc.dot(inc * lambda + lopt.accum.getB());
-
-        std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
-
-        double step_quality = f_diff / l_diff;
-
-        if (step_quality < 0) {
-          std::cout << "\t[REJECTED] lambda:" << lambda
-                    << " step_quality: " << step_quality
-                    << " max_inc: " << max_inc
-                    << " Error: " << after_error_total << std::endl;
-          lambda = std::min(max_lambda, lambda_vee * lambda);
-          lambda_vee *= 2;
-
-          restore();
-        } else {
-          std::cout << "\t[ACCEPTED] lambda:" << lambda
-                    << " step_quality: " << step_quality
-                    << " max_inc: " << max_inc
-                    << " Error: " << after_error_total << std::endl;
-
-          lambda = std::max(
-              min_lambda,
-              lambda *
-                  std::max(1.0 / 3, 1 - std::pow(2 * step_quality - 1, 3.0)));
-          lambda_vee = 2;
-
-          step = true;
-        }
-        max_iter--;
-      }
-
-      if (converged) {
-        std::cout << "[CONVERGED]" << std::endl;
       }
 
       if (config.vio_debug) {
@@ -1099,7 +1136,7 @@ void KeypointVioEstimator::optimize() {
   if (config.vio_debug) {
     std::cout << "=================================" << std::endl;
   }
-}
+}  // namespace basalt
 
 void KeypointVioEstimator::computeProjections(
     std::vector<Eigen::vector<Eigen::Vector4d>>& data) const {