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),
-      min_lambda(1e-32),
-      max_lambda(1000),
+      lambda(config.mapper_lm_lambda_min),
+      min_lambda(config.mapper_lm_lambda_min),
+      max_lambda(config.mapper_lm_lambda_max),
       lambda_vee(2) {
   this->calib = calib;
   this->obs_std_dev = config.mapper_obs_std_dev;
@@ -291,6 +291,8 @@ void NfrMapper::optimize(int num_iterations) {
     const Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
 
     bool converged = false;
+
+    if (config.vio_use_lm) {  // Use Levenberg–Marquardt
       bool step = false;
       int max_iter = 10;
 
@@ -330,8 +332,8 @@ void NfrMapper::optimize(int num_iterations) {
         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 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());
@@ -376,6 +378,32 @@ void NfrMapper::optimize(int num_iterations) {
           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);
+
+      const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag_lambda);
+      double max_inc = inc.array().abs().maxCoeff();
+      if (max_inc < 1e-5) converged = true;
+
+      // 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);
+    }
 
     auto t2 = std::chrono::high_resolution_clock::now();
     auto elapsed =