From 859e4cb37af125b9351db22dc86837077d5c62f9 Mon Sep 17 00:00:00 2001
From: lcarlone <lcarlone@mit.edu>
Date: Wed, 12 Jan 2022 21:31:22 -0500
Subject: [PATCH] thresholded mu to avoid case mu = 0 in TLS. improved
 verbosity handling

---
 gtsam/nonlinear/GncOptimizer.h | 25 +++++++++++++++++++------
 gtsam/nonlinear/GncParams.h    |  2 ++
 2 files changed, 21 insertions(+), 6 deletions(-)

diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h
index 3025d2468..6c8519aac 100644
--- a/gtsam/nonlinear/GncOptimizer.h
+++ b/gtsam/nonlinear/GncOptimizer.h
@@ -206,9 +206,11 @@ class GTSAM_EXPORT GncOptimizer {
         std::cout << "GNC Optimizer stopped because all measurements are already known to be inliers or outliers"
                   << std::endl;
       }
+      if (params_.verbosity >= GncParameters::Verbosity::MU) {
+        std::cout << "mu: " << mu << std::endl;
+      }
       if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
         result.print("result\n");
-        std::cout << "mu: " << mu << std::endl;
       }
       return result;
     }
@@ -217,12 +219,16 @@ class GTSAM_EXPORT GncOptimizer {
     for (iter = 0; iter < params_.maxIterations; iter++) {
 
       // display info
-      if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
+      if (params_.verbosity >= GncParameters::Verbosity::MU) {
         std::cout << "iter: " << iter << std::endl;
-        result.print("result\n");
         std::cout << "mu: " << mu << std::endl;
+      }
+      if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) {
         std::cout << "weights: " << weights_ << std::endl;
       }
+      if (params_.verbosity >= GncParameters::Verbosity::VALUES) {
+        result.print("result\n");
+      }
       // weights update
       weights_ = calculateWeights(result, mu);
 
@@ -253,10 +259,12 @@ class GTSAM_EXPORT GncOptimizer {
     if (params_.verbosity >= GncParameters::Verbosity::SUMMARY) {
       std::cout << "final iterations: " << iter << std::endl;
       std::cout << "final mu: " << mu << std::endl;
-      std::cout << "final weights: " << weights_ << std::endl;
       std::cout << "previous cost: " << prev_cost << std::endl;
       std::cout << "current cost: " << cost << std::endl;
     }
+    if (params_.verbosity >= GncParameters::Verbosity::WEIGHTS) {
+      std::cout << "final weights: " << weights_ << std::endl;
+    }
     return result;
   }
 
@@ -291,6 +299,9 @@ class GTSAM_EXPORT GncOptimizer {
                 std::min(mu_init, barcSq_[k] / (2 * rk - barcSq_[k]) ) : mu_init;
           }
         }
+        if (mu_init >= 0 && mu_init < 1e-6)
+          mu_init = 1e-6; // if mu ~ 0 (but positive), that means we have measurements with large errors,
+        // i.e., rk > barcSq_[k] and rk very large, hence we threshold to 1e-6 to avoid mu = 0
         return mu_init > 0 && !std::isinf(mu_init) ? mu_init : -1; // if mu <= 0 or mu = inf, return -1,
         // which leads to termination of the main gnc loop. In this case, all residuals are already below the threshold
         // and there is no need to robustify (TLS = least squares)
@@ -338,8 +349,10 @@ class GTSAM_EXPORT GncOptimizer {
   bool checkCostConvergence(const double cost, const double prev_cost) const {
     bool costConverged = std::fabs(cost - prev_cost) / std::max(prev_cost, 1e-7)
         < params_.relativeCostTol;
-    if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY)
-      std::cout << "checkCostConvergence = true " << std::endl;
+    if (costConverged && params_.verbosity >= GncParameters::Verbosity::SUMMARY){
+      std::cout << "checkCostConvergence = true (prev. cost = " << prev_cost
+                << ", curr. cost = " << cost << ")" << std::endl;
+    }
     return costConverged;
   }
 
diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h
index 086f08acc..1f324ae38 100644
--- a/gtsam/nonlinear/GncParams.h
+++ b/gtsam/nonlinear/GncParams.h
@@ -48,6 +48,8 @@ class GTSAM_EXPORT GncParams {
   enum Verbosity {
     SILENT = 0,
     SUMMARY,
+    MU,
+    WEIGHTS,
     VALUES
   };