diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index 18fa44e8b..e62af9a05 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -367,13 +367,12 @@ void HessianFactor::hessianDiagonal(double* d) const {
   typedef Eigen::Map<DVector> DMap;
 
   // Loop over all variables in the factor
-    for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
-      Key j = keys_[pos];
-      // Get the diagonal block, and insert its diagonal
-      Matrix B = info_(pos, pos).selfadjointView();
-      DVector dj =  B.diagonal();
-      DMap(d + 9 * j) += dj;
-    }
+  for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
+    Key j = keys_[pos];
+    // Get the diagonal block, and insert its diagonal
+    const Matrix& B = info_(pos, pos).selfadjointView();
+    DMap(d + 9 * j) += B.diagonal();
+  }
 }
 
 /* ************************************************************************* */