add preconditioned conjugate gradient
Yong-Dian Jian
parent
70aa2f7f5d
commit
b53bcc7d66
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <boost/shared_ptr.hpp>
|
||||
|
||||
#include <gtsam/slam/pose2SLAM.h>
|
||||
|
||||
#include <gtsam/linear/Preconditioner.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimization-inl.h>
|
||||
|
||||
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,Values, JacobiPreconditioner<GaussianFactorGraph,VectorValues> >(graph, initial);
|
||||
//result = optimizeCG(graph, initial);
|
||||
result.print("final result") ;
|
||||
return 0 ;
|
||||
}
|
||||
|
||||
|
|
@ -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<const MatrixColMajor> mc (A, i);
|
||||
v[i] = dot(mc, mc) ;
|
||||
}
|
||||
return v ;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void solve(Matrix& A, Matrix& B)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -15,7 +15,6 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
// GaussianFactorGraph, Values
|
||||
template<class GRAPH, class LINEAR, class VALUES>
|
||||
class ConjugateGradientSolver : public IterativeSolver {
|
||||
|
||||
|
|
@ -33,30 +32,104 @@ namespace gtsam {
|
|||
|
||||
typedef boost::shared_ptr<const ConjugateGradientSolver> 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<VectorValues>(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<ConjugateGradientSolver>(parameters_, &graph, zeros_) ;
|
||||
}
|
||||
|
||||
VectorValues::shared_ptr optimize() const {
|
||||
VectorValues x = conjugateGradientDescent(*ptr_, *zeros_, parameters_);
|
||||
VectorValues x = conjugateGradientDescent(*ptr_, *zeros_, *parameters_);
|
||||
return boost::make_shared<VectorValues>(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 GRAPH, class LINEAR, class VALUES, class PC>
|
||||
class PreconditionedConjugateGradientSolver : public IterativeSolver {
|
||||
|
||||
protected:
|
||||
|
||||
typedef boost::shared_ptr<GRAPH> sharedGRAPH ;
|
||||
typedef boost::shared_ptr<LINEAR> sharedLINEAR ;
|
||||
typedef boost::shared_ptr<PC> sharedPreconditioner ;
|
||||
typedef boost::shared_ptr<VALUES> sharedVALUES ;
|
||||
typedef boost::shared_ptr<VectorValues> sharedVectorValues ;
|
||||
|
||||
const LINEAR *ptr_;
|
||||
sharedPreconditioner pc_ ;
|
||||
sharedVectorValues zeros_;
|
||||
public:
|
||||
|
||||
typedef boost::shared_ptr<const PreconditionedConjugateGradientSolver> 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<VectorValues>(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<PreconditionedConjugateGradientSolver>(
|
||||
parameters_,
|
||||
&graph,
|
||||
boost::make_shared<PC>(&graph, parameters_, zeros_),
|
||||
zeros_) ;
|
||||
}
|
||||
|
||||
// optimize with preconditioned conjugate gradient
|
||||
VectorValues::shared_ptr optimize() const {
|
||||
VectorValues x = preconditionedConjugateGradientDescent<LINEAR, PC, VectorValues>
|
||||
(*ptr_, *pc_, *zeros_, *parameters_);
|
||||
return boost::make_shared<VectorValues>(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
|
||||
|
|
|
|||
|
|
@ -195,18 +195,73 @@ bool GaussianFactorGraph::split(const std::map<Index, Index> &M, GaussianFactorG
|
|||
return true ;
|
||||
}
|
||||
|
||||
boost::shared_ptr<VectorValues> GaussianFactorGraph::allocateVectorVavlues() const {
|
||||
VectorValues GaussianFactorGraph::allocateVectorValuesb() const {
|
||||
std::vector<size_t> 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<VectorValues>(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
|
||||
|
|
|
|||
|
|
@ -152,14 +152,22 @@ namespace gtsam {
|
|||
*/
|
||||
GaussianFactorGraph add_priors(double sigma, const std::vector<size_t>& 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<Index, Index> &M, GaussianFactorGraph &A1, GaussianFactorGraph &A2) const ;
|
||||
boost::shared_ptr<VectorValues> 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
|
||||
|
|
|
|||
|
|
@ -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 ;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,88 @@
|
|||
/*
|
||||
* Preconditioner.h
|
||||
*
|
||||
* Created on: Oct 27, 2010
|
||||
* Author: Yong-Dian Jian
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/linear/IterativeOptimizationParameters.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
template <class LINEAR, class VALUES>
|
||||
class Preconditioner {
|
||||
|
||||
public:
|
||||
typedef IterativeOptimizationParameters Parameters;
|
||||
typedef boost::shared_ptr<Parameters> sharedParameters;
|
||||
typedef boost::shared_ptr<LINEAR> sharedLinear;
|
||||
typedef boost::shared_ptr<VALUES> 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 LINEAR, class VALUES>
|
||||
class JacobiPreconditioner : public Preconditioner<LINEAR,VALUES> {
|
||||
|
||||
public:
|
||||
|
||||
typedef Preconditioner<LINEAR,VALUES> Base ;
|
||||
typedef IterativeOptimizationParameters Parameters;
|
||||
typedef boost::shared_ptr<Parameters> sharedParameters;
|
||||
typedef boost::shared_ptr<LINEAR> sharedLinear;
|
||||
typedef boost::shared_ptr<VALUES> 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(){}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -58,7 +58,7 @@ 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_);
|
||||
(*pc_, zeros, *parameters_);
|
||||
|
||||
boost::shared_ptr<VectorValues> xbar = boost::make_shared<VectorValues>() ;
|
||||
*xbar = pc_->x(ybar);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -18,6 +18,9 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
#include <gtsam/linear/IterativeOptimizationParameters.h>
|
||||
#include <gtsam/linear/IterativeSolver.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
|
|
@ -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<class S, class V, class E>
|
||||
V conjugateGradients(
|
||||
const S& Ab, V x,
|
||||
const IterativeSolver::sharedParameters parameters,
|
||||
const S& Ab,
|
||||
V x,
|
||||
const IterativeSolver::Parameters ¶meters,
|
||||
bool steepest = false) {
|
||||
|
||||
CGState<S, V, E> 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 LINEAR, class PC, class V>
|
||||
class PCGState {
|
||||
|
||||
public:
|
||||
typedef IterativeSolver::Parameters Parameters;
|
||||
typedef V Values;
|
||||
typedef boost::shared_ptr<Values> 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<class LINEAR, class PC, class V>
|
||||
V preconditionedConjugateGradientDescent(
|
||||
const LINEAR& Ab,
|
||||
const PC& pc,
|
||||
V x,
|
||||
const IterativeSolver::Parameters ¶meters,
|
||||
bool steepest = false) {
|
||||
|
||||
PCGState<LINEAR, PC, V> state(Ab, pc, x, parameters, steepest);
|
||||
return state.run() ;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -20,6 +20,8 @@
|
|||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/IterativeSolver.h>
|
||||
#include <gtsam/linear/Preconditioner.h>
|
||||
#include <gtsam/linear/IterativeOptimizationParameters.h>
|
||||
#include <gtsam/linear/iterative-inl.h>
|
||||
|
||||
|
|
@ -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<System, Vector, Vector> (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<System, Vector, Vector> (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<System, Vector, Vector> (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<System, Vector, Vector> (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<GaussianFactorGraph, VectorValues, Errors> (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<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
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/linear/IterativeSolver.h>
|
||||
#include <gtsam/linear/Preconditioner.h>
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -93,6 +93,27 @@ namespace gtsam {
|
|||
return *result.values();
|
||||
}
|
||||
|
||||
template<class G, class T, class PC>
|
||||
T optimizePCG(const G& graph,
|
||||
const T& initialEstimate,
|
||||
const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) {
|
||||
|
||||
typedef PreconditionedConjugateGradientSolver<G,GaussianFactorGraph,T,PC> 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, 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();
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -357,7 +357,8 @@ namespace gtsam {
|
|||
template<class G, class C, class L, class S, class W>
|
||||
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::
|
||||
levenbergMarquardt(const NonlinearOptimizationParameters ¶meters) const {
|
||||
return newParameters_(boost::make_shared<Parameters>(parameters)).levenbergMarquardt() ;
|
||||
boost::shared_ptr<NonlinearOptimizationParameters> ptr (new NonlinearOptimizationParameters(parameters)) ;
|
||||
return newParameters_(ptr).levenbergMarquardt() ;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue