12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add a simple conjugate gradient solver with a structure to encapsulate all parameters for iterative methods

release/4.3a0
Yong-Dian Jian 2010-10-25 05:29:52 +00:00
parent 50fb687f96
commit b3bcf0d236
13 changed files with 393 additions and 166 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
linear/ConjugateGradientSolver-inl.h Normal file
View File

@ -0,0 +1,10 @@
/*
* ConjugateGradientSolver-inl.h
*
* Created on: Oct 24, 2010
* Author: Yong-Dian Jian
*/
#pragma once
#include <gtsam/ConjugateGradientSolver.h>

58
linear/ConjugateGradientSolver.h Normal file
View File

@ -0,0 +1,58 @@
/*
* ConjugateGradientSolver.h
*
* Created on: Oct 21, 2010
* Author: Yong-Dian Jian
*/
#pragma once
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
// GaussianFactorGraph, Values
template<class GRAPH, class LINEAR, class VALUES>
class ConjugateGradientSolver : public IterativeSolver {
protected:
const LINEAR *ptr_;
typedef boost::shared_ptr<VectorValues> sharedVectorValues ;
sharedVectorValues zeros_;
public:
typedef boost::shared_ptr<const ConjugateGradientSolver> shared_ptr ;
ConjugateGradientSolver(const GRAPH &graph, const VALUES &initial, const Ordering &ordering, const sharedParameters parameters):
IterativeSolver(parameters), ptr_(0), zeros_(boost::make_shared<VectorValues>(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<ConjugateGradientSolver>(parameters_, &graph, zeros_) ;
}
VectorValues::shared_ptr optimize() const {
//boost::shared_ptr<VectorValues> zeros (ptr_->allocateVectorVavlues());
VectorValues x = conjugateGradientDescent(*ptr_, *zeros_, parameters_);
return boost::make_shared<VectorValues>(x) ;
}
private:
ConjugateGradientSolver():IterativeSolver(),ptr_(0){}
};
} // nsamespace gtsam

20
linear/GaussianFactorGraph.cpp
View File

@ -16,7 +16,10 @@
* @author Christian Potthast
*/
#include <vector>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
@ -165,12 +168,12 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const vector<s
bool GaussianFactorGraph::split(const std::map<Index, Index> &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<Index, Index> &M, GaussianFactorG
return true ;
}
boost::shared_ptr<VectorValues> GaussianFactorGraph::allocateVectorVavlues() const {
std::vector<size_t> 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<VectorValues>(dimensions) ;
}
} // namespace gtsam

1
linear/GaussianFactorGraph.h
View File

@ -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<Index, Index> &M, GaussianFactorGraph &A1, GaussianFactorGraph &A2) const ;
boost::shared_ptr<VectorValues> allocateVectorVavlues() const ;
};

44
linear/IterativeOptimizationParameters.h Normal file
View File

@ -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_;
};
}

37
linear/IterativeSolver.h Normal file
View File

@ -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 <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/linear/IterativeOptimizationParameters.h>
namespace gtsam {
class IterativeSolver {
public:
typedef IterativeOptimizationParameters Parameters;
typedef boost::shared_ptr<Parameters> sharedParameters;
sharedParameters parameters_ ;
IterativeSolver():
parameters_(new IterativeOptimizationParameters()) {}
IterativeSolver(const IterativeSolver &solver):
parameters_(solver.parameters_) {}
IterativeSolver(const sharedParameters parameters):
parameters_(parameters) {}
};
}

87
linear/SubgraphSolver-inl.h
View File

@ -9,18 +9,87 @@
* -------------------------------------------------------------------------- */
/*
* SubgraphSolver-inl.h
*
* Created on: Jan 17, 2010
* Author: nikai
* Description: subgraph preconditioning conjugate gradient solver
*/
#pragma once
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/inference/EliminationTree-inl.h>
using namespace std;
namespace gtsam {}
namespace gtsam {
template<class GRAPH, class LINEAR, class VALUES>
typename SubgraphSolver<GRAPH,LINEAR,VALUES>::shared_ptr
SubgraphSolver<GRAPH,LINEAR,VALUES>::update(const LINEAR &graph) const {
shared_linear Ab1 = boost::make_shared<LINEAR>(),
Ab2 = boost::make_shared<LINEAR>();
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<GRAPH, Constraint, Pose, Values> (T_, tree, theta0[root]);
LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!!
SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree<GaussianFactor>::Create(sacrificialAb1)->eliminate();
SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
shared_preconditioner pc = boost::make_shared<SubgraphPreconditioner>(Ab1,Ab2,Rc1,xbar);
return boost::make_shared<SubgraphSolver>(ordering_, pairs_, pc, parameters_) ;
}
template<class GRAPH, class LINEAR, class VALUES>
VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() const {
// preconditioned conjugate gradient
VectorValues zeros = pc_->zero();
VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>
(*pc_, zeros, parameters_);
boost::shared_ptr<VectorValues> xbar = boost::make_shared<VectorValues>() ;
*xbar = pc_->x(ybar);
return xbar;
}
template<class GRAPH, class LINEAR, class VALUES>
void SubgraphSolver<GRAPH,LINEAR,VALUES>::initialize(const GRAPH& G, const VALUES& theta0) {
// generate spanning tree
PredecessorMap<Key> tree_ = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
// make the ordering
list<Key> keys = predecessorMap2Keys(tree_);
ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
// build factor pairs
pairs_.clear();
typedef pair<Key,Key> EG ;
BOOST_FOREACH( const EG &eg, tree_ ) {
Symbol key1 = Symbol(eg.first),
key2 = Symbol(eg.second) ;
pairs_.insert(pair<Index, Index>((*ordering_)[key1], (*ordering_)[key2])) ;
}
}
}

90
linear/SubgraphSolver.h
View File

@ -11,17 +11,12 @@
#pragma once
#include <boost/foreach.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <gtsam/inference/EliminationTree-inl.h>
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {
@ -33,7 +28,7 @@ namespace gtsam {
* solve : L -> VectorValues
*/
template<class GRAPH, class LINEAR, class VALUES>
class SubgraphSolver {
class SubgraphSolver : public IterativeSolver {
private:
typedef typename VALUES::Key Key;
@ -48,11 +43,6 @@ namespace gtsam {
typedef boost::shared_ptr<SubgraphPreconditioner> shared_preconditioner ;
typedef std::map<Index,Index> 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<Parameters>()) :
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<LINEAR>(),
Ab2 = boost::make_shared<LINEAR>();
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<GRAPH, Constraint, Pose, Values> (T_, tree, theta0[root]);
LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!!
SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree<GaussianFactor>::Create(sacrificialAb1)->eliminate();
SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
shared_preconditioner pc = boost::make_shared<SubgraphPreconditioner>(Ab1,Ab2,Rc1,xbar);
return boost::make_shared<SubgraphSolver>(ordering_, pairs_, pc) ;
}
VectorValues::shared_ptr optimize() const {
// preconditioned conjugate gradient
VectorValues zeros = pc_->zero();
VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues,
Errors> (*pc_, zeros, verbose_, epsilon_, epsilon_abs_, maxIterations_);
boost::shared_ptr<VectorValues> xbar = boost::make_shared<VectorValues>() ;
*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<Key> tree_ = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
void initialize(const GRAPH& G, const VALUES& theta0);
// make the ordering
list<Key> keys = predecessorMap2Keys(tree_);
ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
// build factor pairs
pairs_.clear();
typedef pair<Key,Key> EG ;
BOOST_FOREACH( const EG &eg, tree_ ) {
Symbol key1 = Symbol(eg.first),
key2 = Symbol(eg.second) ;
pairs_.insert(pair<Index, Index>((*ordering_)[key1], (*ordering_)[key2])) ;
}
}
private:
SubgraphSolver():IterativeSolver(){}
};
template<class GRAPH, class LINEAR, class VALUES> const size_t SubgraphSolver<GRAPH, LINEAR, VALUES>::maxIterations_;
template<class GRAPH, class LINEAR, class VALUES> const bool SubgraphSolver<GRAPH, LINEAR, VALUES>::verbose_;
template<class GRAPH, class LINEAR, class VALUES> const double SubgraphSolver<GRAPH, LINEAR, VALUES>::epsilon_;
template<class GRAPH, class LINEAR, class VALUES> const double SubgraphSolver<GRAPH, LINEAR, VALUES>::epsilon_abs_;
} // nsamespace gtsam

68
linear/iterative-inl.h
View File

@ -18,6 +18,8 @@
#pragma once
#include <gtsam/linear/IterativeOptimizationParameters.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/iterative.h>
@ -30,21 +32,21 @@ namespace gtsam {
template<class S, class V, class E>
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
if (k % parameters_->reset() == 0) g = Ab.gradient(x);
// axpy(alpha, Ab ^ Ad, g); // g += alpha*(Ab^Ad)
Ab.transposeMultiplyAdd(alpha, Ad, g);
else Ab.transposeMultiplyAdd(alpha, Ad, g);
// check for convergence
double new_gamma = dot(g, g);
if (verbose) cout << "iteration " << k << ": alpha = " << alpha
if (parameters_->verbosity())
cout << "iteration " << k << ": alpha = " << alpha
<< ", dotg = " << new_gamma << endl;
if (new_gamma < threshold) return true; //---------------------------------->
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<class S, class V, class E>
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<S, V, E> state(Ab, x, verbose, epsilon, epsilon_abs, maxIterations,steepest);
if (verbose) cout << "CG: epsilon = " << epsilon << ", maxIterations = "
<< state.maxIterations << ", ||g0||^2 = " << state.gamma
CGState<S, V, E> 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;
}

78
linear/iterative.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/IterativeOptimizationParameters.h>
#include <gtsam/linear/iterative-inl.h>
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<System, Vector, Vector> (Ab, x, verbose, epsilon,
epsilon_abs, maxIterations, true);
Vector steepestDescent(const System& Ab, const Vector& x, const IterativeSolver::sharedParameters parameters) {
return conjugateGradients<System, Vector, Vector> (Ab, x, parameters, true);
}
Vector conjugateGradientDescent(const System& Ab, const Vector& x,
bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
return conjugateGradients<System, Vector, Vector> (Ab, x, verbose, epsilon,
epsilon_abs, maxIterations);
Vector conjugateGradientDescent(const System& Ab, const Vector& x, const IterativeSolver::sharedParameters parameters) {
return conjugateGradients<System, Vector, Vector> (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<System, Vector, Vector> (Ab, x, verbose, epsilon,
epsilon_abs, maxIterations, true);
return conjugateGradients<System, Vector, Vector> (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<System, Vector, Vector> (Ab, x, verbose, epsilon,
epsilon_abs, maxIterations);
return conjugateGradients<System, Vector, Vector> (Ab, x, parameters);
}
/* ************************************************************************* */
VectorValues steepestDescent(const GaussianFactorGraph& fg,
const VectorValues& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg,
x, verbose, epsilon, epsilon_abs, maxIterations, true);
VectorValues steepestDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters) {
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg, x, parameters, true);
}
VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg,
const VectorValues& x, bool verbose, double epsilon, double epsilon_abs, size_t maxIterations) {
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg,
x, verbose, epsilon, epsilon_abs, maxIterations);
VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg, const VectorValues& x, const IterativeSolver::sharedParameters parameters) {
return conjugateGradients<GaussianFactorGraph, VectorValues, Errors> (fg, x, parameters);
}
// Vector steepestDescent(const System& Ab, const Vector& x, bool verbose,
// double epsilon, double epsilon_abs, size_t maxIterations) {
// return conjugateGradients<System, Vector, Vector> (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<System, Vector, Vector> (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<System, Vector, Vector> (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<System, Vector, Vector> (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<GaussianFactorGraph, VectorValues, Errors> (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<GaussianFactorGraph, VectorValues, Errors> (fg,
// x, verbose, epsilon, epsilon_abs, maxIterations);
// }
/* ************************************************************************* */
} // namespace gtsam

24
linear/iterative.h
View File

@ -20,6 +20,7 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/IterativeSolver.h>
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

32
nonlinear/NonlinearOptimization-inl.h
View File

@ -19,8 +19,11 @@
#pragma once
#include <boost/make_shared.hpp>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/SubgraphSolver-inl.h>
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
@ -67,6 +70,31 @@ namespace gtsam {
return *result.values();
}
/**
* The cg solver
*/
template<class G, class T>
T optimizeCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) {
typedef ConjugateGradientSolver<G,GaussianFactorGraph,T> Solver;
typedef boost::shared_ptr<Solver> sharedSolver;
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, Solver> Optimizer;
Ordering::shared_ptr ordering = initialEstimate.orderingArbitrary() ;
sharedSolver solver = boost::make_shared<Solver>(graph, initialEstimate, *ordering, boost::make_shared<IterativeOptimizationParameters>());
Optimizer optimizer(
boost::make_shared<const G>(graph),
boost::make_shared<const T>(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<G,T>(graph, initialEstimate, parameters);
case MULTIFRONTAL:
return optimizeMultiFrontal<G,T>(graph, initialEstimate, parameters);
case CG:
return optimizeCG<G,T>(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");
}

4
nonlinear/NonlinearOptimization.h
View File

@ -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<class G, class T>
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);
}