From a2829fffad0ca30a5fe32d00a3f78944e5156018 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Fri, 14 Feb 2014 13:31:40 -0500
Subject: [PATCH] Working and tested implementation of hessianBlockDiagonal

---
 gtsam/linear/JacobianFactor.cpp | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 66f7a5f05..85eb6880f 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -458,6 +458,12 @@ namespace gtsam {
   /* ************************************************************************* */
   map<Key,Matrix> JacobianFactor::hessianBlockDiagonal() const {
     map<Key,Matrix> blocks;
+    for(size_t pos=0; pos<size(); ++pos)
+    {
+      Key j = keys_[pos];
+      Matrix Aj = model_->Whiten(Ab_(pos));
+      blocks.insert(make_pair(j,Aj.transpose()*Aj));
+    }
     return blocks;
   }