From 766a9622e89a24bf83c1996a60ea839e0e6d6c70 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 11 Mar 2015 06:33:16 -0700
Subject: [PATCH] Fixed Hessian by whitening...

---
 gtsam/slam/SmartProjectionFactor.h | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h
index bf6d20c45..067d30329 100644
--- a/gtsam/slam/SmartProjectionFactor.h
+++ b/gtsam/slam/SmartProjectionFactor.h
@@ -204,11 +204,15 @@ public:
           Gs, gs, 0.0);
     }
 
+    // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
     std::vector<typename Base::MatrixZD> Fblocks;
     Matrix E;
     Vector b;
     computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
 
+    // Whiten using noise model
+    Base::whitenJacobians(Fblocks, E, b);
+
     // build augmented hessian
     SymmetricBlockMatrix augmentedHessian = //
         Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping);