diff --git a/gtsam.h b/gtsam.h index 4aa1480df..5b21f726f 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1045,6 +1045,46 @@ class GaussianSequentialSolver { Matrix marginalCovariance(size_t j) const; }; +#include +virtual class IterativeOptimizationParameters { + string getKernel() const ; + string getVerbosity() const; + void setKernel(string s) ; + void setVerbosity(string s) ; +}; + +//virtual class IterativeSolver { +// IterativeSolver(); +// gtsam::VectorValues optimize (); +//}; + +#include +virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { + ConjugateGradientParameters(); + size_t getMinIterations() const ; + size_t getMaxIterations() const ; + size_t getReset() const; + double getEpsilon_rel() const; + double getEpsilon_abs() const; + + void setMinIterations(size_t value); + void setMaxIterations(size_t value); + void setReset(size_t value); + void setEpsilon_rel(double value); + void setEpsilon_abs(double value); +}; + +#include +virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { + SubgraphSolverParameters(); + void print(string s) const; +}; + +class SubgraphSolver { + SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters); + gtsam::VectorValues optimize() const; +}; + #include class KalmanFilter { KalmanFilter(size_t n); @@ -1288,6 +1328,7 @@ virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { void setLinearSolverType(string solver); void setOrdering(const gtsam::Ordering& ordering); + void setIterativeParams(const gtsam::SubgraphSolverParameters ¶ms); bool isMultifrontal() const; bool isSequential() const; diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index cf1d31d85..a89d7a10c 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -20,9 +20,7 @@ namespace gtsam { */ class ConjugateGradientParameters : public IterativeOptimizationParameters { - public: - typedef IterativeOptimizationParameters Base; typedef boost::shared_ptr shared_ptr; @@ -49,7 +47,21 @@ public: inline double epsilon_rel() const { return epsilon_rel_; } inline double epsilon_abs() const { return epsilon_abs_; } - void print() const { + inline size_t getMinIterations() const { return minIterations_; } + inline size_t getMaxIterations() const { return maxIterations_; } + inline size_t getReset() const { return reset_; } + inline double getEpsilon() const { return epsilon_rel_; } + inline double getEpsilon_rel() const { return epsilon_rel_; } + inline double getEpsilon_abs() const { return epsilon_abs_; } + + inline void setMinIterations(size_t value) { minIterations_ = value; } + inline void setMaxIterations(size_t value) { maxIterations_ = value; } + inline void setReset(size_t value) { reset_ = value; } + inline void setEpsilon(double value) { epsilon_rel_ = value; } + inline void setEpsilon_rel(double value) { epsilon_rel_ = value; } + inline void setEpsilon_abs(double value) { epsilon_abs_ = value; } + + virtual void print(const std::string &s="") const { Base::print(); std::cout << "ConjugateGradientParameters: " << "minIter = " << minIterations_ @@ -61,18 +73,4 @@ public: } }; -//class ConjugateGradientSolver : public IterativeSolver { -// -//public: -// -// typedef ConjugateGradientParameters Parameters; -// -// Parameters parameters_; -// -// ConjugateGradientSolver(const ConjugateGradientParameters ¶meters) : parameters_(parameters) {} -// virtual VectorValues::shared_ptr optimize () = 0; -// virtual const IterativeOptimizationParameters& _params() const = 0; -//}; - - } diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp new file mode 100644 index 000000000..2bba3e12b --- /dev/null +++ b/gtsam/linear/IterativeSolver.cpp @@ -0,0 +1,50 @@ +/** + * @file IterativeSolver.cpp + * @brief + * @date Sep 3, 2012 + * @author Yong-Dian Jian + */ + +#include +#include +#include + +namespace gtsam { + +std::string IterativeOptimizationParameters::getKernel() const { return kernelTranslator(kernel_); } +std::string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); } +void IterativeOptimizationParameters::setKernel(const std::string &src) { kernel_ = kernelTranslator(src); } +void IterativeOptimizationParameters::setVerbosity(const std::string &src) { verbosity_ = verbosityTranslator(src); } + +IterativeOptimizationParameters::Kernel IterativeOptimizationParameters::kernelTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "CG") return IterativeOptimizationParameters::CG; + /* default is cg */ + else return IterativeOptimizationParameters::CG; +} + +IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "SILENT") return IterativeOptimizationParameters::SILENT; + else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY; + else if (s == "ERROR") return IterativeOptimizationParameters::ERROR; + /* default is default */ + else return IterativeOptimizationParameters::SILENT; +} + +std::string IterativeOptimizationParameters::kernelTranslator(IterativeOptimizationParameters::Kernel k) { + if ( k == CG ) return "CG"; + else return "UNKNOWN"; +} + +std::string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) { + if (verbosity == SILENT) return "SILENT"; + else if (verbosity == COMPLEXITY) return "COMPLEXITY"; + else if (verbosity == ERROR) return "ERROR"; + else return "UNKNOWN"; +} + + +} + + diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 8292b3831..2f76df2b7 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -13,6 +13,7 @@ #include #include +#include namespace gtsam { @@ -41,22 +42,32 @@ namespace gtsam { inline Kernel kernel() const { return kernel_; } inline Verbosity verbosity() const { return verbosity_; } + /* matlab interface */ + std::string getKernel() const ; + std::string getVerbosity() const; + void setKernel(const std::string &s) ; + void setVerbosity(const std::string &s) ; + void print() const { const std::string kernelStr[1] = {"cg"}; std::cout << "IterativeOptimizationParameters: " << "kernel = " << kernelStr[kernel_] << ", verbosity = " << verbosity_ << std::endl; } + + static Kernel kernelTranslator(const std::string &s); + static Verbosity verbosityTranslator(const std::string &s); + static std::string kernelTranslator(Kernel k); + static std::string verbosityTranslator(Verbosity v); }; class IterativeSolver { public: typedef boost::shared_ptr shared_ptr; - IterativeSolver(){} + IterativeSolver() {} virtual ~IterativeSolver() {} - /* update interface to the nonlinear optimizer */ - virtual void replaceFactors(const GaussianFactorGraph::shared_ptr &factorGraph, const double lambda) {} + /* interface to the nonlinear optimizer */ virtual VectorValues optimize () = 0; }; diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 04b1cb127..e4d99933b 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -25,9 +25,9 @@ class SubgraphSolverParameters : public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; SubgraphSolverParameters() : Base() {} + virtual void print(const std::string &s="") const { Base::print(s); } }; - /** * This class implements the SPCG solver presented in Dellaert et al in IROS'10. * diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp index 9d85f26fe..fec460b08 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp @@ -15,6 +15,10 @@ namespace gtsam { +void SuccessiveLinearizationParams::setIterativeParams(const SubgraphSolverParameters ¶ms) { + iterativeParams = boost::make_shared(params); +} + void SuccessiveLinearizationParams::print(const std::string& str) const { NonlinearOptimizerParams::print(str); switch ( linearSolverType ) { diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index e79a364c5..d422c09a9 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -19,7 +19,7 @@ #pragma once #include -#include +#include namespace gtsam { @@ -74,6 +74,7 @@ public: std::string getLinearSolverType() const { return linearSolverTranslator(linearSolverType); } void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); } + void setIterativeParams(const SubgraphSolverParameters ¶ms); void setOrdering(const Ordering& ordering) { this->ordering = ordering; } private: diff --git a/matlab/gtsam_examples/Pose2SLAMwSPCG.m b/matlab/gtsam_examples/Pose2SLAMwSPCG.m index a45ac40a0..67f22fe1d 100644 --- a/matlab/gtsam_examples/Pose2SLAMwSPCG.m +++ b/matlab/gtsam_examples/Pose2SLAMwSPCG.m @@ -51,7 +51,14 @@ initialEstimate.insert(4, Pose2(4.0, 2.0, pi )); initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2)); initialEstimate.print(sprintf('\nInitial estimate:\n')); -%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -optimizer = DoglegOptimizer(graph, initialEstimate); -result = optimizer.optimizeSafely(); -result.print(sprintf('\nFinal result:\n')); \ No newline at end of file +%% Optimize using Levenberg-Marquardt optimization with SubgraphSolver +params = gtsam.LevenbergMarquardtParams; +subgraphParams = gtsam.SubgraphSolverParameters; +params.setLinearSolverType('CONJUGATE_GRADIENT'); +params.setIterativeParams(subgraphParams); +optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimize(); +result.print(sprintf('\nFinal result:\n')); + + +