diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 7411c082d..358d72ac3 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -99,7 +99,7 @@ void LevenbergMarquardtOptimizer::iterate() { const LevenbergMarquardtParams::VerbosityLM lmVerbosity = params_.verbosityLM; // Linearize graph - if(nloVerbosity >= NonlinearOptimizerParams::ERROR) + if(lmVerbosity >= LevenbergMarquardtParams::DAMPED) cout << "linearizing = " << endl; GaussianFactorGraph::shared_ptr linear = linearize(); @@ -172,7 +172,8 @@ void LevenbergMarquardtOptimizer::iterate() { // create new optimization state with more adventurous lambda gttic (compute_error); - if(nloVerbosity >= NonlinearOptimizerParams::ERROR) cout << "calculating error" << endl; + + if(lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "calculating error" << endl; double error = graph_.error(newValues); gttoc(compute_error); @@ -211,11 +212,15 @@ void LevenbergMarquardtOptimizer::iterate() { increaseLambda(modelFidelity); } + // bool converged = checkConvergence(_params().relativeErrorTol, _params().absoluteErrorTol, _params().errorTol, state_.error, error); + // cout << " Inner iteration - converged " << converged << endl; } } catch (IndeterminantLinearSystemException& e) { (void) e; // Prevent unused variable warning if(lmVerbosity >= LevenbergMarquardtParams::LAMBDA) 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. diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 52c1e1f08..1ff84ebfd 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -55,8 +55,12 @@ void NonlinearOptimizer::defaultOptimize() { if (params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "Initial error: " << currentError << endl; // Return if we already have too many iterations - if(this->iterations() >= params.maxIterations) + if(this->iterations() >= params.maxIterations){ + if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) { + cout << "iterations: " << this->iterations() << " >? " << params.maxIterations << endl; + } return; + } // Iterative loop do { @@ -152,11 +156,16 @@ bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold } bool converged = (relativeErrorTreshold && (relativeDecrease <= relativeErrorTreshold)) || (absoluteDecrease <= absoluteErrorTreshold); - if (verbosity >= NonlinearOptimizerParams::ERROR && converged) { + if (verbosity >= NonlinearOptimizerParams::TERMINATION && converged) { + if(absoluteDecrease >= 0.0) cout << "converged" << endl; else cout << "Warning: stopping nonlinear iterations because error increased" << endl; + + cout << "errorThreshold: " << newError << "