From a48bf9499ad702881796dde052c5512434bf2068 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Fri, 1 Oct 2010 14:47:28 +0000 Subject: [PATCH] add new interface to levenbergMarquardt, now can take a threshold for sum of error --- nonlinear/NonlinearOptimizer-inl.h | 71 ++++++++++++++++++++++-------- nonlinear/NonlinearOptimizer.h | 35 +++++++++++++-- 2 files changed, 84 insertions(+), 22 deletions(-) diff --git a/nonlinear/NonlinearOptimizer-inl.h b/nonlinear/NonlinearOptimizer-inl.h index 60ab68c7d..09062cfb0 100644 --- a/nonlinear/NonlinearOptimizer-inl.h +++ b/nonlinear/NonlinearOptimizer-inl.h @@ -21,9 +21,21 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - inline bool check_convergence(double relativeErrorTreshold, - double absoluteErrorTreshold, double currentError, double newError, - int verbosity) { + inline bool check_convergence( + double relativeErrorTreshold, + double absoluteErrorTreshold, + double errorThreshold, + double currentError, double newError, int verbosity) { + + if ( verbosity >= 2 ) { + if ( newError <= errorThreshold ) + cout << "errorThreshold: " << newError << " < " << errorThreshold << endl; + else + cout << "errorThreshold: " << newError << " > " << errorThreshold << endl; + } + + if ( newError <= errorThreshold ) return true ; + // check if diverges double absoluteDecrease = currentError - newError; if (verbosity >= 2) { @@ -120,8 +132,13 @@ namespace gtsam { writer.write(next.error_); // check convergence - bool converged = gtsam::check_convergence(relativeThreshold, - absoluteThreshold, error_, next.error_, verbosity); + bool converged = gtsam::check_convergence( + relativeThreshold, + absoluteThreshold, + 0.0, + error_, + next.error_, + verbosity); // return converged state or iterate if (converged) @@ -236,37 +253,53 @@ namespace gtsam { double relativeThreshold, double absoluteThreshold, verbosityLevel verbosity, int maxIterations, double lambdaFactor, LambdaMode lambdaMode) const { - if (maxIterations <= 0) return *this; + return levenbergMarquardt(NonLinearOptimizerPara (absoluteThreshold, relativeThreshold, absoluteThreshold, + maxIterations, lambdaFactor, verbosity, lambdaMode)) ; + } + + + template + NonlinearOptimizer NonlinearOptimizer:: + levenbergMarquardt(const NonLinearOptimizerPara ¶) const { + + if (para.maxIterations_ <= 0) return *this; // check if we're already close enough - if (error_ < absoluteThreshold) { - if (verbosity >= ERROR) cout << "Exiting, as error = " << error_ - << " < absoluteThreshold (" << absoluteThreshold << ")" << endl; + if (error_ < para.sumError_) { + if (para.verbosity_ >= ERROR) + cout << "Exiting, as error = " << error_ << " < " << para.sumError_ << endl; return *this; } // do one iteration of LM - NonlinearOptimizer next = iterateLM(verbosity, lambdaFactor, lambdaMode); + NonlinearOptimizer next = iterateLM(para.verbosity_, para.lambdaFactor_, para.lambdaMode_); // check convergence // TODO: move convergence checks here and incorporate in verbosity levels // TODO: build into iterations somehow as an instance variable - bool converged = gtsam::check_convergence(relativeThreshold, - absoluteThreshold, error_, next.error_, verbosity); + bool converged = gtsam::check_convergence( + para.relDecrease_, + para.absDecrease_, + para.sumError_, + error_, + next.error_, + para.verbosity_); // return converged state or iterate - if (converged || maxIterations <= 1) { + if (converged || para.maxIterations_ <= 1) { // maybe show output - if (verbosity >= CONFIG) + if (para.verbosity_ >= CONFIG) next.config_->print("final config"); - if (verbosity >= ERROR) + if (para.verbosity_ >= ERROR) cout << "final error: " << next.error_ << endl; - if (verbosity >= LAMBDA) + if (para.verbosity_ >= LAMBDA) cout << "final lambda = " << next.lambda_ << endl; return next; - } else - return next.levenbergMarquardt(relativeThreshold, absoluteThreshold, - verbosity, maxIterations-1, lambdaFactor, lambdaMode); + } else { + NonLinearOptimizerPara newPara = para ; + newPara.maxIterations_ = newPara.maxIterations_ - 1; + return next.levenbergMarquardt(newPara) ; + } } /* ************************************************************************* */ diff --git a/nonlinear/NonlinearOptimizer.h b/nonlinear/NonlinearOptimizer.h index b2160ab18..32f26ba61 100644 --- a/nonlinear/NonlinearOptimizer.h +++ b/nonlinear/NonlinearOptimizer.h @@ -68,6 +68,30 @@ namespace gtsam { CAUTIOUS } LambdaMode; + // a container for all related parameters + struct NonLinearOptimizerPara { + public: + double absDecrease_; /* threshold for the absolute decrease per iteration */ + double relDecrease_; /* threshold for the relative decrease per iteration */ + double sumError_; /* threshold for the sum of error */ + int maxIterations_ ; + double lambdaFactor_ ; + verbosityLevel verbosity_; + LambdaMode lambdaMode_; + + public: + + NonLinearOptimizerPara(): absDecrease_(1), relDecrease_(1e-3), sumError_(0.0), + maxIterations_(100), lambdaFactor_(10.0), verbosity_(ERROR), lambdaMode_(BOUNDED){} + + NonLinearOptimizerPara(double absDecrease, double relDecrease, double sumError, + int iIters = 100, double lambdaFactor = 10, verbosityLevel v = ERROR, LambdaMode lambdaMode = BOUNDED) + :absDecrease_(absDecrease), relDecrease_(relDecrease), sumError_(sumError), + maxIterations_(iIters), lambdaFactor_(lambdaFactor), verbosity_(v), lambdaMode_(lambdaMode){} + + }; + + private: // keep a reference to const version of the graph @@ -180,6 +204,9 @@ namespace gtsam { double lambdaFactor = 10, LambdaMode lambdaMode = BOUNDED) const; + NonlinearOptimizer + levenbergMarquardt(const NonLinearOptimizerPara ¶) const; + /** * Static interface to LM optimization using default ordering and thresholds * @param graph Nonlinear factor graph to optimize @@ -247,10 +274,12 @@ namespace gtsam { /** * Check convergence */ - bool check_convergence (double relativeErrorTreshold, + bool check_convergence ( + double relativeErrorTreshold, double absoluteErrorTreshold, - double currentError, double newError, - int verbosity); + double errorThreshold, + double currentError, double newError, int verbosity); + } // gtsam