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

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

diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index a56e02461..ce5f093dc 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -366,6 +366,12 @@ VectorValues HessianFactor::hessianDiagonal() const {
 /* ************************************************************************* */
 map<Key,Matrix> HessianFactor::hessianBlockDiagonal() const {
   map<Key,Matrix> blocks;
+  // Loop over all variables
+  for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
+    // Get the diagonal block, and insert it
+    Matrix B = info_(j, j).selfadjointView();
+    blocks.insert(make_pair(keys_[j],B));
+  }
   return blocks;
 }