diff --git a/gtsam.h b/gtsam.h index f32b12131..b6b48e795 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1130,32 +1130,46 @@ class Marginals { Matrix marginalInformation(size_t variable) const; }; -#include -class LevenbergMarquardtParams { - LevenbergMarquardtParams(); - void print(string s) const; +//************************************************************************* +// Nonlinear optimizers +//************************************************************************* - size_t getMaxIterations() const; - double getRelativeErrorTol() const; - double getAbsoluteErrorTol() const; - double getErrorTol() const; - string getVerbosity() const; +#include +virtual class NonlinearOptimizerParams { + NonlinearOptimizerParams(); + void print(string s) const; - void setMaxIterations(size_t value); - void setRelativeErrorTol(double value); - void setAbsoluteErrorTol(double value); - void setErrorTol(double value); - void setVerbosity(string s); + size_t getMaxIterations() const; + double getRelativeErrorTol() const; + double getAbsoluteErrorTol() const; + double getErrorTol() const; + string getVerbosity() const; + + void setMaxIterations(size_t value); + void setRelativeErrorTol(double value); + void setAbsoluteErrorTol(double value); + void setErrorTol(double value); + void setVerbosity(string s); +}; + +#include +virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { + SuccessiveLinearizationParams(); bool isMultifrontal() const; bool isSequential() const; bool isCholmod() const; bool isCG() const; +}; - double getlambdaInitial() const ; - double getlambdaFactor() const ; +#include +virtual class LevenbergMarquardtParams : gtsam::SuccessiveLinearizationParams { + LevenbergMarquardtParams(); + + double getlambdaInitial() const; + double getlambdaFactor() const; double getlambdaUpperBound() const; - string getVerbosityLM() const ; + string getVerbosityLM() const; void setlambdaInitial(double value); void setlambdaFactor(double value); @@ -1163,22 +1177,53 @@ class LevenbergMarquardtParams { void setVerbosityLM(string s); }; +#include +virtual class DoglegParams : gtsam::SuccessiveLinearizationParams { + DoglegParams(); + + double getDeltaInitial() const; + string getVerbosityDL() const; + + void setDeltaInitial(double deltaInitial) const; + void setVerbosityDL(string verbosityDL) const; +}; + +virtual class NonlinearOptimizer { + gtsam::Values optimizeSafely(); + double error() const; + int iterations() const; + gtsam::Values values() const; + void iterate() const; +}; + +virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); + double getDelta() const; +}; + +virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); + double lambda() const; +}; + //************************************************************************* // Nonlinear factor types //************************************************************************* -template +template virtual class PriorFactor : gtsam::NonlinearFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); }; -template +template virtual class BetweenFactor : gtsam::NonlinearFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); }; -template +template virtual class NonlinearEquality : gtsam::NonlinearFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index b0ee9f460..e3f4e4dcd 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -22,10 +22,33 @@ #include #include +#include + using namespace std; namespace gtsam { +/* ************************************************************************* */ +DoglegParams::VerbosityDL DoglegParams::verbosityDLTranslator(const std::string &verbosityDL) const { + std::string s = verbosityDL; boost::algorithm::to_upper(s); + if (s == "SILENT") return DoglegParams::SILENT; + if (s == "VERBOSE") return DoglegParams::VERBOSE; + + /* default is silent */ + return DoglegParams::SILENT; +} + +/* ************************************************************************* */ +std::string DoglegParams::verbosityDLTranslator(VerbosityDL verbosityDL) const { + std::string s; + switch (verbosityDL) { + case DoglegParams::SILENT: s = "SILENT"; break; + case DoglegParams::VERBOSE: s = "VERBOSE"; break; + default: s = "UNDEFINED"; break; + } + return s; +} + /* ************************************************************************* */ void DoglegOptimizer::iterate(void) { diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index 065d0b34d..8a21dd96e 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -50,6 +50,15 @@ public: std::cout << " deltaInitial: " << deltaInitial << "\n"; std::cout.flush(); } + + double getDeltaInitial() const { return deltaInitial; } + std::string getVerbosityDL() const { return verbosityDLTranslator(verbosityDL); } + + void setDeltaInitial(double deltaInitial) { this->deltaInitial = deltaInitial; } + void setVerbosityDL(const std::string& verbosityDL) { this->verbosityDL = verbosityDLTranslator(verbosityDL); } + + VerbosityDL verbosityDLTranslator(const std::string& verbosityDL) const; + std::string verbosityDLTranslator(VerbosityDL verbosityDL) const; }; /** @@ -128,7 +137,7 @@ public: const DoglegState& state() const { return state_; } /** Access the current trust region radius Delta */ - double Delta() const { return state_.Delta; } + double getDelta() const { return state_.Delta; } /// @}