From 9186e656a5235df501758d4c037ff07fa897d12f Mon Sep 17 00:00:00 2001
From: Fan Jiang <prof.fan@foxmail.com>
Date: Tue, 2 Jun 2020 22:00:11 -0400
Subject: [PATCH] Zero initialize the allocated vector

---
 gtsam/linear/JacobianFactor.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index af2aa5f45..fcd517edd 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -556,11 +556,15 @@ void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const {
     auto result = d.emplace(j, nj);
 
     Vector& dj = result.first->second;
+
     for (size_t k = 0; k < nj; ++k) {
       Vector column_k = Ab_(pos).col(k);
       if (model_)
         model_->whitenInPlace(column_k);
-      dj(k) += dot(column_k, column_k);
+      if(!result.second)
+        dj(k) += dot(column_k, column_k);
+      else
+        dj(k) = dot(column_k, column_k);
     }
   }
 }