From b3bcf0d236287fa7ea796643873314dc51b90ea8 Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Mon, 25 Oct 2010 05:29:52 +0000 Subject: [PATCH] add a simple conjugate gradient solver with a structure to encapsulate all parameters for iterative methods --- linear/ConjugateGradientSolver-inl.h | 10 +++ linear/ConjugateGradientSolver.h | 58 +++++++++++++++ linear/GaussianFactorGraph.cpp | 20 +++++- linear/GaussianFactorGraph.h | 1 + linear/IterativeOptimizationParameters.h | 44 ++++++++++++ linear/IterativeSolver.h | 37 ++++++++++ linear/SubgraphSolver-inl.h | 87 ++++++++++++++++++++--- linear/SubgraphSolver.h | 90 ++++-------------------- linear/iterative-inl.h | 74 +++++++++---------- linear/iterative.cpp | 78 +++++++++++++------- linear/iterative.h | 24 ++----- nonlinear/NonlinearOptimization-inl.h | 32 ++++++++- nonlinear/NonlinearOptimization.h | 4 +- 13 files changed, 393 insertions(+), 166 deletions(-) create mode 100644 linear/ConjugateGradientSolver-inl.h create mode 100644 linear/ConjugateGradientSolver.h create mode 100644 linear/IterativeOptimizationParameters.h create mode 100644 linear/IterativeSolver.h diff --git a/linear/ConjugateGradientSolver-inl.h b/linear/ConjugateGradientSolver-inl.h new file mode 100644 index 000000000..cc3b33079 --- /dev/null +++ b/linear/ConjugateGradientSolver-inl.h @@ -0,0 +1,10 @@ +/* + * ConjugateGradientSolver-inl.h + * + * Created on: Oct 24, 2010 + * Author: Yong-Dian Jian + */ + +#pragma once + +#include diff --git a/linear/ConjugateGradientSolver.h b/linear/ConjugateGradientSolver.h new file mode 100644 index 000000000..83a614a98 --- /dev/null +++ b/linear/ConjugateGradientSolver.h @@ -0,0 +1,58 @@ +/* + * ConjugateGradientSolver.h + * + * Created on: Oct 21, 2010 + * Author: Yong-Dian Jian + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + + // GaussianFactorGraph, Values + template + class ConjugateGradientSolver : public IterativeSolver { + + protected: + const LINEAR *ptr_; + + typedef boost::shared_ptr sharedVectorValues ; + sharedVectorValues zeros_; + + public: + + typedef boost::shared_ptr shared_ptr ; + + ConjugateGradientSolver(const GRAPH &graph, const VALUES &initial, const Ordering &ordering, const sharedParameters parameters): + IterativeSolver(parameters), ptr_(0), zeros_(boost::make_shared(initial.zero(ordering))) {} + + ConjugateGradientSolver(const LINEAR &GFG) { + throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported"); + } + + ConjugateGradientSolver(const ConjugateGradientSolver& solver) : IterativeSolver(solver), ptr_(solver.ptr_), zeros_(solver.zeros_){} + ConjugateGradientSolver(const sharedParameters parameters, const LINEAR *ptr, const sharedVectorValues zeros): + IterativeSolver(parameters), ptr_(ptr), zeros_(zeros) {} + + shared_ptr update(const LINEAR &graph) const { + return boost::make_shared(parameters_, &graph, zeros_) ; + } + + VectorValues::shared_ptr optimize() const { + //boost::shared_ptr zeros (ptr_->allocateVectorVavlues()); + VectorValues x = conjugateGradientDescent(*ptr_, *zeros_, parameters_); + return boost::make_shared(x) ; + } + + private: + ConjugateGradientSolver():IterativeSolver(),ptr_(0){} + + }; + +} // nsamespace gtsam diff --git a/linear/GaussianFactorGraph.cpp b/linear/GaussianFactorGraph.cpp index 277339c46..b9ba766b3 100644 --- a/linear/GaussianFactorGraph.cpp +++ b/linear/GaussianFactorGraph.cpp @@ -16,7 +16,10 @@ * @author Christian Potthast */ +#include + #include +#include #include #include #include @@ -165,12 +168,12 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const vector &M, GaussianFactorGraph &Ab1, GaussianFactorGraph &Ab2) const { - typedef sharedFactor F ; + //typedef sharedFactor F ; Ab1 = GaussianFactorGraph(); Ab2 = GaussianFactorGraph(); - BOOST_FOREACH(const F& factor, *this) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { if (factor->keys().size() > 2) throw(invalid_argument("split: only support factors with at most two keys")); @@ -192,5 +195,18 @@ bool GaussianFactorGraph::split(const std::map &M, GaussianFactorG return true ; } +boost::shared_ptr GaussianFactorGraph::allocateVectorVavlues() const { + std::vector dimensions(size()) ; + BOOST_FOREACH( const sharedFactor& factor, factors_) { + Index i = 0 ; + BOOST_FOREACH( const Index& idx, factor->keys_) { + dimensions[idx] = factor->Ab_(i).size2() ; + i++; + } + } + + return boost::make_shared(dimensions) ; +} + } // namespace gtsam diff --git a/linear/GaussianFactorGraph.h b/linear/GaussianFactorGraph.h index cfb37912e..e8ebe76a8 100644 --- a/linear/GaussianFactorGraph.h +++ b/linear/GaussianFactorGraph.h @@ -158,6 +158,7 @@ namespace gtsam { * M keeps the vertex indices of edges of A1. The others belong to A2. */ bool split(const std::map &M, GaussianFactorGraph &A1, GaussianFactorGraph &A2) const ; + boost::shared_ptr allocateVectorVavlues() const ; }; diff --git a/linear/IterativeOptimizationParameters.h b/linear/IterativeOptimizationParameters.h new file mode 100644 index 000000000..97ea92577 --- /dev/null +++ b/linear/IterativeOptimizationParameters.h @@ -0,0 +1,44 @@ +/* + * IterativeOptimizationParameters.h + * + * Created on: Oct 22, 2010 + * Author: Yong-Dian Jian + */ + +#pragma once + +namespace gtsam { + + // a container for all related parameters + struct IterativeOptimizationParameters { + typedef enum { + SILENT, + ERROR, + } verbosityLevel; + + IterativeOptimizationParameters(): + maxIterations_(100), + reset_(101), + epsilon_(1e-5),epsilon_abs_(1e-5),verbosity_(SILENT) {} + + IterativeOptimizationParameters + (int maxIterations, double epsilon, double epsilon_abs, verbosityLevel verbosity=SILENT, int reset=-1): + maxIterations_(maxIterations), reset_(reset), + epsilon_(epsilon), epsilon_abs_(epsilon_abs), verbosity_(verbosity) { + if (reset_==-1) reset_ = maxIterations_ + 1 ; + } + + int maxIterations() const { return maxIterations_; } + int reset() const { return reset_; } + double epsilon() const { return epsilon_ ;} + double epsilon_abs() const { return epsilon_abs_ ; } + verbosityLevel verbosity() const { return verbosity_ ; } + + protected: + int maxIterations_; + int reset_ ; // number of iterations before reset, for cg and gmres + double epsilon_; // relative error + double epsilon_abs_; // absolute error + verbosityLevel verbosity_; + }; +} diff --git a/linear/IterativeSolver.h b/linear/IterativeSolver.h new file mode 100644 index 000000000..2d45e2f78 --- /dev/null +++ b/linear/IterativeSolver.h @@ -0,0 +1,37 @@ +/* + * IterativeSolver.h + * + * Created on: Oct 24, 2010 + * Author: Yong-Dian Jian + * + * Base Class for all iterative solvers of linear systems + */ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + +class IterativeSolver { + +public: + typedef IterativeOptimizationParameters Parameters; + typedef boost::shared_ptr sharedParameters; + + sharedParameters parameters_ ; + + IterativeSolver(): + parameters_(new IterativeOptimizationParameters()) {} + + IterativeSolver(const IterativeSolver &solver): + parameters_(solver.parameters_) {} + + IterativeSolver(const sharedParameters parameters): + parameters_(parameters) {} +}; + +} diff --git a/linear/SubgraphSolver-inl.h b/linear/SubgraphSolver-inl.h index e5a1890e5..8242854ad 100644 --- a/linear/SubgraphSolver-inl.h +++ b/linear/SubgraphSolver-inl.h @@ -9,18 +9,87 @@ * -------------------------------------------------------------------------- */ -/* - * SubgraphSolver-inl.h - * - * Created on: Jan 17, 2010 - * Author: nikai - * Description: subgraph preconditioning conjugate gradient solver - */ - #pragma once +#include +#include + #include +#include +#include +#include +#include +#include + using namespace std; -namespace gtsam {} +namespace gtsam { + +template +typename SubgraphSolver::shared_ptr +SubgraphSolver::update(const LINEAR &graph) const { + + shared_linear Ab1 = boost::make_shared(), + Ab2 = boost::make_shared(); + + if (parameters_->verbosity()) cout << "split the graph ..."; + graph.split(pairs_, *Ab1, *Ab2) ; + if (parameters_->verbosity()) cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl; + + // // Add a HardConstra int to the root, otherwise the root will be singular + // Key root = keys.back(); + // T_.addHardConstraint(root, theta0[root]); + // + // // compose the approximate solution + // theta_bar_ = composePoses (T_, tree, theta0[root]); + + LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!! + SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree::Create(sacrificialAb1)->eliminate(); + SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1); + + shared_preconditioner pc = boost::make_shared(Ab1,Ab2,Rc1,xbar); + return boost::make_shared(ordering_, pairs_, pc, parameters_) ; +} + +template +VectorValues::shared_ptr SubgraphSolver::optimize() const { + + // preconditioned conjugate gradient + VectorValues zeros = pc_->zero(); + VectorValues ybar = conjugateGradients + (*pc_, zeros, parameters_); + + boost::shared_ptr xbar = boost::make_shared() ; + *xbar = pc_->x(ybar); + return xbar; +} + +template +void SubgraphSolver::initialize(const GRAPH& G, const VALUES& theta0) { + // generate spanning tree + PredecessorMap tree_ = gtsam::findMinimumSpanningTree(G); + + // make the ordering + list keys = predecessorMap2Keys(tree_); + ordering_ = boost::make_shared(list(keys.begin(), keys.end())); + + // build factor pairs + pairs_.clear(); + typedef pair EG ; + BOOST_FOREACH( const EG &eg, tree_ ) { + Symbol key1 = Symbol(eg.first), + key2 = Symbol(eg.second) ; + pairs_.insert(pair((*ordering_)[key1], (*ordering_)[key2])) ; + } + } + + + + + + + + + +} diff --git a/linear/SubgraphSolver.h b/linear/SubgraphSolver.h index da763a785..364cb97ef 100644 --- a/linear/SubgraphSolver.h +++ b/linear/SubgraphSolver.h @@ -11,17 +11,12 @@ #pragma once -#include -#include + #include #include -#include -#include -#include -#include +#include #include -#include #include namespace gtsam { @@ -33,7 +28,7 @@ namespace gtsam { * solve : L -> VectorValues */ template - class SubgraphSolver { + class SubgraphSolver : public IterativeSolver { private: typedef typename VALUES::Key Key; @@ -48,11 +43,6 @@ namespace gtsam { typedef boost::shared_ptr shared_preconditioner ; typedef std::map mapPairIndex ; - // TODO not hardcode - static const size_t maxIterations_=100; - static const double epsilon_=1e-4, epsilon_abs_=1e-5; - static const bool verbose_=true; - /* the ordering derived from the spanning tree */ shared_ordering ordering_; @@ -63,86 +53,36 @@ namespace gtsam { shared_preconditioner pc_; public: - SubgraphSolver(){} SubgraphSolver(const LINEAR &GFG) { throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported"); } SubgraphSolver(const SubgraphSolver& solver) : - ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_){} + IterativeSolver(solver), ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_){} SubgraphSolver(shared_ordering ordering, mapPairIndex pairs, - shared_preconditioner pc) : - ordering_(ordering), pairs_(pairs), pc_(pc) {} + shared_preconditioner pc, + sharedParameters parameters = boost::make_shared()) : + IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc) {} - - SubgraphSolver(const GRAPH& G, const VALUES& theta0) { initialize(G,theta0); } - - shared_ptr update(const LINEAR &graph) const { - - shared_linear Ab1 = boost::make_shared(), - Ab2 = boost::make_shared(); - - if (verbose_) cout << "split the graph ..."; - graph.split(pairs_, *Ab1, *Ab2) ; - if (verbose_) cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl; - - // // Add a HardConstra int to the root, otherwise the root will be singular - // Key root = keys.back(); - // T_.addHardConstraint(root, theta0[root]); - // - // // compose the approximate solution - // theta_bar_ = composePoses (T_, tree, theta0[root]); - - LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!! - SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree::Create(sacrificialAb1)->eliminate(); - SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1); - - shared_preconditioner pc = boost::make_shared(Ab1,Ab2,Rc1,xbar); - return boost::make_shared(ordering_, pairs_, pc) ; - } - - VectorValues::shared_ptr optimize() const { - - // preconditioned conjugate gradient - VectorValues zeros = pc_->zero(); - VectorValues ybar = conjugateGradients (*pc_, zeros, verbose_, epsilon_, epsilon_abs_, maxIterations_); - - boost::shared_ptr xbar = boost::make_shared() ; - *xbar = pc_->x(ybar); - return xbar; + SubgraphSolver(const GRAPH& G, const VALUES& theta0):IterativeSolver(){ + initialize(G,theta0); } + shared_ptr update(const LINEAR &graph) const ; + VectorValues::shared_ptr optimize() const ; shared_ordering ordering() const { return ordering_; } + protected: - void initialize(const GRAPH& G, const VALUES& theta0) { - // generate spanning tree - PredecessorMap tree_ = gtsam::findMinimumSpanningTree(G); + void initialize(const GRAPH& G, const VALUES& theta0); - // make the ordering - list keys = predecessorMap2Keys(tree_); - ordering_ = boost::make_shared(list(keys.begin(), keys.end())); - - // build factor pairs - pairs_.clear(); - typedef pair EG ; - BOOST_FOREACH( const EG &eg, tree_ ) { - Symbol key1 = Symbol(eg.first), - key2 = Symbol(eg.second) ; - pairs_.insert(pair((*ordering_)[key1], (*ordering_)[key2])) ; - } - } + private: + SubgraphSolver():IterativeSolver(){} }; - template const size_t SubgraphSolver::maxIterations_; - template const bool SubgraphSolver::verbose_; - template const double SubgraphSolver::epsilon_; - template const double SubgraphSolver::epsilon_abs_; - } // nsamespace gtsam diff --git a/linear/iterative-inl.h b/linear/iterative-inl.h index a90c235a6..c009a9e64 100644 --- a/linear/iterative-inl.h +++ b/linear/iterative-inl.h @@ -18,6 +18,8 @@ #pragma once +#include +#include #include #include @@ -30,21 +32,21 @@ namespace gtsam { template struct CGState { - bool steepest, verbose; - double gamma, threshold; - size_t k, maxIterations, reset; + typedef IterativeSolver::Parameters Parameters; + typedef IterativeSolver::sharedParameters sharedParameters; + + const sharedParameters parameters_; + + int k; + bool steepest; V g, d; + double gamma, threshold; E Ad; /* ************************************************************************* */ // Constructor - CGState(const S& Ab, const V& x, bool verb, double epsilon, - double epsilon_abs, size_t maxIt, bool steep) { - k = 0; - verbose = verb; - steepest = steep; - maxIterations = (maxIt > 0) ? maxIt : dim(x) * (steepest ? 10 : 1); - reset = (size_t) (sqrt(dim(x)) + 0.5); // when to reset + CGState(const S& Ab, const V& x, const sharedParameters parameters, bool steep): + parameters_(parameters),k(0),steepest(steep) { // Start with g0 = A'*(A*x0-b), d0 = - g0 // i.e., first step is in direction of negative gradient @@ -53,10 +55,10 @@ namespace gtsam { // init gamma and calculate threshold gamma = dot(g,g) ; - threshold = ::max(epsilon_abs, epsilon * epsilon * gamma); + threshold = ::max(parameters->epsilon_abs(), parameters->epsilon() * parameters->epsilon() * gamma); // Allocate and calculate A*d for first iteration - if (gamma > epsilon) Ad = Ab * d; + if (gamma > parameters->epsilon_abs()) Ad = Ab * d; } /* ************************************************************************* */ @@ -82,34 +84,32 @@ namespace gtsam { /* ************************************************************************* */ // take a step, return true if converged bool step(const S& Ab, V& x) { - k += 1; // increase iteration number + if ((++k) >= parameters_->maxIterations()) return true; + //----------------------------------> double alpha = takeOptimalStep(x); - if (k >= maxIterations) return true; //----------------------------------> - // update gradient (or re-calculate at reset time) - if (k % reset == 0) - g = Ab.gradient(x); - else - // axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad) - Ab.transposeMultiplyAdd(alpha, Ad, g); + if (k % parameters_->reset() == 0) g = Ab.gradient(x); + // axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad) + else Ab.transposeMultiplyAdd(alpha, Ad, g); // check for convergence double new_gamma = dot(g, g); - if (verbose) cout << "iteration " << k << ": alpha = " << alpha - << ", dotg = " << new_gamma << endl; - if (new_gamma < threshold) return true; //----------------------------------> + if (parameters_->verbosity()) + cout << "iteration " << k << ": alpha = " << alpha + << ", dotg = " << new_gamma << endl; + if (new_gamma < threshold) return true; // calculate new search direction - if (steepest) - d = g; + if (steepest) d = g; else { double beta = new_gamma / gamma; // d = g + d*beta; scal(beta, d); axpy(1.0, g, d); } + gamma = new_gamma; // In-place recalculation Ad <- A*d to avoid re-allocating Ad @@ -123,23 +123,25 @@ namespace gtsam { // conjugate gradient method. // S: linear system, V: step vector, E: errors template - V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon, - double epsilon_abs, size_t maxIterations, bool steepest = false) { + V conjugateGradients( + const S& Ab, V x, + const IterativeSolver::sharedParameters parameters, + bool steepest = false) { - CGState state(Ab, x, verbose, epsilon, epsilon_abs, maxIterations,steepest); - if (verbose) cout << "CG: epsilon = " << epsilon << ", maxIterations = " - << state.maxIterations << ", ||g0||^2 = " << state.gamma - << ", threshold = " << state.threshold << endl; + CGState state(Ab, x, parameters, steepest); + if (parameters->verbosity()) + cout << "CG: epsilon = " << parameters->epsilon() + << ", maxIterations = " << parameters->maxIterations() + << ", ||g0||^2 = " << state.gamma + << ", threshold = " << state.threshold << endl; - if (state.gamma < state.threshold) { - if (verbose) cout << "||g0||^2 < threshold, exiting immediately !" << endl; + if ( state.gamma < state.threshold ) { + if (parameters->verbosity()) cout << "||g0||^2 < threshold, exiting immediately !" << endl; return x; } // loop maxIterations times - while (!state.step(Ab, x)) - ; - + while (!state.step(Ab, x)) {} return x; } diff --git a/linear/iterative.cpp b/linear/iterative.cpp index 157eb99ff..d103f8944 100644 --- a/linear/iterative.cpp +++ b/linear/iterative.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include using namespace std; @@ -34,46 +35,75 @@ namespace gtsam { } /* ************************************************************************* */ - Vector steepestDescent(const System& Ab, const Vector& x, bool verbose, - double epsilon, double epsilon_abs, size_t maxIterations) { - return conjugateGradients (Ab, x, verbose, epsilon, - epsilon_abs, maxIterations, true); + + Vector steepestDescent(const System& Ab, const Vector& x, const IterativeSolver::sharedParameters parameters) { + return conjugateGradients (Ab, x, parameters, true); } - Vector conjugateGradientDescent(const System& Ab, const Vector& x, - bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) { - return conjugateGradients (Ab, x, verbose, epsilon, - epsilon_abs, maxIterations); + Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeSolver::sharedParameters parameters) { + return conjugateGradients (Ab, x, parameters); } /* ************************************************************************* */ - Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, - bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) { + Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::sharedParameters parameters) { System Ab(A, b); - return conjugateGradients (Ab, x, verbose, epsilon, - epsilon_abs, maxIterations, true); + return conjugateGradients (Ab, x, parameters, true); } - Vector conjugateGradientDescent(const Matrix& A, const Vector& b, - const Vector& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) { + Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::sharedParameters parameters) { System Ab(A, b); - return conjugateGradients (Ab, x, verbose, epsilon, - epsilon_abs, maxIterations); + return conjugateGradients (Ab, x, parameters); } /* ************************************************************************* */ - VectorValues steepestDescent(const GaussianFactorGraph& fg, - const VectorValues& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) { - return conjugateGradients (fg, - x, verbose, epsilon, epsilon_abs, maxIterations, true); + VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters) { + return conjugateGradients (fg, x, parameters, true); } - VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, - const VectorValues& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) { - return conjugateGradients (fg, - x, verbose, epsilon, epsilon_abs, maxIterations); + VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters) { + return conjugateGradients (fg, x, parameters); } +// Vector steepestDescent(const System& Ab, const Vector& x, bool verbose, +// double epsilon, double epsilon_abs, size_t maxIterations) { +// return conjugateGradients (Ab, x, verbose, epsilon, +// epsilon_abs, maxIterations, true); +// } +// +// Vector conjugateGradientDescent(const System& Ab, const Vector& x, +// bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) { +// return conjugateGradients (Ab, x, verbose, epsilon, +// epsilon_abs, maxIterations); +// } +// +// /* ************************************************************************* */ +// Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, +// bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) { +// System Ab(A, b); +// return conjugateGradients (Ab, x, verbose, epsilon, +// epsilon_abs, maxIterations, true); +// } +// +// Vector conjugateGradientDescent(const Matrix& A, const Vector& b, +// const Vector& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) { +// System Ab(A, b); +// return conjugateGradients (Ab, x, verbose, epsilon, +// epsilon_abs, maxIterations); +// } +// +// /* ************************************************************************* */ +// VectorValues steepestDescent(const GaussianFactorGraph& fg, +// const VectorValues& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) { +// return conjugateGradients (fg, +// x, verbose, epsilon, epsilon_abs, maxIterations, true); +// } +// +// VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, +// const VectorValues& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) { +// return conjugateGradients (fg, +// x, verbose, epsilon, epsilon_abs, maxIterations); +// } + /* ************************************************************************* */ } // namespace gtsam diff --git a/linear/iterative.h b/linear/iterative.h index fd24875b7..e6ab2f074 100644 --- a/linear/iterative.h +++ b/linear/iterative.h @@ -20,6 +20,7 @@ #include #include +#include namespace gtsam { @@ -87,46 +88,35 @@ namespace gtsam { /** * Method of steepest gradients, System version */ - Vector steepestDescent(const System& Ab, const Vector& x, bool verbose = - false, double epsilon = 1e-3, double epsilon_abs = 1e-5, size_t maxIterations = 0); + Vector steepestDescent(const System& Ab, const Vector& x,const IterativeSolver::sharedParameters parameters); /** * Method of conjugate gradients (CG), System version */ - Vector conjugateGradientDescent(const System& Ab, const Vector& x, - bool verbose = false, double epsilon = 1e-3, double epsilon_abs = 1e-5, - size_t maxIterations = 0); + Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeSolver::sharedParameters parameters); /** convenience calls using matrices, will create System class internally: */ /** * Method of steepest gradients, Matrix version */ - Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, - bool verbose = false, double epsilon = 1e-3, double epsilon_abs = 1e-5, - size_t maxIterations = 0); + Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x,const IterativeSolver::sharedParameters parameters); /** * Method of conjugate gradients (CG), Matrix version */ - Vector conjugateGradientDescent(const Matrix& A, const Vector& b, - const Vector& x, bool verbose = false, double epsilon = 1e-3, - double epsilon_abs = 1e-5, size_t maxIterations = 0); + Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x,const IterativeSolver::sharedParameters parameters); class GaussianFactorGraph; /** * Method of steepest gradients, Gaussian Factor Graph version * */ - VectorValues steepestDescent(const GaussianFactorGraph& fg, - const VectorValues& x, bool verbose = false, double epsilon = 1e-3, - double epsilon_abs = 1e-5, size_t maxIterations = 0); + VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters); /** * Method of conjugate gradients (CG), Gaussian Factor Graph version * */ - VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, - const VectorValues& x, bool verbose = false, double epsilon = 1e-3, - double epsilon_abs = 1e-5, size_t maxIterations = 0); + VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters); } // namespace gtsam diff --git a/nonlinear/NonlinearOptimization-inl.h b/nonlinear/NonlinearOptimization-inl.h index c2e243ca5..91924a6f3 100644 --- a/nonlinear/NonlinearOptimization-inl.h +++ b/nonlinear/NonlinearOptimization-inl.h @@ -19,8 +19,11 @@ #pragma once +#include + #include #include +#include #include #include #include @@ -67,6 +70,31 @@ namespace gtsam { return *result.values(); } + /** + * The cg solver + */ + template + T optimizeCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) { + + typedef ConjugateGradientSolver Solver; + typedef boost::shared_ptr sharedSolver; + typedef NonlinearOptimizer Optimizer; + + Ordering::shared_ptr ordering = initialEstimate.orderingArbitrary() ; + sharedSolver solver = boost::make_shared(graph, initialEstimate, *ordering, boost::make_shared()); + Optimizer optimizer( + boost::make_shared(graph), + boost::make_shared(initialEstimate), + ordering, + solver); + + // Levenberg-Marquardt + Optimizer result = optimizer.levenbergMarquardt(parameters); + return *result.values(); + } + + + /** * The sparse preconditioned conjucate gradient solver */ @@ -100,10 +128,10 @@ namespace gtsam { return optimizeSequential(graph, initialEstimate, parameters); case MULTIFRONTAL: return optimizeMultiFrontal(graph, initialEstimate, parameters); + case CG: + return optimizeCG(graph, initialEstimate, parameters); case SPCG: throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint"); - - break; } throw runtime_error("optimize: undefined solver"); } diff --git a/nonlinear/NonlinearOptimization.h b/nonlinear/NonlinearOptimization.h index 4312760f1..e4107dd24 100644 --- a/nonlinear/NonlinearOptimization.h +++ b/nonlinear/NonlinearOptimization.h @@ -37,6 +37,7 @@ namespace gtsam { enum LinearSolver{ SEQUENTIAL, // Sequential elimination MULTIFRONTAL, // Multi-frontal elimination + CG, // vanilla conjugate gradient SPCG, // Subgraph Preconditioned Conjugate Gradient }; @@ -45,7 +46,8 @@ namespace gtsam { * optimization that returns the values */ template - T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(), + T optimize(const G& graph, const T& initialEstimate, + const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(), const enum LinearSolver& solver = MULTIFRONTAL); }