Fix LM behavior when GaussianFactorGraph is subclassed. Use clone method instead of assignment to local GaussianFactorGraph when creating damped system.

gtsam/linear/GaussianFactorGraph.cpp

@@ -75,6 +75,13 @@ namespace gtsam {
       return dims_accumulated;
     }
 
+  /* ************************************************************************* */
+  GaussianFactorGraph::shared_ptr GaussianFactorGraph::cloneToPtr() const {
+    gtsam::GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
+    *result = *this;
+    return result;
+  }
+
   /* ************************************************************************* */
   GaussianFactorGraph GaussianFactorGraph::clone() const {
     GaussianFactorGraph result;

gtsam/linear/GaussianFactorGraph.h

@@ -160,7 +160,13 @@ namespace gtsam {
      * Cloning preserves null factors so indices for the original graph are still
      * valid for the cloned graph.
      */
-    GaussianFactorGraph clone() const;
+    virtual GaussianFactorGraph clone() const;
+
+    /**
+     * CloneToPtr() performs a simple assignment to a new graph and returns it.
+     * There is no preservation of null factors!
+     */
+    virtual GaussianFactorGraph::shared_ptr cloneToPtr() const;
 
     /**
      * Returns the negation of all factors in this graph - corresponds to antifactors.

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -138,7 +138,7 @@ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) {
 }
 
 /* ************************************************************************* */
-GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
+GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
     const GaussianFactorGraph& linear) {
 
   gttic(damp);
@@ -159,7 +159,8 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
 
   // for each of the variables, add a prior
   double sigma = 1.0 / std::sqrt(state_.lambda);
-  GaussianFactorGraph damped = linear;
+  GaussianFactorGraph::shared_ptr dampedPtr = linear.cloneToPtr();
+  GaussianFactorGraph &damped = (*dampedPtr);
   damped.reserve(damped.size() + state_.values.size());
   if (params_.diagonalDamping) {
     BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) {
@@ -188,7 +189,7 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
     }
   }
   gttoc(damp);
-  return damped;
+  return dampedPtr;
 }
 
 /* ************************************************************************* */
@@ -212,7 +213,8 @@ void LevenbergMarquardtOptimizer::iterate() {
       cout << "trying lambda = " << state_.lambda << endl;
 
     // Build damped system for this lambda (adds prior factors that make it like gradient descent)
-    GaussianFactorGraph dampedSystem = buildDampedSystem(*linear);
+    GaussianFactorGraph::shared_ptr dampedSystemPtr = buildDampedSystem(*linear);
+    GaussianFactorGraph &dampedSystem = (*dampedSystemPtr);
 
     // Try solving
     double modelFidelity = 0.0;

gtsam/nonlinear/LevenbergMarquardtOptimizer.h

@@ -255,7 +255,7 @@ public:
   }
 
   /** Build a damped system for a specific lambda */
-  GaussianFactorGraph buildDampedSystem(const GaussianFactorGraph& linear);
+  GaussianFactorGraph::shared_ptr buildDampedSystem(const GaussianFactorGraph& linear);
   friend class ::NonlinearOptimizerMoreOptimizationTest;
 
   /// @}