update mapper

src/vi_estimator/nfr_mapper.cpp

@@ -45,9 +45,9 @@ namespace basalt {
 
 NfrMapper::NfrMapper(const Calibration<double>& calib, const VioConfig& config)
     : config(config),
-      lambda(1e-10),
+      lambda(config.mapper_lm_lambda_min),
-      min_lambda(1e-32),
+      min_lambda(config.mapper_lm_lambda_min),
-      max_lambda(1000),
+      max_lambda(config.mapper_lm_lambda_max),
       lambda_vee(2) {
   this->calib = calib;
   this->obs_std_dev = config.mapper_obs_std_dev;
@@ -291,11 +291,95 @@ void NfrMapper::optimize(int num_iterations) {
     const 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-5) converged = true;
+
+        backup();
+
+        // apply increment to poses
+        for (auto& kv : frame_poses) {
+          int idx = aom.abs_order_map.at(kv.first).first;
+          BASALT_ASSERT(!kv.second.isLinearized());
+          kv.second.applyInc(-inc.segment<POSE_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_vision_error = 0;
+        double after_rel_error = 0;
+        double after_roll_pitch_error = 0;
+
+        computeError(after_update_vision_error);
+        computeRelPose(after_rel_error);
+        computeRollPitch(after_roll_pitch_error);
+
+        double after_error_total = after_update_vision_error + after_rel_error +
+                                   after_roll_pitch_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
+                    << " vision_error: " << after_update_vision_error
+                    << " rel_error: " << after_rel_error
+                    << " roll_pitch_error: " << after_roll_pitch_error
+                    << " total: " << 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
+                    << " vision_error: " << after_update_vision_error
+                    << " rel_error: " << after_rel_error
+                    << " roll_pitch_error: " << after_roll_pitch_error
+                    << " total: " << 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 (after_error_total > error_total) {
+          std::cout << "increased error after update!!!" << 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);
 
@@ -303,8 +387,6 @@ void NfrMapper::optimize(int num_iterations) {
       double max_inc = inc.array().abs().maxCoeff();
       if (max_inc < 1e-5) converged = true;
 
-      backup();
-
       // apply increment to poses
       for (auto& kv : frame_poses) {
         int idx = aom.abs_order_map.at(kv.first).first;
@@ -321,60 +403,6 @@ void NfrMapper::optimize(int num_iterations) {
         }
       };
       tbb::parallel_for(keys_range, update_points_func);
-
-      double after_update_vision_error = 0;
-      double after_rel_error = 0;
-      double after_roll_pitch_error = 0;
-
-      computeError(after_update_vision_error);
-      computeRelPose(after_rel_error);
-      computeRollPitch(after_roll_pitch_error);
-
-      double after_error_total =
-          after_update_vision_error + after_rel_error + after_roll_pitch_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
-                  << " vision_error: " << after_update_vision_error
-                  << " rel_error: " << after_rel_error
-                  << " roll_pitch_error: " << after_roll_pitch_error
-                  << " total: " << 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
-                  << " vision_error: " << after_update_vision_error
-                  << " rel_error: " << after_rel_error
-                  << " roll_pitch_error: " << after_roll_pitch_error
-                  << " total: " << 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 (after_error_total > error_total) {
-        std::cout << "increased error after update!!!" << std::endl;
-      }
     }
 
     auto t2 = std::chrono::high_resolution_clock::now();