From d6c2b415a5d4447cea12b87ffee6406a4da6c4d8 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 4 Mar 2010 22:46:27 +0000 Subject: [PATCH] Added a check to NonlinearOptimizer to short circuit optimization if the initial error is below the absolute threshold. --- cpp/NonlinearOptimizer-inl.h | 4 ++++ cpp/testNonlinearOptimizer.cpp | 2 +- cpp/testSQP.cpp | 4 ++-- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/cpp/NonlinearOptimizer-inl.h b/cpp/NonlinearOptimizer-inl.h index 1c9bdf5fe..a555da1cd 100644 --- a/cpp/NonlinearOptimizer-inl.h +++ b/cpp/NonlinearOptimizer-inl.h @@ -186,6 +186,10 @@ namespace gtsam { double relativeThreshold, double absoluteThreshold, verbosityLevel verbosity, int maxIterations, double lambdaFactor) const { + // check if we're already close enough + if (error_ < absoluteThreshold) + return *this; + // do one iteration of LM NonlinearOptimizer next = iterateLM(verbosity, lambdaFactor); diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index 4976a34bc..799667cb6 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -32,7 +32,7 @@ using namespace boost; using namespace gtsam; using namespace example; -const double tol = 1e-6; +const double tol = 1e-5; typedef NonlinearOptimizer Optimizer; diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 4bb5b7ace..a50843662 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -627,14 +627,14 @@ TEST (SQP, stereo_truth_noisy ) { VOptimizer optimizer(optimizer0.levenbergMarquardt(relThresh, absThresh, VOptimizer::SILENT)); // verify - DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-5); // check if correct if (verbose) { optimizer.config()->print("After iteration"); cout << "Final error: " << optimizer.error() << endl; } - CHECK(assert_equal(*truthConfig,*(optimizer.config()))); + CHECK(assert_equal(*truthConfig,*(optimizer.config()), 1e-5)); } /* ********************************************************************* */