From 77142f0c2aae79466bbea69705a9109dec92cc1f Mon Sep 17 00:00:00 2001
From: Vladyslav Usenko <vlad.usenko@tum.de>
Date: Wed, 14 Aug 2019 10:57:48 +0200
Subject: [PATCH] small fixes LM

---
 src/vi_estimator/keypoint_vio.cpp | 19 ++++---------------
 src/vi_estimator/nfr_mapper.cpp   | 20 +++++---------------
 2 files changed, 9 insertions(+), 30 deletions(-)

diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp
index 3795149..6d88dc9 100644
--- a/src/vi_estimator/keypoint_vio.cpp
+++ b/src/vi_estimator/keypoint_vio.cpp
@@ -1014,18 +1014,11 @@ void KeypointVioEstimator::optimize() {
               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 (f_diff < 0) {
             if (config.vio_debug)
               std::cout << "\t[REJECTED] lambda:" << lambda
-                        << " step_quality: " << step_quality
-                        << " max_inc: " << max_inc
+                        << " f_diff: " << f_diff << " max_inc: " << max_inc
                         << " Error: " << after_error_total << std::endl;
             lambda = std::min(max_lambda, lambda_vee * lambda);
             lambda_vee *= 2;
@@ -1034,14 +1027,10 @@ void KeypointVioEstimator::optimize() {
           } else {
             if (config.vio_debug)
               std::cout << "\t[ACCEPTED] lambda:" << lambda
-                        << " step_quality: " << step_quality
-                        << " max_inc: " << max_inc
+                        << " f_diff: " << f_diff << " 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 = std::max(min_lambda, lambda / 3);
             lambda_vee = 2;
 
             step = true;
diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp
index b0c8c44..84b4344 100644
--- a/src/vi_estimator/nfr_mapper.cpp
+++ b/src/vi_estimator/nfr_mapper.cpp
@@ -292,7 +292,7 @@ void NfrMapper::optimize(int num_iterations) {
 
     bool converged = false;
 
-    if (config.vio_use_lm) {  // Use Levenberg–Marquardt
+    if (config.mapper_use_lm) {  // Use Levenberg–Marquardt
       bool step = false;
       int max_iter = 10;
 
@@ -336,15 +336,9 @@ void NfrMapper::optimize(int num_iterations) {
                                    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
+        if (f_diff < 0) {
+          std::cout << "\t[REJECTED] lambda:" << lambda << " f_diff: " << f_diff
                     << " max_inc: " << max_inc
                     << " vision_error: " << after_update_vision_error
                     << " rel_error: " << after_rel_error
@@ -355,18 +349,14 @@ void NfrMapper::optimize(int num_iterations) {
 
           restore();
         } else {
-          std::cout << "\t[ACCEPTED] lambda:" << lambda
-                    << " step_quality: " << step_quality
+          std::cout << "\t[ACCEPTED] lambda:" << lambda << " f_diff: " << f_diff
                     << " 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 = std::max(min_lambda, lambda / 3);
           lambda_vee = 2;
 
           step = true;