From b53bcc7d66fa57f3a70fe273dd4af3101f15243f Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Thu, 28 Oct 2010 03:26:03 +0000 Subject: [PATCH] add preconditioned conjugate gradient --- examples/Makefile.am | 1 + examples/Pose2SLAMwCG.cpp | 84 ++++++++++ gtsam/base/Matrix.cpp | 11 ++ gtsam/base/Matrix.h | 2 + gtsam/linear/ConjugateGradientSolver.h | 85 ++++++++++- gtsam/linear/GaussianFactorGraph.cpp | 69 ++++++++- gtsam/linear/GaussianFactorGraph.h | 12 +- .../linear/IterativeOptimizationParameters.h | 4 +- gtsam/linear/Preconditioner.h | 88 +++++++++++ gtsam/linear/SubgraphSolver-inl.h | 2 +- gtsam/linear/VectorValues.h | 16 ++ gtsam/linear/iterative-inl.h | 143 ++++++++++++++++-- gtsam/linear/iterative.cpp | 54 +------ gtsam/linear/iterative.h | 34 ++++- gtsam/nonlinear/NonlinearOptimization-inl.h | 21 +++ gtsam/nonlinear/NonlinearOptimizer-inl.h | 3 +- 16 files changed, 543 insertions(+), 86 deletions(-) create mode 100644 examples/Pose2SLAMwCG.cpp create mode 100644 gtsam/linear/Preconditioner.h diff --git a/examples/Makefile.am b/examples/Makefile.am index 3f7de2b33..cbc24d00e 100644 --- a/examples/Makefile.am +++ b/examples/Makefile.am @@ -16,6 +16,7 @@ noinst_PROGRAMS += PlanarSLAMExample_easy # Solves SLAM example from tutorial noinst_PROGRAMS += PlanarSLAMSelfContained_advanced # Solves SLAM example from tutorial with all typedefs in the script noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface +noinst_PROGRAMS += Pose2SLAMwCG # Solves a simple Pose2 SLAM example with easy CG solver noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver SUBDIRS = vSLAMexample # visual SLAM examples with 3D point landmarks and 6D camera poses diff --git a/examples/Pose2SLAMwCG.cpp b/examples/Pose2SLAMwCG.cpp new file mode 100644 index 000000000..2d2aa0a30 --- /dev/null +++ b/examples/Pose2SLAMwCG.cpp @@ -0,0 +1,84 @@ +/* + * Pose2SLAMwCG.cpp + * + * Created on: Oct 27, 2010 + * Author: Yong-Dian Jian + */ + +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ + +/* + * Solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface + */ + + +#include + +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace pose2SLAM; + +Graph graph; +Values initial; +Values result; + +/* ************************************************************************* */ +int main(void) { + + /* generate synthetic data */ + const SharedGaussian sigma(noiseModel::Unit::Create(0.1)); + Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9); + + // create a 3 by 3 grid + // x3 x6 x9 + // x2 x5 x8 + // x1 x4 x7 + graph.addConstraint(x1,x2,Pose2(0,2,0),sigma) ; + graph.addConstraint(x2,x3,Pose2(0,2,0),sigma) ; + graph.addConstraint(x4,x5,Pose2(0,2,0),sigma) ; + graph.addConstraint(x5,x6,Pose2(0,2,0),sigma) ; + graph.addConstraint(x7,x8,Pose2(0,2,0),sigma) ; + graph.addConstraint(x8,x9,Pose2(0,2,0),sigma) ; + graph.addConstraint(x1,x4,Pose2(2,0,0),sigma) ; + graph.addConstraint(x4,x7,Pose2(2,0,0),sigma) ; + graph.addConstraint(x2,x5,Pose2(2,0,0),sigma) ; + graph.addConstraint(x5,x8,Pose2(2,0,0),sigma) ; + graph.addConstraint(x3,x6,Pose2(2,0,0),sigma) ; + graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ; + graph.addPrior(x1, Pose2(0,0,0), sigma) ; + + initial.insert(x1, Pose2( 0, 0, 0)); + initial.insert(x2, Pose2( 0, 2.1, 0.01)); + initial.insert(x3, Pose2( 0, 3.9,-0.01)); + initial.insert(x4, Pose2(2.1,-0.1, 0)); + initial.insert(x5, Pose2(1.9, 2.1, 0.02)); + initial.insert(x6, Pose2(2.0, 3.9,-0.02)); + initial.insert(x7, Pose2(4.0, 0.1, 0.03 )); + initial.insert(x8, Pose2(3.9, 2.1, 0.01)); + initial.insert(x9, Pose2(4.1, 3.9,-0.01)); + /* done */ + + + graph.print("full graph") ; + initial.print("initial estimate"); + + result = optimizePCG >(graph, initial); + //result = optimizeCG(graph, initial); + result.print("final result") ; + return 0 ; +} + diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index e3f05a8d6..a94c7f8d4 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -399,6 +399,17 @@ void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j) { colsubproxy = col; } +/* ************************************************************************* */ + +Vector columnNormSquare(const MatrixColMajor &A) { + Vector v (A.size2()) ; + for ( size_t i = 0 ; i < A.size2() ; ++i ) { + ublas::matrix_column mc (A, i); + v[i] = dot(mc, mc) ; + } + return v ; +} + /* ************************************************************************* */ void solve(Matrix& A, Matrix& B) { diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 378c15b07..c78d01f3a 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -205,6 +205,8 @@ Vector column_(const Matrix& A, size_t j); void insertColumn(Matrix& A, const Vector& col, size_t j); void insertColumn(Matrix& A, const Vector& col, size_t i, size_t j); +Vector columnNormSquare(const MatrixColMajor &A) ; + /** * extracts a row from a matrix * @param matrix to extract row from diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index cbbe7d938..2c8d70134 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -15,7 +15,6 @@ namespace gtsam { - // GaussianFactorGraph, Values template class ConjugateGradientSolver : public IterativeSolver { @@ -33,30 +32,104 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr ; + // suggested constructor ConjugateGradientSolver(const GRAPH &graph, const VALUES &initial, const Ordering &ordering, const Parameters ¶meters = Parameters()): IterativeSolver(parameters), ptr_(0), zeros_(boost::make_shared(initial.zero(ordering))) {} + // to be compatible to NonlinearOptimizer, but shouldn't be called... ConjugateGradientSolver(const LINEAR &GFG) { std::cout << "[ConjugateGradientSolver] Unexpected usage.." << std::endl; throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported"); } + // copy constructor 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 { - VectorValues x = conjugateGradientDescent(*ptr_, *zeros_, parameters_); + VectorValues x = conjugateGradientDescent(*ptr_, *zeros_, *parameters_); return boost::make_shared(x) ; } - private: - ConjugateGradientSolver():IterativeSolver(),ptr_(0){} + // for NonlinearOptimizer update only + ConjugateGradientSolver(const sharedParameters parameters, + const LINEAR *ptr, + const sharedVectorValues zeros): + IterativeSolver(parameters), ptr_(ptr), zeros_(zeros) {} + + private: + // shouldn't be used + ConjugateGradientSolver(){} }; + template + class PreconditionedConjugateGradientSolver : public IterativeSolver { + + protected: + + typedef boost::shared_ptr sharedGRAPH ; + typedef boost::shared_ptr sharedLINEAR ; + typedef boost::shared_ptr sharedPreconditioner ; + typedef boost::shared_ptr sharedVALUES ; + typedef boost::shared_ptr sharedVectorValues ; + + const LINEAR *ptr_; + sharedPreconditioner pc_ ; + sharedVectorValues zeros_; + public: + + typedef boost::shared_ptr shared_ptr ; + + // suggested constructor + PreconditionedConjugateGradientSolver( + const GRAPH &graph, + const VALUES &initial, + const Ordering &ordering, + const Parameters ¶meters = Parameters()): + IterativeSolver(parameters), ptr_(0), pc_(), zeros_(boost::make_shared(initial.zero(ordering))) {} + + // to be compatible to NonlinearOptimizer, but shouldn't be called... + PreconditionedConjugateGradientSolver(const LINEAR &GFG) { + std::cout << "[PreconditionedConjugateGradientSolver] Unexpected usage.." << std::endl; + throw std::runtime_error("PreconditionedConjugateGradientSolver: gaussian factor graph initialization not supported"); + } + + // copy + PreconditionedConjugateGradientSolver(const PreconditionedConjugateGradientSolver& solver) : + IterativeSolver(solver), ptr_(solver.ptr_), pc_(solver.pc_), zeros_(solver.zeros_){} + + // called by NonlinearOptimizer + shared_ptr update(const LINEAR &graph) const { + + return boost::make_shared( + parameters_, + &graph, + boost::make_shared(&graph, parameters_, zeros_), + zeros_) ; + } + + // optimize with preconditioned conjugate gradient + VectorValues::shared_ptr optimize() const { + VectorValues x = preconditionedConjugateGradientDescent + (*ptr_, *pc_, *zeros_, *parameters_); + return boost::make_shared(x) ; + } + + PreconditionedConjugateGradientSolver( + const sharedParameters parameters, + const LINEAR *ptr, + const sharedPreconditioner pc, + const sharedVectorValues zeros): + IterativeSolver(parameters), ptr_(ptr), pc_(pc), zeros_(zeros) {} + private: + + // shouldn't be used + PreconditionedConjugateGradientSolver(){} + }; + + } // nsamespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index b9ba766b3..0531bbf55 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -195,18 +195,73 @@ bool GaussianFactorGraph::split(const std::map &M, GaussianFactorG return true ; } -boost::shared_ptr GaussianFactorGraph::allocateVectorVavlues() const { +VectorValues GaussianFactorGraph::allocateVectorValuesb() const { std::vector dimensions(size()) ; + Index i = 0 ; BOOST_FOREACH( const sharedFactor& factor, factors_) { - Index i = 0 ; - BOOST_FOREACH( const Index& idx, factor->keys_) { - dimensions[idx] = factor->Ab_(i).size2() ; - i++; - } + dimensions[i] = factor->numberOfRows() ; + i++; } - return boost::make_shared(dimensions) ; + return VectorValues(dimensions) ; } +bool GaussianFactorGraph::getDiagonalOfHessian(VectorValues &values) const { + + values.makeZero() ; + + BOOST_FOREACH( const sharedFactor& factor, factors_) { + Index i = 0 ; + BOOST_FOREACH( const Index& idx, factor->keys_) { + Vector v = columnNormSquare(factor->Ab_(i)) ; + values[idx] += v; + } + } + return true ; +} + +void GaussianFactorGraph::residual(const VectorValues &x, VectorValues &r) const { + + getb(r) ; + VectorValues Ax = VectorValues::SameStructure(r) ; + multiply(x,Ax) ; + axpy(-1.0,Ax,r) ; +} + +void GaussianFactorGraph::multiply(const VectorValues &x, VectorValues &r) const { + + r.makeZero() ; + Index i = 0 ; + BOOST_FOREACH(const sharedFactor& factor, factors_) { + Index j = 0 ; + BOOST_FOREACH( const Index& idx, factor->keys_ ) { + r[i] += prod(factor->Ab_(j), x[idx]) ; + ++j ; + } + ++i ; + } +} + +void GaussianFactorGraph::transposeMultiply(const VectorValues &r, VectorValues &x) const { + x.makeZero() ; + Index i = 0 ; + BOOST_FOREACH(const sharedFactor& factor, factors_) { + Index j = 0 ; + BOOST_FOREACH( const Index& idx, factor->keys_ ) { + x[idx] += prod(trans(factor->Ab_(j)), r[i]) ; + ++j ; + } + ++i ; + } +} + +void GaussianFactorGraph::getb(VectorValues &b) const { + Index i = 0 ; + BOOST_FOREACH( const sharedFactor& factor, factors_) { + b[i] = factor->getb() ; + i++; + } +} + } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index e8ebe76a8..be0c30c1a 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -152,14 +152,22 @@ namespace gtsam { */ GaussianFactorGraph add_priors(double sigma, const std::vector& dimensions) const; - /** * Split a Gaussian factor graph into two, according to M * 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 ; + // allocate a vectorvalues of b's structure + VectorValues allocateVectorValuesb() const ; + + /* get the diagonal of A^ A, used to build jacobi preconditioner */ + bool getDiagonalOfHessian(VectorValues &values) const ; + + void residual(const VectorValues &x, VectorValues &r) const ; + void multiply(const VectorValues &x, VectorValues &r) const ; + void transposeMultiply(const VectorValues &r, VectorValues &x) const ; + void getb(VectorValues &b) const ; }; } // namespace gtsam diff --git a/gtsam/linear/IterativeOptimizationParameters.h b/gtsam/linear/IterativeOptimizationParameters.h index d1e5c9fa7..1650cb169 100644 --- a/gtsam/linear/IterativeOptimizationParameters.h +++ b/gtsam/linear/IterativeOptimizationParameters.h @@ -32,7 +32,7 @@ namespace gtsam { reset_(101), epsilon_(1e-5), epsilon_abs_(1e-5), - verbosity_(SILENT) {} + verbosity_(ERROR) {} IterativeOptimizationParameters(const IterativeOptimizationParameters ¶meters): maxIterations_(parameters.maxIterations_), @@ -43,7 +43,7 @@ namespace gtsam { IterativeOptimizationParameters - (int maxIterations, double epsilon, double epsilon_abs, verbosityLevel verbosity=SILENT, int reset=-1): + (int maxIterations, double epsilon, double epsilon_abs, verbosityLevel verbosity=ERROR, int reset=-1): maxIterations_(maxIterations), reset_(reset), epsilon_(epsilon), epsilon_abs_(epsilon_abs), verbosity_(verbosity) { if (reset_==-1) reset_ = maxIterations_ + 1 ; diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h new file mode 100644 index 000000000..9cf8c0de0 --- /dev/null +++ b/gtsam/linear/Preconditioner.h @@ -0,0 +1,88 @@ +/* + * Preconditioner.h + * + * Created on: Oct 27, 2010 + * Author: Yong-Dian Jian + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + +template +class Preconditioner { + +public: + typedef IterativeOptimizationParameters Parameters; + typedef boost::shared_ptr sharedParameters; + typedef boost::shared_ptr sharedLinear; + typedef boost::shared_ptr sharedValues ; + +public: + Preconditioner(const LINEAR *ptr, const sharedParameters parameters, const sharedValues zeros): + pLinear_(ptr), parameters_(parameters), zeros_(zeros){} + + void solve(const VALUES &values, VALUES &result) const { result = values ; } + void solveTranspose(const VALUES &values, VALUES &result) const { result = values ; } + +protected: + const LINEAR* pLinear_ ; + const sharedParameters parameters_ ; + const sharedValues zeros_; + +private: + Preconditioner(){} +}; + +template +class JacobiPreconditioner : public Preconditioner { + +public: + + typedef Preconditioner Base ; + typedef IterativeOptimizationParameters Parameters; + typedef boost::shared_ptr sharedParameters; + typedef boost::shared_ptr sharedLinear; + typedef boost::shared_ptr sharedValues ; + + + JacobiPreconditioner(const LINEAR *ptr, const sharedParameters parameters, const sharedValues zeros): + Base(ptr, parameters, zeros), scale_(new VALUES(*zeros)) { + initialize() ; + } + + void initialize() { + Base::pLinear_->getDiagonalOfHessian(*scale_); + sqrt(*scale_) ; + + // check if any zero in scale_ + if ( anyZero(*scale_) ) { + std::cout << "[JacobiPreconditioner] some diagonal element of hessian is zero" << std::endl ; + throw std::runtime_error("JacobiPreconditioner"); + } + } + + void solve(const VALUES &values, VALUES &result) const { + //std::cout << "solve" << std::endl; + ediv(values, *scale_, result) ; + } + + void solveTranspose(const VALUES &values, VALUES &result) const { + //std::cout << "solveTranspose" << std::endl; + ediv(values, *scale_, result) ; + } + +protected: + sharedValues scale_ ; + +private: + JacobiPreconditioner(){} +}; + +} diff --git a/gtsam/linear/SubgraphSolver-inl.h b/gtsam/linear/SubgraphSolver-inl.h index 08c23beee..afd388a8a 100644 --- a/gtsam/linear/SubgraphSolver-inl.h +++ b/gtsam/linear/SubgraphSolver-inl.h @@ -58,7 +58,7 @@ VectorValues::shared_ptr SubgraphSolver::optimize() const { // preconditioned conjugate gradient VectorValues zeros = pc_->zero(); VectorValues ybar = conjugateGradients - (*pc_, zeros, parameters_); + (*pc_, zeros, *parameters_); boost::shared_ptr xbar = boost::make_shared() ; *xbar = pc_->x(ybar); diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index a3ee25dfa..2c82fd5ca 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -194,7 +194,23 @@ public: friend double dot(const VectorValues& V1, const VectorValues& V2) { return gtsam::dot(V1.values_, V2.values_) ; } friend void scal(double alpha, VectorValues& x) { gtsam::scal(alpha, x.values_) ; } friend void axpy(double alpha, const VectorValues& x, VectorValues& y) { gtsam::axpy(alpha, x.values_, y.values_) ; } + friend void sqrt(VectorValues &x) { Vector y = gtsam::esqrt(x.values_); x.values_ = y; } + friend void ediv(const VectorValues& numerator, const VectorValues& denominator, VectorValues &result) { + Vector v = gtsam::ediv(numerator.values_, denominator.values_); + result.values_ = v ; + } + // check whether there's a zero in the vector + friend bool anyZero(const VectorValues& x, double tol=1e-5) { + bool flag = false ; + BOOST_FOREACH(const double &v, x.values_) { + if ( v < tol && v > -tol) { + flag = true ; + break; + } + } + return flag; + } }; diff --git a/gtsam/linear/iterative-inl.h b/gtsam/linear/iterative-inl.h index c009a9e64..3f1da4669 100644 --- a/gtsam/linear/iterative-inl.h +++ b/gtsam/linear/iterative-inl.h @@ -18,6 +18,9 @@ #pragma once +#include +#include + #include #include #include @@ -33,9 +36,7 @@ namespace gtsam { struct CGState { typedef IterativeSolver::Parameters Parameters; - typedef IterativeSolver::sharedParameters sharedParameters; - - const sharedParameters parameters_; + const Parameters ¶meters_; int k; bool steepest; @@ -45,7 +46,7 @@ namespace gtsam { /* ************************************************************************* */ // Constructor - CGState(const S& Ab, const V& x, const sharedParameters parameters, bool steep): + CGState(const S& Ab, const V& x, const Parameters ¶meters, bool steep): parameters_(parameters),k(0),steepest(steep) { // Start with g0 = A'*(A*x0-b), d0 = - g0 @@ -55,10 +56,10 @@ namespace gtsam { // init gamma and calculate threshold gamma = dot(g,g) ; - threshold = ::max(parameters->epsilon_abs(), parameters->epsilon() * parameters->epsilon() * gamma); + threshold = ::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma); // Allocate and calculate A*d for first iteration - if (gamma > parameters->epsilon_abs()) Ad = Ab * d; + if (gamma > parameters_.epsilon_abs()) Ad = Ab * d; } /* ************************************************************************* */ @@ -84,19 +85,19 @@ namespace gtsam { /* ************************************************************************* */ // take a step, return true if converged bool step(const S& Ab, V& x) { - if ((++k) >= parameters_->maxIterations()) return true; + if ((++k) >= parameters_.maxIterations()) return true; //----------------------------------> double alpha = takeOptimalStep(x); // update gradient (or re-calculate at reset time) - if (k % parameters_->reset() == 0) g = Ab.gradient(x); + 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 (parameters_->verbosity()) + if (parameters_.verbosity()) cout << "iteration " << k << ": alpha = " << alpha << ", dotg = " << new_gamma << endl; if (new_gamma < threshold) return true; @@ -124,19 +125,22 @@ namespace gtsam { // S: linear system, V: step vector, E: errors template V conjugateGradients( - const S& Ab, V x, - const IterativeSolver::sharedParameters parameters, + const S& Ab, + V x, + const IterativeSolver::Parameters ¶meters, bool steepest = false) { CGState state(Ab, x, parameters, steepest); - if (parameters->verbosity()) - cout << "CG: epsilon = " << parameters->epsilon() - << ", maxIterations = " << parameters->maxIterations() + 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 (parameters->verbosity()) cout << "||g0||^2 < threshold, exiting immediately !" << endl; + if (parameters.verbosity()) + cout << "||g0||^2 < threshold, exiting immediately !" << endl; + return x; } @@ -145,6 +149,115 @@ namespace gtsam { return x; } + /* ************************************************************************* */ + // state for PCG method + template + class PCGState { + + public: + typedef IterativeSolver::Parameters Parameters; + typedef V Values; + typedef boost::shared_ptr sharedValues; + + protected: + const LINEAR &Ab_; + const PC &pc_ ; + V x_ ; + const Parameters ¶meters_; + bool steepest_; + + public: + /* ************************************************************************* */ + // Constructor + PCGState(const LINEAR& Ab, const PC &pc, const V &x0, const Parameters ¶meters, bool steep): + Ab_(Ab), pc_(pc), x_(x0), parameters_(parameters),steepest_(steep) {} + + V run() { + + // refer to Bjorck pp. 294 + V r = Ab_.allocateVectorValuesb() ; + V q = V::SameStructure(r) ; + + V p = V::SameStructure(x_) ; + V s = V::SameStructure(x_) ; + V t = V::SameStructure(x_) ; + V tmp = V::SameStructure(x_) ; + + // initial PCG + Ab_.residual(x_, r) ; + Ab_.transposeMultiply(r, tmp) ; + pc_.solveTranspose(tmp, s) ; + p = s ; + + double gamma = dot(s,s), new_gamma = 0.0, alpha = 0.0, beta = 0.0 ; + + const double threshold = + ::max(parameters_.epsilon_abs(), + parameters_.epsilon() * parameters_.epsilon() * gamma); + + const int iMaxIterations = parameters_.maxIterations(); + const int iReset = parameters_.reset() ; + + if (parameters_.verbosity()) + cout << "PCG: epsilon = " << parameters_.epsilon() + << ", maxIterations = " << parameters_.maxIterations() + << ", ||g0||^2 = " << gamma + << ", threshold = " << threshold << endl; + + for ( int k = 0 ; k < iMaxIterations ; ++k ) { + + if ( gamma < threshold ) break ; + + if ( k % iReset == 0) { + Ab_.residual(x_, r) ; + Ab_.transposeMultiply(r, tmp) ; + pc_.solveTranspose(tmp, s) ; + p = s ; + gamma = dot(s,s) ; + } + + + pc_.solve(p, t) ; + Ab_.multiply(t, q) ; + alpha = gamma / dot(q,q) ; + axpy( alpha, t, x_) ; + axpy(-alpha, q, r) ; + Ab_.transposeMultiply(r, tmp) ; + pc_.solveTranspose(tmp, s) ; + new_gamma = dot(s,s) ; + + if (parameters_.verbosity()) + cout << "iteration " << k + << ": alpha = " << alpha + << ", dotg = " << new_gamma << endl; + + beta = new_gamma / gamma ; + scal(beta,p) ; + axpy(1.0,s,p) ; + gamma = new_gamma ; + } + + return x_ ; + } // function + + private: + PCGState(){} + + }; // PCGState Class + + /* ************************************************************************* */ + template + V preconditionedConjugateGradientDescent( + const LINEAR& Ab, + const PC& pc, + V x, + const IterativeSolver::Parameters ¶meters, + bool steepest = false) { + + PCGState state(Ab, pc, x, parameters, steepest); + return state.run() ; + } + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/iterative.cpp b/gtsam/linear/iterative.cpp index d103f8944..3894f8b0d 100644 --- a/gtsam/linear/iterative.cpp +++ b/gtsam/linear/iterative.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include @@ -36,74 +38,34 @@ namespace gtsam { /* ************************************************************************* */ - Vector steepestDescent(const System& Ab, const Vector& x, const IterativeSolver::sharedParameters parameters) { + Vector steepestDescent(const System& Ab, const Vector& x, const IterativeSolver::Parameters & parameters) { return conjugateGradients (Ab, x, parameters, true); } - Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeSolver::sharedParameters parameters) { + Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeSolver::Parameters & parameters) { return conjugateGradients (Ab, x, parameters); } /* ************************************************************************* */ - Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::sharedParameters parameters) { + Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::Parameters & parameters) { System Ab(A, b); return conjugateGradients (Ab, x, parameters, true); } - Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::sharedParameters parameters) { + Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const IterativeSolver::Parameters & parameters) { System Ab(A, b); return conjugateGradients (Ab, x, parameters); } /* ************************************************************************* */ - VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters) { + VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::Parameters & parameters) { return conjugateGradients (fg, x, parameters, true); } - VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters) { + VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::Parameters & 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/gtsam/linear/iterative.h b/gtsam/linear/iterative.h index e6ab2f074..1bd042fc1 100644 --- a/gtsam/linear/iterative.h +++ b/gtsam/linear/iterative.h @@ -21,6 +21,7 @@ #include #include #include +#include namespace gtsam { @@ -88,35 +89,56 @@ namespace gtsam { /** * Method of steepest gradients, System version */ - Vector steepestDescent(const System& Ab, const Vector& x,const IterativeSolver::sharedParameters parameters); + Vector steepestDescent( + const System& Ab, + const Vector& x, + const IterativeSolver::Parameters & parameters); /** * Method of conjugate gradients (CG), System version */ - Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeSolver::sharedParameters parameters); + Vector conjugateGradientDescent( + const System& Ab, + const Vector& x, + const IterativeSolver::Parameters & 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,const IterativeSolver::sharedParameters parameters); + Vector steepestDescent( + const Matrix& A, + const Vector& b, + const Vector& x, + const IterativeSolver::Parameters & parameters); /** * Method of conjugate gradients (CG), Matrix version */ - Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x,const IterativeSolver::sharedParameters parameters); + Vector conjugateGradientDescent( + const Matrix& A, + const Vector& b, + const Vector& x, + const IterativeSolver::Parameters & parameters); class GaussianFactorGraph; /** * Method of steepest gradients, Gaussian Factor Graph version * */ - VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters); + VectorValues steepestDescent( + const GaussianFactorGraph& fg, + const VectorValues& x, + const IterativeSolver::Parameters & parameters); /** * Method of conjugate gradients (CG), Gaussian Factor Graph version * */ - VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters); + VectorValues conjugateGradientDescent( + const GaussianFactorGraph& fg, + const VectorValues& x, + const IterativeSolver::Parameters & parameters); + } // namespace gtsam diff --git a/gtsam/nonlinear/NonlinearOptimization-inl.h b/gtsam/nonlinear/NonlinearOptimization-inl.h index 6d1ed228c..7d0bddf8c 100644 --- a/gtsam/nonlinear/NonlinearOptimization-inl.h +++ b/gtsam/nonlinear/NonlinearOptimization-inl.h @@ -93,6 +93,27 @@ namespace gtsam { return *result.values(); } + template + T optimizePCG(const G& graph, + const T& initialEstimate, + const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) { + + typedef PreconditionedConjugateGradientSolver Solver; + typedef boost::shared_ptr sharedSolver; + typedef NonlinearOptimizer Optimizer; + + Ordering::shared_ptr ordering = initialEstimate.orderingArbitrary() ; + sharedSolver solver = boost::make_shared(graph, initialEstimate, *ordering, IterativeOptimizationParameters()); + Optimizer optimizer( + boost::make_shared(graph), + boost::make_shared(initialEstimate), + ordering, + solver); + + // Levenberg-Marquardt + Optimizer result = optimizer.levenbergMarquardt(parameters); + return *result.values(); + } /** diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h index fd7c32d2a..38c26059a 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -357,7 +357,8 @@ namespace gtsam { template NonlinearOptimizer NonlinearOptimizer:: levenbergMarquardt(const NonlinearOptimizationParameters ¶meters) const { - return newParameters_(boost::make_shared(parameters)).levenbergMarquardt() ; + boost::shared_ptr ptr (new NonlinearOptimizationParameters(parameters)) ; + return newParameters_(ptr).levenbergMarquardt() ; } /* ************************************************************************* */