From 1949fc251183e66f82a70a7c5b3e9d2df67b7b0a Mon Sep 17 00:00:00 2001
From: cbeall3 <cbeall3@gatech.edu>
Date: Tue, 20 Oct 2015 16:25:41 -0400
Subject: [PATCH] Cache noise models in LM damping instead of constructing from
 scratch for each variable. Time savings of 5.6%!

---
 gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
index cacb0a1ff..960ba59dd 100644
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
@@ -189,11 +189,19 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
     }
   } else {
     // Straightforward damping:
+    typedef map<size_t, SharedDiagonal> NoiseMap; // Cache noise models
+    NoiseMap noises;
     BOOST_FOREACH(const Values::KeyValuePair& key_value, state_.values) {
       size_t dim = key_value.value.dim();
       Matrix A = Matrix::Identity(dim, dim);
       Vector b = Vector::Zero(dim);
-      SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
+
+      // Check if noise model of appropriate size already exists, else create it and cache it!
+      NoiseMap::iterator it = noises.find(dim);
+      SharedDiagonal model = it == noises.end() ? noiseModel::Isotropic::Sigma(dim, sigma) : it->second;
+      if(it == noises.end()) {
+        noises[dim] = model;
+      }
       damped += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);
     }
   }