From b669e3aa3fe29f09d5f2db253045a210b8c15f23 Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 2 Jun 2020 16:18:18 -0400
Subject: [PATCH] fix checks

---
 gtsam/slam/RegularImplicitSchurFactor.h | 19 ++++++++++++++++---
 1 file changed, 16 insertions(+), 3 deletions(-)

diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h
index 07b749811..656e75723 100644
--- a/gtsam/slam/RegularImplicitSchurFactor.h
+++ b/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];
 
@@ -153,7 +161,7 @@ public:
       // D x 3 = (D x ZDim) * (ZDim x 3)
       const MatrixZD& Fj = FBlocks_[k];
       Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
-          * E_.block<ZDim, 3>(ZDim * k, 0);
+                                        * E_.block<ZDim, 3>(ZDim * k, 0);
 
       Eigen::Matrix<double, D, 1> dj;
       for (int k = 0; k < D; ++k) { // for each diagonal element of the camera hessian
@@ -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;
   }
 
   /**