Added LM to mapping

include/basalt/vi_estimator/nfr_mapper.h

@@ -205,5 +205,7 @@ class NfrMapper : public BundleAdjustmentBase {
   std::shared_ptr<HashBow<256>> hash_bow_database;
 
   VioConfig config;
+
+  double lambda, min_lambda, max_lambda, lambda_vee;
 };
 }  // namespace basalt

src/vi_estimator/nfr_mapper.cpp

@@ -44,7 +44,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace basalt {
 
 NfrMapper::NfrMapper(const Calibration<double>& calib, const VioConfig& config)
-    : config(config) {
+    : config(config),
+      lambda(1e-10),
+      min_lambda(1e-32),
+      max_lambda(100),
+      lambda_vee(2) {
   this->calib = calib;
   this->obs_std_dev = config.mapper_obs_std_dev;
   this->huber_thresh = config.mapper_obs_huber_thresh;
@@ -274,7 +278,7 @@ void NfrMapper::optimize(int num_iterations) {
 
     double error_total = rld_error + lopt.rel_error + lopt.roll_pitch_error;
 
-    std::cout << "iter " << iter
+    std::cout << "[LINEARIZE] iter " << iter
               << " before_update_error: vision: " << rld_error
               << " rel_error: " << lopt.rel_error
               << " roll_pitch_error: " << lopt.roll_pitch_error
@@ -285,9 +289,21 @@ void NfrMapper::optimize(int num_iterations) {
 
     lopt.accum.setup_solver();
     Eigen::VectorXd Hdiag = lopt.accum.Hdiagonal();
-    Hdiag.setConstant(Hdiag.size(), 1e-6);
 
-    const Eigen::VectorXd inc = lopt.accum.solve(&Hdiag);
+    bool converged = false;
+    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) {
@@ -317,28 +333,59 @@ void NfrMapper::optimize(int num_iterations) {
       double after_error_total =
           after_update_vision_error + after_rel_error + after_roll_pitch_error;
 
-    double error_diff = error_total - after_error_total;
+      double f_diff = (error_total - after_error_total);
+      double l_diff = 0.5 * inc.dot(inc * lambda + lopt.accum.getB());
 
-    auto t2 = std::chrono::high_resolution_clock::now();
+      std::cout << "f_diff " << f_diff << " l_diff " << l_diff << std::endl;
 
-    auto elapsed =
+      double step_quality = f_diff / l_diff;
-        std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
 
-    std::cout << "iter " << iter
+      if (step_quality < 0) {
-              << "  after_update_error: vision: " << after_update_vision_error
+        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 << " max_inc "
+                  << " total: " << after_error_total << std::endl;
-              << inc.array().abs().maxCoeff() << " error_diff " << error_diff
+        lambda = std::min(max_lambda, lambda_vee * lambda);
-              << " time : " << elapsed.count() << "(us),  num_states "
+        lambda_vee *= 2;
-              << frame_states.size() << " num_poses " << frame_poses.size()
+
-              << std::endl;
+        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;
       }
+    }
 
-    if (inc.array().abs().maxCoeff() < 1e-4) break;
+    auto t2 = std::chrono::high_resolution_clock::now();
+    auto elapsed =
+        std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
+
+    std::cout << "iter " << iter << " time : " << elapsed.count()
+              << "(us),  num_states " << frame_states.size() << " num_poses "
+              << frame_poses.size() << std::endl;
+
+    if (converged) break;
 
     // std::cerr << "LT\n" << LT << std::endl;
     // std::cerr << "z_p\n" << z_p.transpose() << std::endl;
@@ -508,7 +555,8 @@ void NfrMapper::match_all() {
 
       // std::cout << "Closest frames for " << tcid << ": ";
       for (const auto& otcid_score : results) {
-        // std::cout << otcid_score.first << "(" << otcid_score.second << ") ";
+        // std::cout << otcid_score.first << "(" << otcid_score.second << ")
+        // ";
         if (otcid_score.first.frame_id != tcid.frame_id &&
             otcid_score.second > config.mapper_frames_to_match_threshold) {
           match_pair m;