From b84f7ed22d221f5fd71fa382145bcd0cd7c692d8 Mon Sep 17 00:00:00 2001
From: Cyril Roussillon <cyril.roussillon@easymile.com>
Date: Tue, 9 Oct 2018 17:30:41 +0200
Subject: [PATCH] (#378) Allow zero dimension factors, as it was in gtsam 3.x

---
 gtsam/linear/Scatter.cpp                           | 3 ---
 gtsam/nonlinear/internal/LevenbergMarquardtState.h | 6 +++---
 2 files changed, 3 insertions(+), 6 deletions(-)

diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp
index db5c25442..5312da34b 100644
--- a/gtsam/linear/Scatter.cpp
+++ b/gtsam/linear/Scatter.cpp
@@ -70,9 +70,6 @@ Scatter::Scatter(const GaussianFactorGraph& gfg,
   iterator first = begin();
   if (ordering) first += ordering->size();
   if (first != end()) std::sort(first, end());
-
-  // Filter out keys with zero dimensions (if ordering had more keys)
-  erase(std::remove_if(begin(), end(), SlotEntry::Zero), end());
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/nonlinear/internal/LevenbergMarquardtState.h b/gtsam/nonlinear/internal/LevenbergMarquardtState.h
index 19a390c45..bd5465dda 100644
--- a/gtsam/nonlinear/internal/LevenbergMarquardtState.h
+++ b/gtsam/nonlinear/internal/LevenbergMarquardtState.h
@@ -113,9 +113,9 @@ struct LevenbergMarquardtState : public NonlinearOptimizerState {
   // Small cache of A|b|model indexed by dimension. Can save many mallocs.
   mutable std::vector<CachedModel> noiseModelCache;
   CachedModel* getCachedModel(size_t dim) const {
-    if (dim > noiseModelCache.size())
-      noiseModelCache.resize(dim);
-    CachedModel* item = &noiseModelCache[dim - 1];
+    if (dim >= noiseModelCache.size())
+      noiseModelCache.resize(dim+1);
+    CachedModel* item = &noiseModelCache[dim];
     if (!item->model)
       *item = CachedModel(dim, 1.0 / std::sqrt(lambda));
     return item;