fix checks

gtsam/slam/RegularImplicitSchurFactor.h

@@ -146,6 +146,14 @@ public:
     // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
     VectorValues d;
 
+    hessianDiagonalAdd(d);
+    
+    return d;
+  }
+
+  /// Return the diagonal of the Hessian for this factor
+  virtual void hessianDiagonalAdd(VectorValues &d) const override {
+    // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F);
     for (size_t k = 0; k < size(); ++k) { // for each camera
       Key j = keys_[k];
 
@@ -163,9 +171,14 @@ public:
         // (1 x 1) = (1 x 3) * (3 * 3) * (3 x 1)
         dj(k) -= FtE.row(k) * PointCovariance_ * FtE.row(k).transpose();
       }
-      d.insert(j, dj);
+
+      auto item = d.find(j);
+      if(item != d.end()) {
+        item->second += dj;
+      } else {
+        d.emplace(j, dj);
+      }
     }
-    return d;
   }
 
   /**