From def9b84e45828303d4bcb5d53a6ec0a901b2b733 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 Oct 2013 18:27:43 +0000 Subject: [PATCH] Added virtual solve method to NonlinearOptimizer that you can override, e.g., with Ceres See example in testNonlinearOptimizer As part of this, I also merged SuccessiveLinearizationParams into NonlinearOptimizerParams, which is now in its own separate file NonlinearOptimizerParams.h --- examples/Pose2SLAMwSPCG.cpp | 2 +- examples/SolverComparer.cpp | 2 +- gtsam.h | 12 +- gtsam/linear/SubgraphSolver.h | 2 +- gtsam/nonlinear/DoglegOptimizer.h | 6 +- gtsam/nonlinear/GaussNewtonOptimizer.cpp | 2 +- gtsam/nonlinear/GaussNewtonOptimizer.h | 4 +- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 10 +- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 4 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 86 ++++----- gtsam/nonlinear/NonlinearOptimizer.h | 49 +----- .../NonlinearOptimizerParameters.cpp | 1 + .../nonlinear/NonlinearOptimizerParameters.h | 1 + gtsam/nonlinear/NonlinearOptimizerParams.cpp | 155 ++++++++++++++++ gtsam/nonlinear/NonlinearOptimizerParams.h | 165 ++++++++++++++++++ .../SuccessiveLinearizationOptimizer.cpp | 79 --------- .../SuccessiveLinearizationOptimizer.h | 108 ------------ gtsam_unstable/geometry/triangulation.h | 2 +- .../nonlinear/ConcurrentBatchSmoother.cpp | 1 - .../tests/testConcurrentIncrementalFilter.cpp | 2 +- .../testConcurrentIncrementalSmootherDL.cpp | 2 +- .../testConcurrentIncrementalSmootherGN.cpp | 2 +- tests/testGaussianFactorGraphB.cpp | 2 +- tests/testNonlinearOptimizer.cpp | 46 +++++ 24 files changed, 445 insertions(+), 300 deletions(-) create mode 100644 gtsam/nonlinear/NonlinearOptimizerParameters.cpp create mode 100644 gtsam/nonlinear/NonlinearOptimizerParameters.h create mode 100644 gtsam/nonlinear/NonlinearOptimizerParams.cpp create mode 100644 gtsam/nonlinear/NonlinearOptimizerParams.h delete mode 100644 gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp delete mode 100644 gtsam/nonlinear/SuccessiveLinearizationOptimizer.h diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index ca2ea00b8..ec96063ee 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -104,7 +104,7 @@ int main(int argc, char** argv) { LevenbergMarquardtParams parameters; parameters.verbosity = NonlinearOptimizerParams::ERROR; parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; + parameters.linearSolverType = NonlinearSolverParams::CONJUGATE_GRADIENT; { parameters.iterativeParams = boost::make_shared(); diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 49159924d..5fc08921f 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -442,7 +442,7 @@ void runBatch() gttic_(Create_optimizer); GaussNewtonParams params; - params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY; + params.linearSolverType = NonlinearSolverParams::MULTIFRONTAL_CHOLESKY; GaussNewtonOptimizer optimizer(measurements, initial, params); gttoc_(Create_optimizer); double lastError; diff --git a/gtsam.h b/gtsam.h index 81094ac89..bc44bc543 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1761,9 +1761,9 @@ bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError); -#include -virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { - SuccessiveLinearizationParams(); +#include +virtual class NonlinearSolverParams : gtsam::NonlinearOptimizerParams { + NonlinearSolverParams(); string getLinearSolverType() const; @@ -1778,12 +1778,12 @@ virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams { }; #include -virtual class GaussNewtonParams : gtsam::SuccessiveLinearizationParams { +virtual class GaussNewtonParams : gtsam::NonlinearSolverParams { GaussNewtonParams(); }; #include -virtual class LevenbergMarquardtParams : gtsam::SuccessiveLinearizationParams { +virtual class LevenbergMarquardtParams : gtsam::NonlinearSolverParams { LevenbergMarquardtParams(); double getlambdaInitial() const; @@ -1798,7 +1798,7 @@ virtual class LevenbergMarquardtParams : gtsam::SuccessiveLinearizationParams { }; #include -virtual class DoglegParams : gtsam::SuccessiveLinearizationParams { +virtual class DoglegParams : gtsam::NonlinearSolverParams { DoglegParams(); double getDeltaInitial() const; diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index acb654ba5..a635efcd2 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -45,7 +45,7 @@ public: * To use it in nonlinear optimization, please see the following example * * LevenbergMarquardtParams parameters; - * parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT; + * parameters.linearSolverType = NonlinearSolverParams::CONJUGATE_GRADIENT; * parameters.iterativeParams = boost::make_shared(); * LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); * Values result = optimizer.optimize(); diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index 41028d018..8a43b4d96 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -18,7 +18,7 @@ #pragma once -#include +#include namespace gtsam { @@ -29,7 +29,7 @@ class DoglegOptimizer; * common to all nonlinear optimization algorithms. This class also contains * all of those parameters. */ -class GTSAM_EXPORT DoglegParams : public SuccessiveLinearizationParams { +class GTSAM_EXPORT DoglegParams : public NonlinearOptimizerParams { public: /** See DoglegParams::dlVerbosity */ enum VerbosityDL { @@ -46,7 +46,7 @@ public: virtual ~DoglegParams() {} virtual void print(const std::string& str = "") const { - SuccessiveLinearizationParams::print(str); + NonlinearOptimizerParams::print(str); std::cout << " deltaInitial: " << deltaInitial << "\n"; std::cout.flush(); } diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.cpp b/gtsam/nonlinear/GaussNewtonOptimizer.cpp index e0e66adab..823f3a6ac 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.cpp +++ b/gtsam/nonlinear/GaussNewtonOptimizer.cpp @@ -33,7 +33,7 @@ void GaussNewtonOptimizer::iterate() { GaussianFactorGraph::shared_ptr linear = graph_.linearize(current.values); // Solve Factor Graph - const VectorValues delta = solveGaussianFactorGraph(*linear, params_); + const VectorValues delta = solve(*linear, current.values, params_); // Maybe show output if(params_.verbosity >= NonlinearOptimizerParams::DELTA) delta.print("delta"); diff --git a/gtsam/nonlinear/GaussNewtonOptimizer.h b/gtsam/nonlinear/GaussNewtonOptimizer.h index 1d267071c..8b41a979c 100644 --- a/gtsam/nonlinear/GaussNewtonOptimizer.h +++ b/gtsam/nonlinear/GaussNewtonOptimizer.h @@ -18,7 +18,7 @@ #pragma once -#include +#include namespace gtsam { @@ -27,7 +27,7 @@ class GaussNewtonOptimizer; /** Parameters for Gauss-Newton optimization, inherits from * NonlinearOptimizationParams. */ -class GTSAM_EXPORT GaussNewtonParams : public SuccessiveLinearizationParams { +class GTSAM_EXPORT GaussNewtonParams : public NonlinearOptimizerParams { }; class GTSAM_EXPORT GaussNewtonState : public NonlinearOptimizerState { diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 3480c3171..035bae319 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -16,16 +16,14 @@ * @date Feb 26, 2012 */ -#include - +#include #include #include #include -#include -#include #include #include +#include using namespace std; @@ -62,7 +60,7 @@ std::string LevenbergMarquardtParams::verbosityLMTranslator(VerbosityLM value) c /* ************************************************************************* */ void LevenbergMarquardtParams::print(const std::string& str) const { - SuccessiveLinearizationParams::print(str); + NonlinearOptimizerParams::print(str); std::cout << " lambdaInitial: " << lambdaInitial << "\n"; std::cout << " lambdaFactor: " << lambdaFactor << "\n"; std::cout << " lambdaUpperBound: " << lambdaUpperBound << "\n"; @@ -110,7 +108,7 @@ void LevenbergMarquardtOptimizer::iterate() { // Try solving try { // Solve Damped Gaussian Factor Graph - const VectorValues delta = solveGaussianFactorGraph(dampedSystem, params_); + const VectorValues delta = solve(dampedSystem, state_.values, params_); if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.norm() << endl; if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta"); diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index c58598606..24e30922b 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -18,7 +18,7 @@ #pragma once -#include +#include namespace gtsam { @@ -29,7 +29,7 @@ class LevenbergMarquardtOptimizer; * common to all nonlinear optimization algorithms. This class also contains * all of those parameters. */ -class GTSAM_EXPORT LevenbergMarquardtParams : public SuccessiveLinearizationParams { +class GTSAM_EXPORT LevenbergMarquardtParams : public NonlinearOptimizerParams { public: /** See LevenbergMarquardtParams::lmVerbosity */ diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 3b5ec556e..52c1e1f08 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -16,52 +16,27 @@ * @date Jul 17, 2010 */ +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include #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() { @@ -114,6 +89,36 @@ const Values& NonlinearOptimizer::optimizeSafely() { } } +/* ************************************************************************* */ +VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg, + const Values& initial, const NonlinearOptimizerParams& params) const { + VectorValues delta; + if (params.isMultifrontal()) { + delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); + } else if (params.isSequential()) { + delta = gfg.eliminateSequential(*params.ordering, + params.getEliminationFunction())->optimize(); + } else if (params.isCG()) { + if (!params.iterativeParams) + throw std::runtime_error( + "NonlinearOptimizer::solve: cg parameter has to be assigned ..."); + if (boost::dynamic_pointer_cast( + params.iterativeParams)) { + SubgraphSolver solver(gfg, + dynamic_cast(*params.iterativeParams), + *params.ordering); + delta = solver.optimize(); + } else { + throw std::runtime_error( + "NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ..."); + } + } else { + throw std::runtime_error( + "NonlinearOptimizer::solve: Optimization parameter is invalid"); + } + return delta; +} + /* ************************************************************************* */ bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, @@ -155,6 +160,7 @@ bool checkConvergence(double relativeErrorTreshold, double absoluteErrorTreshold } return converged; } + /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 78ba79e9d..72c2ee0a0 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -19,55 +19,12 @@ #pragma once #include +#include namespace gtsam { class NonlinearOptimizer; -/** The common parameters for Nonlinear optimizers. Most optimizers - * deriving from NonlinearOptimizer also subclass the parameters. - */ -class GTSAM_EXPORT NonlinearOptimizerParams { -public: - /** See NonlinearOptimizerParams::verbosity */ - enum Verbosity { - SILENT, - ERROR, - VALUES, - DELTA, - LINEAR - }; - - size_t maxIterations; ///< The maximum iterations to stop iterating (default 100) - double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5) - double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) - double errorTol; ///< The maximum total error to stop iterating (default 0.0) - Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) - - NonlinearOptimizerParams() : - maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), - errorTol(0.0), verbosity(SILENT) {} - - virtual ~NonlinearOptimizerParams() {} - virtual void print(const std::string& str = "") const ; - - size_t getMaxIterations() const { return maxIterations; } - double getRelativeErrorTol() const { return relativeErrorTol; } - double getAbsoluteErrorTol() const { return absoluteErrorTol; } - double getErrorTol() const { return errorTol; } - std::string getVerbosity() const { return verbosityTranslator(verbosity); } - - void setMaxIterations(size_t value) { maxIterations = value; } - void setRelativeErrorTol(double value) { relativeErrorTol = value; } - void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; } - 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; -}; - /** * Base class for a nonlinear optimization state, including the current estimate * of the variable values, error, and number of iterations. Optimizers derived @@ -222,6 +179,10 @@ public: /** Virtual destructor */ virtual ~NonlinearOptimizer() {} + /** Default function to do linear solve, i.e. optimize a GaussianFactorGraph */ + virtual VectorValues solve(const GaussianFactorGraph &gfg, + const Values& initial, const NonlinearOptimizerParams& params) const; + /** Perform a single iteration, returning a new NonlinearOptimizer class * containing the updated variable assignments, which may be retrieved with * values(). diff --git a/gtsam/nonlinear/NonlinearOptimizerParameters.cpp b/gtsam/nonlinear/NonlinearOptimizerParameters.cpp new file mode 100644 index 000000000..0d4e03e76 --- /dev/null +++ b/gtsam/nonlinear/NonlinearOptimizerParameters.cpp @@ -0,0 +1 @@ +// moved to NonlinearOptimizerParams diff --git a/gtsam/nonlinear/NonlinearOptimizerParameters.h b/gtsam/nonlinear/NonlinearOptimizerParameters.h new file mode 100644 index 000000000..0d4e03e76 --- /dev/null +++ b/gtsam/nonlinear/NonlinearOptimizerParameters.h @@ -0,0 +1 @@ +// moved to NonlinearOptimizerParams diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp new file mode 100644 index 000000000..b1ec2124a --- /dev/null +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -0,0 +1,155 @@ +/** + * @file NonlinearOptimizerParams.cpp + * @brief Parameters for nonlinear optimization + * @date Jul 24, 2012 + * @author Yong-Dian Jian + * @author Richard Roberts + * @author Frank Dellaert + */ + +#include +#include + +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::setIterativeParams( + const SubgraphSolverParameters ¶ms) { + iterativeParams = boost::make_shared(params); +} + +/* ************************************************************************* */ +void NonlinearOptimizerParams::print(const std::string& str) const { + + //NonlinearOptimizerParams::print(str); + 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(); + + switch (linearSolverType) { + case MULTIFRONTAL_CHOLESKY: + std::cout << " linear solver type: MULTIFRONTAL CHOLESKY\n"; + break; + case MULTIFRONTAL_QR: + std::cout << " linear solver type: MULTIFRONTAL QR\n"; + break; + case SEQUENTIAL_CHOLESKY: + std::cout << " linear solver type: SEQUENTIAL CHOLESKY\n"; + break; + case SEQUENTIAL_QR: + std::cout << " linear solver type: SEQUENTIAL QR\n"; + break; + case CHOLMOD: + std::cout << " linear solver type: CHOLMOD\n"; + break; + case CONJUGATE_GRADIENT: + std::cout << " linear solver type: CONJUGATE GRADIENT\n"; + break; + default: + std::cout << " linear solver type: (invalid)\n"; + break; + } + + if (ordering) + std::cout << " ordering: custom\n"; + else + std::cout << " ordering: COLAMD\n"; + + std::cout.flush(); +} + +/* ************************************************************************* */ +std::string NonlinearOptimizerParams::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"); + } +} + +/* ************************************************************************* */ +NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::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 diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h new file mode 100644 index 000000000..6e0faa1f1 --- /dev/null +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -0,0 +1,165 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file NonlinearOptimizerParams.h + * @brief Parameters for nonlinear optimization + * @author Yong-Dian Jian + * @author Richard Roberts + * @author Frank Dellaert + * @date Apr 1, 2012 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** The common parameters for Nonlinear optimizers. Most optimizers + * deriving from NonlinearOptimizer also subclass the parameters. + */ +class GTSAM_EXPORT NonlinearOptimizerParams { +public: + /** See NonlinearOptimizerParams::verbosity */ + enum Verbosity { + SILENT, ERROR, VALUES, DELTA, LINEAR + }; + + size_t maxIterations; ///< The maximum iterations to stop iterating (default 100) + double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5) + double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) + double errorTol; ///< The maximum total error to stop iterating (default 0.0) + Verbosity verbosity; ///< The printing verbosity during optimization (default SILENT) + + NonlinearOptimizerParams() : + maxIterations(100), relativeErrorTol(1e-5), absoluteErrorTol(1e-5), errorTol( + 0.0), verbosity(SILENT), linearSolverType(MULTIFRONTAL_CHOLESKY) { + } + + virtual ~NonlinearOptimizerParams() { + } + virtual void print(const std::string& str = "") const; + + size_t getMaxIterations() const { + return maxIterations; + } + double getRelativeErrorTol() const { + return relativeErrorTol; + } + double getAbsoluteErrorTol() const { + return absoluteErrorTol; + } + double getErrorTol() const { + return errorTol; + } + std::string getVerbosity() const { + return verbosityTranslator(verbosity); + } + + void setMaxIterations(size_t value) { + maxIterations = value; + } + void setRelativeErrorTol(double value) { + relativeErrorTol = value; + } + void setAbsoluteErrorTol(double value) { + absoluteErrorTol = value; + } + 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; + + // Successive Linearization Parameters + +public: + + /** See NonlinearSolverParams::linearSolverType */ + + enum LinearSolverType { + MULTIFRONTAL_CHOLESKY, + MULTIFRONTAL_QR, + SEQUENTIAL_CHOLESKY, + SEQUENTIAL_QR, + CONJUGATE_GRADIENT, /* Experimental Flag */ + CHOLMOD, /* Experimental Flag */ + }; + + LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer + boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) + IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. + + inline bool isMultifrontal() const { + return (linearSolverType == MULTIFRONTAL_CHOLESKY) + || (linearSolverType == MULTIFRONTAL_QR); + } + + inline bool isSequential() const { + return (linearSolverType == SEQUENTIAL_CHOLESKY) + || (linearSolverType == SEQUENTIAL_QR); + } + + inline bool isCholmod() const { + return (linearSolverType == CHOLMOD); + } + + inline bool isCG() const { + return (linearSolverType == CONJUGATE_GRADIENT); + } + + GaussianFactorGraph::Eliminate getEliminationFunction() const { + switch (linearSolverType) { + case MULTIFRONTAL_CHOLESKY: + case SEQUENTIAL_CHOLESKY: + return EliminatePreferCholesky; + + case MULTIFRONTAL_QR: + case SEQUENTIAL_QR: + return EliminateQR; + + default: + throw std::runtime_error( + "Nonlinear optimization parameter \"factorization\" is invalid"); + } + } + + std::string getLinearSolverType() const { + return linearSolverTranslator(linearSolverType); + } + + void setLinearSolverType(const std::string& solver) { + linearSolverType = linearSolverTranslator(solver); + } + void setIterativeParams(const SubgraphSolverParameters& params); + void setOrdering(const Ordering& ordering) { + this->ordering = ordering; + } + +private: + std::string linearSolverTranslator(LinearSolverType linearSolverType) const; + LinearSolverType linearSolverTranslator( + const std::string& linearSolverType) const; +}; + +// For backward compatibility: +typedef NonlinearOptimizerParams SuccessiveLinearizationParams; + +} /* namespace gtsam */ diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp deleted file mode 100644 index 1533a96e9..000000000 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp +++ /dev/null @@ -1,79 +0,0 @@ -/** - * @file SuccessiveLinearizationOptimizer.cpp - * @brief - * @date Jul 24, 2012 - * @author Yong-Dian Jian - */ - -#include -#include -#include -#include -#include - -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 ) { - case MULTIFRONTAL_CHOLESKY: - std::cout << " linear solver type: MULTIFRONTAL CHOLESKY\n"; - break; - case MULTIFRONTAL_QR: - std::cout << " linear solver type: MULTIFRONTAL QR\n"; - break; - case SEQUENTIAL_CHOLESKY: - std::cout << " linear solver type: SEQUENTIAL CHOLESKY\n"; - break; - case SEQUENTIAL_QR: - std::cout << " linear solver type: SEQUENTIAL QR\n"; - break; - case CHOLMOD: - std::cout << " linear solver type: CHOLMOD\n"; - break; - case CONJUGATE_GRADIENT: - std::cout << " linear solver type: CONJUGATE GRADIENT\n"; - break; - default: - std::cout << " linear solver type: (invalid)\n"; - break; - } - - if(ordering) - std::cout << " ordering: custom\n"; - else - std::cout << " ordering: COLAMD\n"; - - std::cout.flush(); -} - -VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const SuccessiveLinearizationParams ¶ms) -{ - gttic(solveGaussianFactorGraph); - VectorValues delta; - if (params.isMultifrontal()) { - delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); - } else if(params.isSequential()) { - delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize(); - } - else if ( params.isCG() ) { - if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ..."); - if ( boost::dynamic_pointer_cast(params.iterativeParams) ) { - SubgraphSolver solver(gfg, dynamic_cast(*params.iterativeParams), *params.ordering); - delta = solver.optimize(); - } - else { - throw std::runtime_error("solveGaussianFactorGraph: special cg parameter type is not handled in LM solver ..."); - } - } - else { - throw std::runtime_error("solveGaussianFactorGraph: Optimization parameter is invalid"); - } - return delta; -} - -} diff --git a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h b/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h deleted file mode 100644 index 323fa88aa..000000000 --- a/gtsam/nonlinear/SuccessiveLinearizationOptimizer.h +++ /dev/null @@ -1,108 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file SuccessiveLinearizationOptimizer.h - * @brief - * @author Richard Roberts - * @date Apr 1, 2012 - */ - -#pragma once - -#include -#include -#include -#include - -namespace gtsam { - - // Forward declarations - -class GTSAM_EXPORT SuccessiveLinearizationParams : public NonlinearOptimizerParams { -public: - /** See SuccessiveLinearizationParams::linearSolverType */ - enum LinearSolverType { - MULTIFRONTAL_CHOLESKY, - MULTIFRONTAL_QR, - SEQUENTIAL_CHOLESKY, - SEQUENTIAL_QR, - CONJUGATE_GRADIENT, /* Experimental Flag */ - CHOLMOD, /* Experimental Flag */ - }; - - LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer - boost::optional ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) - IterativeOptimizationParameters::shared_ptr iterativeParams; ///< The container for iterativeOptimization parameters. used in CG Solvers. - - SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {} - virtual ~SuccessiveLinearizationParams() {} - - inline bool isMultifrontal() const { - return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); } - - inline bool isSequential() const { - return (linearSolverType == SEQUENTIAL_CHOLESKY) || (linearSolverType == SEQUENTIAL_QR); } - - inline bool isCholmod() const { return (linearSolverType == CHOLMOD); } - - inline bool isCG() const { return (linearSolverType == CONJUGATE_GRADIENT); } - - virtual void print(const std::string& str) const; - - GaussianFactorGraph::Eliminate getEliminationFunction() const { - switch (linearSolverType) { - case MULTIFRONTAL_CHOLESKY: - case SEQUENTIAL_CHOLESKY: - return EliminatePreferCholesky; - - case MULTIFRONTAL_QR: - case SEQUENTIAL_QR: - return EliminateQR; - - default: - throw std::runtime_error("Nonlinear optimization parameter \"factorization\" is invalid"); - } - } - - std::string getLinearSolverType() const { return linearSolverTranslator(linearSolverType); } - - void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); } - void setIterativeParams(const SubgraphSolverParameters& params); - 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"); - } -}; - -/* a wrapper for solving a GaussianFactorGraph according to the parameters */ -GTSAM_EXPORT VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const SuccessiveLinearizationParams ¶ms); - -} /* namespace gtsam */ diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index 8efbd5b97..b662b2bee 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -133,7 +133,7 @@ Point3 triangulateDLT(const std::vector& poses, params.absoluteErrorTol = 1.0; params.verbosityLM = LevenbergMarquardtParams::SILENT; params.verbosity = NonlinearOptimizerParams::SILENT; - params.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_CHOLESKY; + params.linearSolverType = NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY; LevenbergMarquardtOptimizer optimizer(graph, values, params); Values result = optimizer.optimize(); diff --git a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp index 68a0e1a45..2761e3af6 100644 --- a/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp +++ b/gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp @@ -244,7 +244,6 @@ ConcurrentBatchSmoother::Result ConcurrentBatchSmoother::optimize() { result.linearVariables = separatorValues_.size(); // Pull out parameters we'll use - const NonlinearOptimizerParams::Verbosity nloVerbosity = parameters_.verbosity; const LevenbergMarquardtParams::VerbosityLM lmVerbosity = parameters_.verbosityLM; double lambda = parameters_.lambdaInitial; diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp index 435965087..ae6c93ca0 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp @@ -58,7 +58,7 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int // parameters.lambdaInitial = 1; // parameters.verbosity = NonlinearOptimizerParams::ERROR; // parameters.verbosityLM = ISAM2Params::DAMPED; -// parameters.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_QR; +// parameters.linearSolverType = NonlinearSolverParams::MULTIFRONTAL_QR; // it is the same as the input graph, but we removed the empty factors that may be present in the input graph NonlinearFactorGraph graphForISAM2; diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp index a25b97550..d08618f47 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp @@ -58,7 +58,7 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int // parameters.lambdaInitial = 1; // parameters.verbosity = NonlinearOptimizerParams::ERROR; // parameters.verbosityLM = ISAM2Params::DAMPED; -// parameters.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_QR; +// parameters.linearSolverType = NonlinearSolverParams::MULTIFRONTAL_QR; ISAM2 optimizer(parameters); optimizer.update( graph, theta ); diff --git a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp index 73d2a0e9e..e485ef783 100644 --- a/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp +++ b/gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp @@ -57,7 +57,7 @@ Values BatchOptimize(const NonlinearFactorGraph& graph, const Values& theta, int // parameters.lambdaInitial = 1; // parameters.verbosity = NonlinearOptimizerParams::ERROR; // parameters.verbosityLM = ISAM2Params::DAMPED; -// parameters.linearSolverType = SuccessiveLinearizationParams::MULTIFRONTAL_QR; +// parameters.linearSolverType = NonlinearSolverParams::MULTIFRONTAL_QR; ISAM2 optimizer(parameters); optimizer.update( graph, theta ); diff --git a/tests/testGaussianFactorGraphB.cpp b/tests/testGaussianFactorGraphB.cpp index f3a27e794..51619f1f0 100644 --- a/tests/testGaussianFactorGraphB.cpp +++ b/tests/testGaussianFactorGraphB.cpp @@ -587,7 +587,7 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) { // Check that all sigmas in an unconstrained bayes tree are set to one BOOST_FOREACH(const GaussianBayesTree::sharedClique& clique, actBT.nodes() | br::map_values) { GaussianConditional::shared_ptr conditional = clique->conditional(); - size_t dim = conditional->rows(); + //size_t dim = conditional->rows(); //EXPECT(assert_equal(gtsam::ones(dim), conditional->get_model()->sigmas(), tol)); EXPECT(!conditional->get_model()); } diff --git a/tests/testNonlinearOptimizer.cpp b/tests/testNonlinearOptimizer.cpp index 72e6ae445..7737fdde0 100644 --- a/tests/testNonlinearOptimizer.cpp +++ b/tests/testNonlinearOptimizer.cpp @@ -302,6 +302,52 @@ TEST(NonlinearOptimizer, disconnected_graph) { EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(graph, init).optimize())); } +/* ************************************************************************* */ +#include + +class IterativeLM: public LevenbergMarquardtOptimizer { + + /// Solver specific parameters + ConjugateGradientParameters cgParams_; + +public: + /// Constructor + IterativeLM(const NonlinearFactorGraph& graph, const Values& initialValues, + const ConjugateGradientParameters &p, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()) : + LevenbergMarquardtOptimizer(graph, initialValues, params), cgParams_(p) { + } + + /// Solve that uses conjugate gradient + virtual VectorValues solve(const GaussianFactorGraph &gfg, + const Values& initial, const NonlinearOptimizerParams& params) const { + VectorValues zeros = initial.zeroVectors(); + return conjugateGradientDescent(gfg, zeros, cgParams_); + } +}; + +/* ************************************************************************* */ +TEST(NonlinearOptimizer, subclass_solver) { + Values expected; + expected.insert(X(1), Pose2(0.,0.,0.)); + expected.insert(X(2), Pose2(1.5,0.,0.)); + expected.insert(X(3), Pose2(3.0,0.,0.)); + + Values init; + init.insert(X(1), Pose2(0.,0.,0.)); + init.insert(X(2), Pose2(0.,0.,0.)); + init.insert(X(3), Pose2(0.,0.,0.)); + + NonlinearFactorGraph graph; + graph += PriorFactor(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph += BetweenFactor(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + graph += PriorFactor(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1)); + + ConjugateGradientParameters p; + Values actual = IterativeLM(graph, init, p).optimize(); + EXPECT(assert_equal(expected, actual, 1e-4)); +} + /* ************************************************************************* */ int main() { TestResult tr;