diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 035bae319..ac9bd8812 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -68,13 +68,18 @@ void LevenbergMarquardtParams::print(const std::string& str) const { std::cout.flush(); } +/* ************************************************************************* */ +GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const { + return graph_.linearize(state_.values); +} + /* ************************************************************************* */ void LevenbergMarquardtOptimizer::iterate() { gttic(LM_iterate); // Linearize graph - GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values); + GaussianFactorGraph::shared_ptr linear = linearize(); // Pull out parameters we'll use const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity; diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 24e30922b..5e95382ad 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -177,6 +177,9 @@ protected: /** Internal function for computing a COLAMD ordering if no ordering is specified */ LevenbergMarquardtParams ensureHasOrdering(LevenbergMarquardtParams params, const NonlinearFactorGraph& graph) const; + + /** linearize, can be overwritten */ + virtual GaussianFactorGraph::shared_ptr linearize() const; }; }