diff --git a/gtsam.h b/gtsam.h index 6fa694330..8e0aa522a 100644 --- a/gtsam.h +++ b/gtsam.h @@ -961,6 +961,15 @@ class LevenbergMarquardtParams { LevenbergMarquardtParams(); LevenbergMarquardtParams(double initial, double factor, double bound, size_t verbose); void print(string s) const; + double getlambdaInitial() const ; + double getlambdaFactor() const ; + double getlambdaUpperBound() const; + string getVerbosityLM() const ; + + void setlambdaInitial(double value); + void setlambdaFactor(double value); + void setlambdaUpperBound(double value); + void setVerbosityLM(string s); }; }///\namespace gtsam diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 5847986be..15c2262fa 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -25,11 +25,52 @@ #include #include #include +#include +#include using namespace std; namespace gtsam { +/* ************************************************************************* */ +LevenbergMarquardtParams::VerbosityLM LevenbergMarquardtParams::verbosityLMTranslator(const std::string &src) const { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "SILENT") return LevenbergMarquardtParams::SILENT; + if (s == "LAMBDA") return LevenbergMarquardtParams::LAMBDA; + if (s == "TRYLAMBDA") return LevenbergMarquardtParams::TRYLAMBDA; + if (s == "TRYCONFIG") return LevenbergMarquardtParams::TRYCONFIG; + if (s == "TRYDELTA") return LevenbergMarquardtParams::TRYDELTA; + if (s == "DAMPED") return LevenbergMarquardtParams::DAMPED; + + /* default is silent */ + return LevenbergMarquardtParams::SILENT; +} + +/* ************************************************************************* */ +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; + } + return s; +} + +/* ************************************************************************* */ +void LevenbergMarquardtParams::print(const std::string& str) const { + SuccessiveLinearizationParams::print(str); + std::cout << " lambdaInitial: " << lambdaInitial << "\n"; + std::cout << " lambdaFactor: " << lambdaFactor << "\n"; + std::cout << " lambdaUpperBound: " << lambdaUpperBound << "\n"; + std::cout << " verbosityLM: " << verbosityLMTranslator(verbosityLM) << "\n"; + std::cout.flush(); +} + /* ************************************************************************* */ void LevenbergMarquardtOptimizer::iterate() { diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 231df9f4e..f5e4195da 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -53,15 +53,20 @@ public: lambdaInitial(initial), lambdaFactor(factor), lambdaUpperBound(bound), verbosityLM(VerbosityLM(verbose)) {} virtual ~LevenbergMarquardtParams() {} + virtual void print(const std::string& str = "") const; - virtual void print(const std::string& str = "") const { - SuccessiveLinearizationParams::print(str); - std::cout << " lambdaInitial: " << lambdaInitial << "\n"; - std::cout << " lambdaFactor: " << lambdaFactor << "\n"; - std::cout << " lambdaUpperBound: " << lambdaUpperBound << "\n"; - std::cout << " verbosityLM: " << verbosityLM << "\n"; - std::cout.flush(); - } + inline double getlambdaInitial() const { return lambdaInitial; } + inline double getlambdaFactor() const { return lambdaFactor; } + inline double getlambdaUpperBound() const { return lambdaUpperBound; } + inline std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM); } + + inline void setlambdaInitial(double value) { lambdaInitial = value; } + inline void setlambdaFactor(double value) { lambdaFactor = value; } + inline void setlambdaUpperBound(double value) { lambdaUpperBound = value; } + inline void setVerbosityLM(const std::string &s) { verbosityLM = verbosityLMTranslator(s); } + + VerbosityLM verbosityLMTranslator(const std::string &s) const; + std::string verbosityLMTranslator(VerbosityLM value) const; }; /** diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 76d1fe573..5f2f2d7d4 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -19,11 +19,49 @@ #include #include #include - +#include using namespace std; namespace gtsam { +/* ************************************************************************* */ +NonlinearOptimizerParams::Verbosity NonlinearOptimizerParams::verbosityTranslator(const std::string &src) const { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "SILENT") return NonlinearOptimizerParams::SILENT; + if (s == "ERROR") return NonlinearOptimizerParams::ERROR; + if (s == "VALUES") return NonlinearOptimizerParams::VALUES; + if (s == "DELTA") return NonlinearOptimizerParams::DELTA; + if (s == "LINEAR") return NonlinearOptimizerParams::LINEAR; + + /* default is silent */ + return NonlinearOptimizerParams::SILENT; +} + +/* ************************************************************************* */ +std::string NonlinearOptimizerParams::verbosityTranslator(Verbosity value) const { + std::string s; + switch (value) { + case NonlinearOptimizerParams::SILENT: s = "SILENT"; break; + case NonlinearOptimizerParams::ERROR: s = "ERROR"; break; + case NonlinearOptimizerParams::VALUES: s = "VALUES"; break; + case NonlinearOptimizerParams::DELTA: s = "DELTA"; break; + case NonlinearOptimizerParams::LINEAR: s = "LINEAR"; break; + default: s = "UNDEFINED"; break; + } + return s; +} + +/* ************************************************************************* */ +void NonlinearOptimizerParams::print(const std::string& str) const { + std::cout << str << "\n"; + std::cout << "relative decrease threshold: " << relativeErrorTol << "\n"; + std::cout << "absolute decrease threshold: " << absoluteErrorTol << "\n"; + std::cout << " total error threshold: " << errorTol << "\n"; + std::cout << " maximum iterations: " << maxIterations << "\n"; + std::cout << " verbosity: " << verbosityTranslator(verbosity) << "\n"; + std::cout.flush(); +} + /* ************************************************************************* */ void NonlinearOptimizer::defaultOptimize() { diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 2483aa800..8a7670f85 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -48,18 +48,24 @@ public: maxIterations(100.0), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol(0.0), verbosity(SILENT) {} - virtual void print(const std::string& str = "") const { - std::cout << str << "\n"; - std::cout << "relative decrease threshold: " << relativeErrorTol << "\n"; - std::cout << "absolute decrease threshold: " << absoluteErrorTol << "\n"; - std::cout << " total error threshold: " << errorTol << "\n"; - std::cout << " maximum iterations: " << maxIterations << "\n"; - std::cout << " verbosity level: " << verbosity << std::endl; - } - virtual ~NonlinearOptimizerParams() {} -}; + virtual void print(const std::string& str = "") const ; + inline double getMaxIterations() const { return maxIterations; } + inline double getRelativeErrorTol() const { return relativeErrorTol; } + inline double getAbsoluteErrorTol() const { return absoluteErrorTol; } + inline double getErrorTol() const { return errorTol; } + inline std::string getVerbosity() const { return verbosityTranslator(verbosity); } + + inline void setMaxIterations(double value) { maxIterations = value; } + inline void setRelativeErrorTol(double value) { relativeErrorTol = value; } + inline void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; } + inline void setErrorTol(double value) { errorTol = value ; } + inline void setVerbosity(const std::string &src) { verbosity = verbosityTranslator(src); } + + Verbosity verbosityTranslator(const std::string &s) const; + std::string verbosityTranslator(Verbosity value) const; +}; /** * Base class for a nonlinear optimization state, including the current estimate diff --git a/matlab/examples/SFMExample.m b/matlab/examples/SFMExample.m index 8aa0105b2..34682e1e6 100644 --- a/matlab/examples/SFMExample.m +++ b/matlab/examples/SFMExample.m @@ -61,14 +61,14 @@ for j=1:size(truth.points,2) end initialEstimate.print(sprintf('\nInitial estimate:\n ')); -%% One-shot Optimize using Levenberg-Marquardt optimization with an ordering from colamd -%result = graph.optimize(initialEstimate,1); -%result.print(sprintf('\nFinal result:\n ')); - %% Fine grain optimization, allowing user to iterate step by step -parameters = gtsamLevenbergMarquardtParams(1e-5, 1e-5, 0, 2); +parameters = gtsamLevenbergMarquardtParams(1e-5, 1e-5, 0, 0); +parameters.setlambdaInitial(1.0); +parameters.setVerbosityLM('trylambda'); + optimizer = graph.optimizer(initialEstimate, parameters); + for i=1:5 optimizer.iterate(); end