From b0618b88942d9d632df438b6b995b437c0b2ad0a Mon Sep 17 00:00:00 2001
From: hchiu <hchiu@hchiu-VirtualBox2.(none)>
Date: Sat, 15 Feb 2014 20:46:28 -0500
Subject: [PATCH] Replace the identity matrix with diagonal of Hessian.

---
 gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index 0b496d2a4..88fb9ba0b 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -118,6 +118,8 @@ void LevenbergMarquardtOptimizer::iterate() {
 
         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);
         Vector b = Vector::Zero(dim);
         SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
         dampedSystem += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);