From 2b52bad56950e4ae2966a503d9cba3c0e3a7a0e0 Mon Sep 17 00:00:00 2001
From: Luca <luca.carlone@gatech.edu>
Date: Wed, 23 Apr 2014 15:15:48 -0400
Subject: [PATCH] optimized point damping

---
 gtsam_unstable/slam/SmartFactorBase.h | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/gtsam_unstable/slam/SmartFactorBase.h b/gtsam_unstable/slam/SmartFactorBase.h
index 2852e9d83..79a56ea7f 100644
--- a/gtsam_unstable/slam/SmartFactorBase.h
+++ b/gtsam_unstable/slam/SmartFactorBase.h
@@ -302,15 +302,18 @@ public:
 
     // Point covariance inv(E'*E)
     Matrix3 EtE = E.transpose() * E;
-    Matrix3 DMatrix = eye(E.cols()); // damping matrix
 
     if (diagonalDamping) { // diagonal of the hessian
-      DMatrix(0, 0) = EtE(0, 0);
-      DMatrix(1, 1) = EtE(1, 1);
-      DMatrix(2, 2) = EtE(2, 2);
+      EtE(0, 0) += lambda * EtE(0, 0);
+      EtE(1, 1) += lambda * EtE(1, 1);
+      EtE(2, 2) += lambda * EtE(2, 2);
+    }else{
+      EtE(0, 0) += lambda;
+      EtE(1, 1) += lambda;
+      EtE(2, 2) += lambda;
     }
 
-    PointCov.noalias() = (EtE + lambda * DMatrix).inverse();
+    PointCov.noalias() = (EtE).inverse();
 
     return f;
   }