From eedbbc9aac4fae3444bf59440437e413159f5352 Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Wed, 16 Oct 2013 23:17:27 +0000 Subject: [PATCH] added number of inner iterations counter in LM --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 3 ++- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 7 ++++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index dc5a9f2a9..7d59197c3 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -86,7 +86,8 @@ void LevenbergMarquardtOptimizer::iterate() { while(true) { if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "trying lambda = " << state_.lambda << endl; - + ++state_.totalNumberInnerIterations; + cout << "state_.totalNumberInnerIterations = " << state_.totalNumberInnerIterations << endl; // Add prior-factors // TODO: replace this dampening with a backsubstitution approach gttic(damp); diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 46db598d8..c58598606 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -74,6 +74,7 @@ class GTSAM_EXPORT LevenbergMarquardtState : public NonlinearOptimizerState { public: double lambda; + int totalNumberInnerIterations; // The total number of inner iterations in the optimization (for each iteration, LM may try multiple iterations with different lambdas) LevenbergMarquardtState() {} @@ -81,7 +82,7 @@ public: protected: LevenbergMarquardtState(const NonlinearFactorGraph& graph, const Values& initialValues, const LevenbergMarquardtParams& params, unsigned int iterations = 0) : - NonlinearOptimizerState(graph, initialValues, iterations), lambda(params.lambdaInitial) {} + NonlinearOptimizerState(graph, initialValues, iterations), lambda(params.lambdaInitial), totalNumberInnerIterations(0) {} friend class LevenbergMarquardtOptimizer; }; @@ -95,6 +96,7 @@ protected: LevenbergMarquardtParams params_; ///< LM parameters LevenbergMarquardtState state_; ///< optimization state + public: typedef boost::shared_ptr shared_ptr; @@ -129,6 +131,9 @@ public: /// Access the current damping value double lambda() const { return state_.lambda; } + /// Access the current number of inner iterations + int getInnerIterations() const { return state_.totalNumberInnerIterations; } + /// print virtual void print(const std::string& str = "") const { std::cout << str << "LevenbergMarquardtOptimizer" << std::endl;