diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 358d72ac3..25c5c69a6 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -49,13 +49,14 @@ LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTrans std::string LevenbergMarquardtParams::verbosityLMTranslator(VerbosityLM value) const { std::string s; switch (value) { - case LevenbergMarquardtParams::SILENT: s = "SILENT" ; break; - case LevenbergMarquardtParams::LAMBDA: s = "LAMBDA" ; break; - case LevenbergMarquardtParams::TRYLAMBDA: s = "TRYLAMBDA" ; break; - case LevenbergMarquardtParams::TRYCONFIG: s = "TRYCONFIG" ; break; - case LevenbergMarquardtParams::TRYDELTA: s = "TRYDELTA" ; break; - case LevenbergMarquardtParams::DAMPED: s = "DAMPED" ; break; - default: s = "UNDEFINED" ; break; + case LevenbergMarquardtParams::SILENT: s = "SILENT" ; break; + case LevenbergMarquardtParams::TERMINATION: s = "TERMINATION" ; break; + case LevenbergMarquardtParams::LAMBDA: s = "LAMBDA" ; break; + case LevenbergMarquardtParams::TRYLAMBDA: s = "TRYLAMBDA" ; break; + case LevenbergMarquardtParams::TRYCONFIG: s = "TRYCONFIG" ; break; + case LevenbergMarquardtParams::TRYDELTA: s = "TRYDELTA" ; break; + case LevenbergMarquardtParams::DAMPED: s = "DAMPED" ; break; + default: s = "UNDEFINED" ; break; } return s; } @@ -203,7 +204,7 @@ void LevenbergMarquardtOptimizer::iterate() { // The more adventurous lambda was worse too, so make lambda more conservative // and keep the same values. if(state_.lambda >= params_.lambdaUpperBound) { - if(nloVerbosity >= NonlinearOptimizerParams::ERROR) + if(nloVerbosity >= NonlinearOptimizerParams::TERMINATION) cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { @@ -212,20 +213,24 @@ void LevenbergMarquardtOptimizer::iterate() { increaseLambda(modelFidelity); } - // bool converged = checkConvergence(_params().relativeErrorTol, _params().absoluteErrorTol, _params().errorTol, state_.error, error); - // cout << " Inner iteration - converged " << converged << endl; +// bool converged = checkConvergence(_params().relativeErrorTol, _params().absoluteErrorTol, _params().errorTol, state_.error, error); +// cout << " Inner iteration - converged " << converged << endl; +// cout << "current error " << state_.error << endl; +// cout << "new error " << error << endl; +// cout << "costChange " << costChange << endl; +// cout << "linearizedCostChange " << linearizedCostChange << endl; +// cout << "modelFidelity " << modelFidelity << endl; } } catch (IndeterminantLinearSystemException& e) { (void) e; // Prevent unused variable warning - if(lmVerbosity >= LevenbergMarquardtParams::LAMBDA) + if(lmVerbosity >= LevenbergMarquardtParams::TERMINATION) cout << "Negative matrix, increasing lambda" << endl; - // cout << "failed to solve current system" << endl; // Either we're not cautious, or the same lambda was worse than the current error. // The more adventurous lambda was worse too, so make lambda more conservative // and keep the same values. if(state_.lambda >= params_.lambdaUpperBound) { - if(nloVerbosity >= NonlinearOptimizerParams::ERROR) + if(nloVerbosity >= NonlinearOptimizerParams::TERMINATION) cout << "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" << endl; break; } else { diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 08fe32002..d9da3e68b 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -35,7 +35,7 @@ class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams { public: /** See LevenbergMarquardtParams::lmVerbosity */ enum VerbosityLM { - SILENT = 0, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA + SILENT = 0, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA }; private: diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index b1ec2124a..7b0e73985 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -27,6 +27,8 @@ NonlinearOptimizerParams::Verbosity NonlinearOptimizerParams::verbosityTranslato return NonlinearOptimizerParams::DELTA; if (s == "LINEAR") return NonlinearOptimizerParams::LINEAR; + if (s == "TERMINATION") + return NonlinearOptimizerParams::TERMINATION; /* default is silent */ return NonlinearOptimizerParams::SILENT; @@ -40,6 +42,9 @@ std::string NonlinearOptimizerParams::verbosityTranslator( case NonlinearOptimizerParams::SILENT: s = "SILENT"; break; + case NonlinearOptimizerParams::TERMINATION: + s = "TERMINATION"; + break; case NonlinearOptimizerParams::ERROR: s = "ERROR"; break;