From 6bee17b60332a19aabcd36dba64b92011c960d9e Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Tue, 24 Jul 2012 15:48:33 +0000 Subject: [PATCH] Added support for SPCG in matlab wrapper --- gtsam.h | 15 ++++++++ gtsam/nonlinear/DoglegOptimizer.h | 1 + gtsam/nonlinear/EasyFactorGraph.cpp | 2 +- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 1 + gtsam/nonlinear/NonlinearOptimizer.h | 1 + .../SuccessiveLinearizationOptimizer.h | 35 ++++++++++++++++--- 6 files changed, 49 insertions(+), 6 deletions(-) diff --git a/gtsam.h b/gtsam.h index 869ee6a09..e48ced5ee 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1209,13 +1209,22 @@ virtual class NonlinearOptimizerParams { virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { SuccessiveLinearizationParams(); + string getLinearSolverType() const; + + void setLinearSolverType(string solver); void setOrdering(const gtsam::Ordering& ordering); + bool isMultifrontal() const; bool isSequential() const; bool isCholmod() const; bool isCG() const; }; +#include +virtual class GaussNewtonParams : gtsam::SuccessiveLinearizationParams { + GaussNewtonParams(); +}; + #include virtual class LevenbergMarquardtParams : gtsam::SuccessiveLinearizationParams { LevenbergMarquardtParams(); @@ -1243,6 +1252,7 @@ virtual class DoglegParams : gtsam::SuccessiveLinearizationParams { }; virtual class NonlinearOptimizer { + gtsam::Values optimize(); gtsam::Values optimizeSafely(); double error() const; int iterations() const; @@ -1250,6 +1260,11 @@ virtual class NonlinearOptimizer { void iterate() const; }; +virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); + GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); +}; + 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); diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index 8a21dd96e..70d55d192 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -57,6 +57,7 @@ public: void setDeltaInitial(double deltaInitial) { this->deltaInitial = deltaInitial; } void setVerbosityDL(const std::string& verbosityDL) { this->verbosityDL = verbosityDLTranslator(verbosityDL); } +private: VerbosityDL verbosityDLTranslator(const std::string& verbosityDL) const; std::string verbosityDLTranslator(VerbosityDL verbosityDL) const; }; diff --git a/gtsam/nonlinear/EasyFactorGraph.cpp b/gtsam/nonlinear/EasyFactorGraph.cpp index 28e344efe..5031975f7 100644 --- a/gtsam/nonlinear/EasyFactorGraph.cpp +++ b/gtsam/nonlinear/EasyFactorGraph.cpp @@ -44,7 +44,7 @@ namespace gtsam { size_t verbosity) const { LevenbergMarquardtParams p; p.verbosity = (NonlinearOptimizerParams::Verbosity) verbosity; - p.linearSolverType = SuccessiveLinearizationParams::CG; + p.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; p.iterativeParams = boost::make_shared(); return optimizer(initialEstimate, p).optimizeSafely(); } diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 5677f6c08..4b3f8d391 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -61,6 +61,7 @@ public: inline void setlambdaUpperBound(double value) { lambdaUpperBound = value; } inline void setVerbosityLM(const std::string &s) { verbosityLM = verbosityLMTranslator(s); } +private: VerbosityLM verbosityLMTranslator(const std::string &s) const; std::string verbosityLMTranslator(VerbosityLM value) const; }; diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 1c071faef..03f3a5e34 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -63,6 +63,7 @@ public: void setErrorTol(double value) { errorTol = value ; } void setVerbosity(const std::string &src) { verbosity = verbosityTranslator(src); } +private: Verbosity verbosityTranslator(const std::string &s) const; std::string verbosityTranslator(Verbosity value) const; }; diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h index 2556134c0..b46686fe7 100644 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h @@ -31,7 +31,7 @@ public: MULTIFRONTAL_QR, SEQUENTIAL_CHOLESKY, SEQUENTIAL_QR, - CG, /* Experimental Flag */ + CONJUGATE_GRADIENT, /* Experimental Flag */ CHOLMOD, /* Experimental Flag */ }; @@ -61,7 +61,7 @@ public: case CHOLMOD: std::cout << " linear solver type: CHOLMOD\n"; break; - case CG: + case CONJUGATE_GRADIENT: std::cout << " linear solver type: CG\n"; break; default: @@ -85,9 +85,7 @@ public: inline bool isCholmod() const { return (linearSolverType == CHOLMOD); } - inline bool isCG() const { return (linearSolverType == CG); } - - void setOrdering(const Ordering& ordering) { this->ordering = ordering; } + inline bool isCG() const { return (linearSolverType == CONJUGATE_GRADIENT); } GaussianFactorGraph::Eliminate getEliminationFunction() { switch (linearSolverType) { @@ -105,6 +103,33 @@ public: break; } } + + std::string getLinearSolverType() const { return linearSolverTranslator(linearSolverType); } + + void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); } + void setOrdering(const Ordering& ordering) { this->ordering = ordering; } + +private: + std::string linearSolverTranslator(LinearSolverType linearSolverType) const { + switch(linearSolverType) { + case MULTIFRONTAL_CHOLESKY: return "MULTIFRONTAL_CHOLESKY"; + case MULTIFRONTAL_QR: return "MULTIFRONTAL_QR"; + case SEQUENTIAL_CHOLESKY: return "SEQUENTIAL_CHOLESKY"; + case SEQUENTIAL_QR: return "SEQUENTIAL_QR"; + case CONJUGATE_GRADIENT: return "CONJUGATE_GRADIENT"; + case CHOLMOD: return "CHOLMOD"; + default: throw std::invalid_argument("Unknown linear solver type in SuccessiveLinearizationOptimizer"); + } + } + LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const { + if(linearSolverType == "MULTIFRONTAL_CHOLESKY") return MULTIFRONTAL_CHOLESKY; + if(linearSolverType == "MULTIFRONTAL_QR") return MULTIFRONTAL_QR; + if(linearSolverType == "SEQUENTIAL_CHOLESKY") return SEQUENTIAL_CHOLESKY; + if(linearSolverType == "SEQUENTIAL_QR") return SEQUENTIAL_QR; + if(linearSolverType == "CONJUGATE_GRADIENT") return CONJUGATE_GRADIENT; + if(linearSolverType == "CHOLMOD") return CHOLMOD; + throw std::invalid_argument("Unknown linear solver type in SuccessiveLinearizationOptimizer"); + } }; } /* namespace gtsam */