diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index b56270a55..d6e4663e3 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/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; diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index ae5730b68..692310f26 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/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. diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index e6c24f050..187abece6 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/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; diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 47952f9e4..211830ed8 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/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; /// @}