Use faster version of in-place add

borglab · Jun 2, 2020 · 15fda68 · dellaert · Jun 2, 2020 · ProfFan
1 parent 50ffeb7
commit 15fda68
Showing 1 changed file with 18 additions and 9 deletions.
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
@@ -552,18 +552,27 @@ VectorValues JacobianFactor::hessianDiagonal() const {
 void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const {
   for (size_t pos = 0; pos < size(); ++pos) {
     Key j = keys_[pos];
-    size_t nj = Ab_(pos).cols();
-    Vector dj(nj);
-    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);
-    }
     auto item = d.find(j);
+
     if(item != d.end()) {
-      item->second += dj;
+      size_t nj = Ab_(pos).cols();
+      Vector& dj = item->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);
+      }
     } else {
+      size_t nj = Ab_(pos).cols();
+      Vector dj(nj);
+      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);
+      }
+
       d.emplace(j, dj);
     }
   }