diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h
index a80b4dfd4..bd9e7d232 100644
--- a/gtsam/linear/GaussianFactor.h
+++ b/gtsam/linear/GaussianFactor.h
@@ -102,6 +102,9 @@ namespace gtsam {
     /// Return the diagonal of the Hessian for this factor
     virtual VectorValues hessianDiagonal() const = 0;
 
+    /// Add the current diagonal to a VectorValues instance
+    virtual void hessianDiagonalAdd(VectorValues& d) const = 0;
+
     /// Raw memory access version of hessianDiagonal
     virtual void hessianDiagonal(double* d) const = 0;
 
diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 8dc3600c6..69890c32d 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -255,8 +255,7 @@ namespace gtsam {
     VectorValues d;
     for (const sharedFactor& factor : *this) {
       if(factor){
-        VectorValues di = factor->hessianDiagonal();
-        d.addInPlace_(di);
+        factor->hessianDiagonalAdd(d);
       }
     }
     return d;
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index cc82e2fa0..06d2ece7b 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -310,6 +310,17 @@ VectorValues HessianFactor::hessianDiagonal() const {
   return d;
 }
 
+/* ************************************************************************* */
+void HessianFactor::hessianDiagonalAdd(VectorValues &d) const {
+  for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
+    if(d.exists(keys_[j])) {
+      d.at(keys_[j]) += info_.diagonal(j);
+    } else {
+      d.emplace(keys_[j], info_.diagonal(j));
+    }
+  }
+}
+
 /* ************************************************************************* */
 // Raw memory access version should be called in Regular Factors only currently
 void HessianFactor::hessianDiagonal(double* d) const {
diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h
index d1c362a54..0ee1d359b 100644
--- a/gtsam/linear/HessianFactor.h
+++ b/gtsam/linear/HessianFactor.h
@@ -296,6 +296,9 @@ namespace gtsam {
     /// Return the diagonal of the Hessian for this factor
     VectorValues hessianDiagonal() const override;
 
+    /// Add the current diagonal to a VectorValues instance
+    void hessianDiagonalAdd(VectorValues& d) const override;
+
     /// Raw memory access version of hessianDiagonal
     void hessianDiagonal(double* d) const override;
 
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 839ffe69d..2e243c033 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -544,6 +544,12 @@ Matrix JacobianFactor::information() const {
 /* ************************************************************************* */
 VectorValues JacobianFactor::hessianDiagonal() const {
   VectorValues d;
+  hessianDiagonalAdd(d);
+  return d;
+}
+
+/* ************************************************************************* */
+void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const {
   for (size_t pos = 0; pos < size(); ++pos) {
     Key j = keys_[pos];
     size_t nj = Ab_(pos).cols();
@@ -554,9 +560,12 @@ VectorValues JacobianFactor::hessianDiagonal() const {
         model_->whitenInPlace(column_k);
       dj(k) = dot(column_k, column_k);
     }
-    d.emplace(j, dj);
+    if(d.exists(j)) {
+      d.at(j) += dj;
+    } else {
+      d.emplace(j, dj);
+    }
   }
-  return d;
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h
index 9b9d02726..ac5ccd0d2 100644
--- a/gtsam/linear/JacobianFactor.h
+++ b/gtsam/linear/JacobianFactor.h
@@ -218,6 +218,9 @@ namespace gtsam {
     /// Return the diagonal of the Hessian for this factor
     VectorValues hessianDiagonal() const override;
 
+    /// Add the current diagonal to a VectorValues instance
+    void hessianDiagonalAdd(VectorValues& d) const override;
+
     /// Raw memory access version of hessianDiagonal
     void hessianDiagonal(double* d) const override;