From c31d48dba1d139ab307a1fd196e25e22c06275f2 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 16 Feb 2014 11:57:51 -0500
Subject: [PATCH] New flag to govern diagonal damping, off by default

---
 gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp |  7 +++++--
 gtsam/nonlinear/LevenbergMarquardtOptimizer.h   | 12 ++++++++++--
 2 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index 88fb9ba0b..50b3e43cd 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -113,13 +113,16 @@ void LevenbergMarquardtOptimizer::iterate() {
       double sigma = 1.0 / std::sqrt(state_.lambda);
       dampedSystem.reserve(dampedSystem.size() + state_.values.size());
       // for each of the variables, add a prior
-      VectorValues diagHessian = linear->hessianDiagonal();
+      VectorValues hessianDiagonal;
+      if (params_.diagonalDamping)
+        hessianDiagonal = linear->hessianDiagonal();
       BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
 
         size_t dim = key_value.value.dim();
         Matrix A = Matrix::Identity(dim, dim);
         //Replace the identity matrix with diagonal of Hessian
-        A.diagonal()=diagHessian.at(key_value.key);
+        if (params_.diagonalDamping)
+          A.diagonal() = hessianDiagonal.at(key_value.key);
         Vector b = Vector::Zero(dim);
         SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
         dampedSystem += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);
diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
index a3c9b14bd..087ae2b6d 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h
@@ -52,10 +52,12 @@ public:
   bool disableInnerIterations; ///< If enabled inner iterations on the linearized system are performed
   double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration
   std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda]
+  bool diagonalDamping; ///< if true, use diagonal of Hessian
 
   LevenbergMarquardtParams() :
-      lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound(0.0),
-      verbosityLM(SILENT), disableInnerIterations(false), minModelFidelity(1e-3) {
+      lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound(
+          0.0), verbosityLM(SILENT), disableInnerIterations(false), minModelFidelity(
+          1e-3), diagonalDamping(false) {
   }
   virtual ~LevenbergMarquardtParams() {
   }
@@ -80,6 +82,9 @@ public:
   inline std::string getLogFile() const {
     return logFile;
   }
+  inline bool getDiagonalDamping() const {
+    return diagonalDamping;
+  }
 
   inline void setlambdaInitial(double value) {
     lambdaInitial = value;
@@ -99,6 +104,9 @@ public:
   inline void setLogFile(const std::string &s) {
     logFile = s;
   }
+  inline void setDiagonalDamping(bool flag) {
+    diagonalDamping = flag;
+  }
 };
 
 /**