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),
-      min_lambda(1e-32),
-      max_lambda(100),
+      lambda(config.vio_lm_lambda_min),
+      min_lambda(config.vio_lm_lambda_min),
+      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,6 +945,7 @@ void KeypointVioEstimator::optimize() {
       double error_total =
           rld_error + imu_error + marg_prior_error + ba_error + bg_error;
 
+      if (config.vio_debug)
         std::cout << "[LINEARIZE] Error: " << error_total << " num points "
                   << std::endl;
 
@@ -952,6 +953,8 @@ void KeypointVioEstimator::optimize() {
       Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
 
       bool converged = false;
+
+      if (config.vio_use_lm) {  // Use Levenberg–Marquardt
         bool step = false;
         int max_iter = 10;
 
@@ -1005,11 +1008,13 @@ void KeypointVioEstimator::optimize() {
           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;
+          // 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
@@ -1019,6 +1024,7 @@ void KeypointVioEstimator::optimize() {
 
             restore();
           } else {
+            if (config.vio_debug)
               std::cout << "\t[ACCEPTED] lambda:" << lambda
                         << " step_quality: " << step_quality
                         << " max_inc: " << max_inc
@@ -1035,9 +1041,40 @@ void KeypointVioEstimator::optimize() {
           max_iter--;
         }
 
-      if (converged) {
+        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);
+
+        const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
+        double max_inc = inc.array().abs().maxCoeff();
+        if (max_inc < 1e-4) converged = true;
+
+        // 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);
+      }
 
       if (config.vio_debug) {
         double after_update_marg_prior_error = 0;
@@ -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 {