Switched to in-place update of the diagonal Hessian

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;
 

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;

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 {

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;
 

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

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;