diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index c959473a8..d83800cca 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -74,6 +74,16 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::linearize() const { return graph_.linearize(state_.values); } +/* ************************************************************************* */ +void LevenbergMarquardtOptimizer::increaseLambda(){ + state_.lambda *= params_.lambdaFactor; +} + +/* ************************************************************************* */ +void LevenbergMarquardtOptimizer::decreaseLambda(){ + state_.lambda /= params_.lambdaFactor; +} + /* ************************************************************************* */ void LevenbergMarquardtOptimizer::iterate() { @@ -146,7 +156,7 @@ void LevenbergMarquardtOptimizer::iterate() { if (error <= state_.error) { state_.values.swap(newValues); state_.error = error; - state_.lambda /= params_.lambdaFactor; + decreaseLambda(); break; } else { // Either we're not cautious, or the same lambda was worse than the current error. @@ -157,7 +167,8 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { - state_.lambda *= params_.lambdaFactor; + cout << "increasing lambda: old error (" << state_.error << ") new error (" << error << ")" << endl; + increaseLambda(); } } } catch (IndeterminantLinearSystemException& e) { @@ -172,7 +183,7 @@ void LevenbergMarquardtOptimizer::iterate() { cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { - state_.lambda *= params_.lambdaFactor; + increaseLambda(); } } // Frank asks: why would we do that? diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index d8962da4a..15e14ab47 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -174,6 +174,12 @@ public: return state_.lambda; } + // Apply policy to increase lambda if the current update was successful + virtual void increaseLambda(); + + // Apply policy to decrease lambda if the current update was NOT successful + virtual void decreaseLambda(); + /// Access the current number of inner iterations int getInnerIterations() const { return state_.totalNumberInnerIterations;