From f804fd9bd1465591f4805f3a26ab36924e598c46 Mon Sep 17 00:00:00 2001
From: hchiu <hchiu@hchiu-VirtualBox2.(none)>
Date: Sun, 16 Feb 2014 15:50:26 -0500
Subject: [PATCH] Traced Ceres and took sqrt of diagonal term as they did.

---
 gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index 50b3e43cd..9bc38822e 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -114,15 +114,21 @@ void LevenbergMarquardtOptimizer::iterate() {
       dampedSystem.reserve(dampedSystem.size() + state_.values.size());
       // for each of the variables, add a prior
       VectorValues hessianDiagonal;
-      if (params_.diagonalDamping)
+      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
-        if (params_.diagonalDamping)
-          A.diagonal() = hessianDiagonal.at(key_value.key);
+        if (params_.diagonalDamping) {
+           A.diagonal() = hessianDiagonal.at(key_value.key)*state_.lambda;
+           for (int aa=0; aa<dim; aa++)
+           {
+        	   A(aa,aa)=sqrt(A(aa,aa));
+           }
+         }
         Vector b = Vector::Zero(dim);
         SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
         dampedSystem += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);