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