diff --git a/cpp/NonlinearOptimizer-inl.h b/cpp/NonlinearOptimizer-inl.h index 285b4a127..634b1860b 100644 --- a/cpp/NonlinearOptimizer-inl.h +++ b/cpp/NonlinearOptimizer-inl.h @@ -101,6 +101,13 @@ namespace gtsam { verbosityLevel verbosity, int maxIterations) const { static W writer(error_); + // check if we're already close enough + if (error_ < absoluteThreshold) { + if (verbosity >= ERROR) cout << "Exiting, as error = " << error_ + << " < absoluteThreshold (" << absoluteThreshold << ")" << endl; + return *this; + } + // linearize, solve, update NonlinearOptimizer next = iterate(verbosity); @@ -191,8 +198,11 @@ namespace gtsam { verbosityLevel verbosity, int maxIterations, double lambdaFactor) const { // check if we're already close enough - if (error_ < absoluteThreshold) + if (error_ < absoluteThreshold) { + if (verbosity >= ERROR) cout << "Exiting, as error = " << error_ + << " < absoluteThreshold (" << absoluteThreshold << ")" << endl; return *this; + } // do one iteration of LM NonlinearOptimizer next = iterateLM(verbosity, lambdaFactor);