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;
 }