Cache A and b in addition to noise model for damped system

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -189,20 +189,25 @@ 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);
 
       // 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;
+        NoiseCacheItem item;
+        item.A = Matrix::Identity(dim, dim);
+        item.b = Vector::Zero(dim);
+        item.model =  noiseModel::Isotropic::Sigma(dim, sigma);
+        noises[dim] = item;
+        damped += boost::make_shared<JacobianFactor>(key_value.key, item.A, item.b, item.model);
+
+      } else {
+        const NoiseCacheItem& item = it->second;
+        damped += boost::make_shared<JacobianFactor>(key_value.key, item.A, item.b, item.model);
       }
-      damped += boost::make_shared<JacobianFactor>(key_value.key, A, b, model);
     }
   }
   gttoc(damp);

gtsam/nonlinear/LevenbergMarquardtOptimizer.h

@@ -272,6 +272,17 @@ public:
   GaussianFactorGraph::shared_ptr buildDampedSystem(const GaussianFactorGraph& linear);
   friend class ::NonlinearOptimizerMoreOptimizationTest;
 
+  /** Small struct to cache objects needed for damping.
+   * This is used in buildDampedSystem  */
+  struct NoiseCacheItem {
+    Matrix A;
+    Vector b;
+    SharedDiagonal model;
+  };
+
+  /// Noise model Cache
+  typedef std::map<size_t, NoiseCacheItem> NoiseMap;
+
   void writeLogFile(double currentError);
 
   /// @}