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

Merge remote-tracking branch 'origin/feature/pcg' into develop 

Conflicts:
	gtsam/nonlinear/NonlinearOptimizer.cpp
release/4.3a0
Yong-Dian Jian 2014-06-16 00:41:34 -04:00
parent 21490eadd6 17426d0076
commit 38dc807f32
23 changed files with 2072 additions and 203 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/Pose2SLAMwSPCG.cpp
View File

@ -104,7 +104,7 @@ int main(int argc, char** argv) {
LevenbergMarquardtParams parameters;
parameters.verbosity = NonlinearOptimizerParams::ERROR;
parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
parameters.linearSolverType = NonlinearOptimizerParams::CONJUGATE_GRADIENT;
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
{
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();

4
gtsam.h
View File

@ -1481,9 +1481,7 @@ class GaussianISAM {
#include <gtsam/linear/IterativeSolver.h>
virtual class IterativeOptimizationParameters {
string getKernel() const ;
string getVerbosity() const;
void setKernel(string s) ;
void setVerbosity(string s) ;
void print() const;
};
@ -1860,7 +1858,7 @@ virtual class NonlinearOptimizerParams {
bool isMultifrontal() const;
bool isSequential() const;
bool isCholmod() const;
bool isCG() const;
bool isIterative() const;
};
bool checkConvergence(double relativeErrorTreshold,

11
gtsam/inference/Ordering.h
View File

@ -17,9 +17,11 @@
#pragma once
#include <algorithm>
#include <vector>
#include <boost/assign/list_inserter.hpp>
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/FactorGraph.h>
@ -135,6 +137,15 @@ namespace gtsam {
static GTSAM_EXPORT Ordering COLAMDConstrained(const VariableIndex& variableIndex,
const FastMap<Key, int>& groups);
/// Return a natural Ordering. Typically used by iterative solvers
template <class FACTOR>
static Ordering Natural(const FactorGraph<FACTOR> &fg) {
FastSet<Key> src = fg.keys();
std::vector<Key> keys(src.begin(), src.end());
std::stable_sort(keys.begin(), keys.end());
return Ordering(keys);
}
/// @}
/// @name Testable @{

49
gtsam/linear/ConjugateGradientSolver.cpp Normal file
View File

@ -0,0 +1,49 @@
/*
* ConjugateGradientSolver.cpp
*
* Created on: Jun 4, 2014
* Author: ydjian
*/
#include <gtsam/linear/ConjugateGradientSolver.h>
#include <boost/algorithm/string.hpp>
#include <iostream>
using namespace std;
namespace gtsam {
/*****************************************************************************/
void ConjugateGradientParameters::print(ostream &os) const {
Base::print(os);
cout << "ConjugateGradientParameters" << endl
<< "minIter: " << minIterations_ << endl
<< "maxIter: " << maxIterations_ << endl
<< "resetIter: " << reset_ << endl
<< "eps_rel: " << epsilon_rel_ << endl
<< "eps_abs: " << epsilon_abs_ << endl;
}
/*****************************************************************************/
std::string ConjugateGradientParameters::blasTranslator(const BLASKernel value) {
std::string s;
switch (value) {
case ConjugateGradientParameters::GTSAM: s = "GTSAM" ; break;
default: s = "UNDEFINED" ; break;
}
return s;
}
/*****************************************************************************/
ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "GTSAM") return ConjugateGradientParameters::GTSAM;
/* default is SBM */
return ConjugateGradientParameters::GTSAM;
}
}

104
gtsam/linear/ConjugateGradientSolver.h
View File

@ -12,14 +12,15 @@
#pragma once
#include <gtsam/linear/IterativeSolver.h>
#include <iosfwd>
namespace gtsam {
/**
* parameters for the conjugate gradient method
*/
class GTSAM_EXPORT ConjugateGradientParameters : public IterativeOptimizationParameters {
class ConjugateGradientParameters : public IterativeOptimizationParameters {
public:
typedef IterativeOptimizationParameters Base;
typedef boost::shared_ptr<ConjugateGradientParameters> shared_ptr;
@ -30,14 +31,23 @@ public:
double epsilon_rel_; ///< threshold for relative error decrease
double epsilon_abs_; ///< threshold for absolute error decrease
ConjugateGradientParameters()
: minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3), epsilon_abs_(1e-3){}
/* Matrix Operation Kernel */
enum BLASKernel {
GTSAM = 0, ///< Jacobian Factor Graph of GTSAM
} blas_kernel_ ;
ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset, double epsilon_rel, double epsilon_abs)
: minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset), epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs){}
ConjugateGradientParameters()
: minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3),
epsilon_abs_(1e-3), blas_kernel_(GTSAM) {}
ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset,
double epsilon_rel, double epsilon_abs, BLASKernel blas)
: minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset),
epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas) {}
ConjugateGradientParameters(const ConjugateGradientParameters &p)
: Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_), epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_) {}
: Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_),
epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_), blas_kernel_(GTSAM) {}
/* general interface */
inline size_t minIterations() const { return minIterations_; }
@ -61,15 +71,79 @@ public:
inline void setEpsilon_rel(double value) { epsilon_rel_ = value; }
inline void setEpsilon_abs(double value) { epsilon_abs_ = value; }
virtual void print() const {
Base::print();
std::cout << "ConjugateGradientParameters" << std::endl
<< "minIter: " << minIterations_ << std::endl
<< "maxIter: " << maxIterations_ << std::endl
<< "resetIter: " << reset_ << std::endl
<< "eps_rel: " << epsilon_rel_ << std::endl
<< "eps_abs: " << epsilon_abs_ << std::endl;
}
virtual void print(std::ostream &os) const;
static std::string blasTranslator(const BLASKernel k) ;
static BLASKernel blasTranslator(const std::string &s) ;
};
/*********************************************************************************************/
/*
* A template of linear preconditioned conjugate gradient method.
* System class should support residual(v, g), multiply(v,Av), leftPrecondition(v, S^{-t}v,
* rightPrecondition(v, S^{-1}v), scal(alpha,v), dot(v,v), axpy(alpha,x,y)
* Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book.
*/
template <class S, class V>
V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters &parameters) {
V estimate, residual, direction, q1, q2;
estimate = residual = direction = q1 = q2 = initial;
system.residual(estimate, q1); /* q1 = b-Ax */
system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */
system.rightPrecondition(residual, direction);/* d = S^{-1} r */
double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta;
const size_t iMaxIterations = parameters.maxIterations(),
iMinIterations = parameters.minIterations(),
iReset = parameters.reset() ;
const double threshold = std::max(parameters.epsilon_abs(),
parameters.epsilon() * parameters.epsilon() * currentGamma);
if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY )
std::cout << "[PCG] epsilon = " << parameters.epsilon()
<< ", max = " << parameters.maxIterations()
<< ", reset = " << parameters.reset()
<< ", ||r0||^2 = " << currentGamma
<< ", threshold = " << threshold << std::endl;
size_t k;
for ( k = 1 ; k <= iMaxIterations && (currentGamma > threshold || k <= iMinIterations) ; k++ ) {
if ( k % iReset == 0 ) {
system.residual(estimate, q1); /* q1 = b-Ax */
system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */
system.rightPrecondition(residual, direction); /* d = S^{-1} r */
currentGamma = system.dot(residual, residual);
}
system.multiply(direction, q1); /* q1 = A d */
alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (d' A d) */
system.axpy(alpha, direction, estimate); /* estimate += alpha * direction */
system.leftPrecondition(q1, q2); /* q2 = S^{-T} * q1 */
system.axpy(-alpha, q2, residual); /* residual -= alpha * q2 */
prevGamma = currentGamma;
currentGamma = system.dot(residual, residual); /* gamma = |residual|^2 */
beta = currentGamma / prevGamma;
system.rightPrecondition(residual, q1); /* q1 = S^{-1} residual */
system.scal(beta, direction);
system.axpy(1.0, q1, direction); /* direction = q1 + beta * direction */
if (parameters.verbosity() >= ConjugateGradientParameters::ERROR )
std::cout << "[PCG] k = " << k
<< ", alpha = " << alpha
<< ", beta = " << beta
<< ", ||r||^2 = " << currentGamma
<< std::endl;
}
if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY )
std::cout << "[PCG] iterations = " << k
<< ", ||r||^2 = " << currentGamma
<< std::endl;
return estimate;
}
}

14
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -54,6 +54,20 @@ namespace gtsam {
return keys;
}
/* ************************************************************************* */
std::map<Key, size_t> GaussianFactorGraph::getKeyDimMap() const {
map<Key, size_t> spec;
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, *this ) {
for ( GaussianFactor::const_iterator it = gf->begin() ; it != gf->end() ; it++ ) {
map<Key,size_t>::iterator it2 = spec.find(*it);
if ( it2 == spec.end() ) {
spec.insert(make_pair<Key,size_t>(*it, gf->getDim(it)));
}
}
}
return spec;
}
/* ************************************************************************* */
vector<size_t> GaussianFactorGraph::getkeydim() const {
// First find dimensions of each variable

3
gtsam/linear/GaussianFactorGraph.h
View File

@ -138,6 +138,9 @@ namespace gtsam {
typedef FastSet<Key> Keys;
Keys keys() const;
/* return a map of (Key, dimension) */
std::map<Key, size_t> getKeyDimMap() const;
std::vector<size_t> getkeydim() const;
/** unnormalized error */

118
gtsam/linear/IterativeSolver.cpp
View File

@ -6,25 +6,42 @@
*/
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/algorithm/string.hpp>
#include <iostream>
#include <string>
using namespace std;
namespace gtsam {
std::string IterativeOptimizationParameters::getKernel() const { return kernelTranslator(kernel_); }
std::string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); }
void IterativeOptimizationParameters::setKernel(const std::string &src) { kernel_ = kernelTranslator(src); }
void IterativeOptimizationParameters::setVerbosity(const std::string &src) { verbosity_ = verbosityTranslator(src); }
/*****************************************************************************/
string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); }
IterativeOptimizationParameters::Kernel IterativeOptimizationParameters::kernelTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "CG") return IterativeOptimizationParameters::CG;
/* default is cg */
else return IterativeOptimizationParameters::CG;
/*****************************************************************************/
void IterativeOptimizationParameters::setVerbosity(const string &src) { verbosity_ = verbosityTranslator(src); }
/*****************************************************************************/
void IterativeOptimizationParameters::print() const {
print(cout);
}
IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
/*****************************************************************************/
void IterativeOptimizationParameters::print(ostream &os) const {
os << "IterativeOptimizationParameters:" << endl
<< "verbosity: " << verbosityTranslator(verbosity_) << endl;
}
/*****************************************************************************/
ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) {
p.print(os);
return os;
}
/*****************************************************************************/
IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const string &src) {
string s = src; boost::algorithm::to_upper(s);
if (s == "SILENT") return IterativeOptimizationParameters::SILENT;
else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY;
else if (s == "ERROR") return IterativeOptimizationParameters::ERROR;
@ -32,18 +49,85 @@ IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verb
else return IterativeOptimizationParameters::SILENT;
}
std::string IterativeOptimizationParameters::kernelTranslator(IterativeOptimizationParameters::Kernel k) {
if ( k == CG ) return "CG";
else return "UNKNOWN";
}
std::string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) {
/*****************************************************************************/
string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) {
if (verbosity == SILENT) return "SILENT";
else if (verbosity == COMPLEXITY) return "COMPLEXITY";
else if (verbosity == ERROR) return "ERROR";
else return "UNKNOWN";
}
/*****************************************************************************/
VectorValues IterativeSolver::optimize(
const GaussianFactorGraph &gfg,
boost::optional<const KeyInfo&> keyInfo,
boost::optional<const std::map<Key, Vector>&> lambda)
{
return optimize(
gfg,
keyInfo ? *keyInfo : KeyInfo(gfg),
lambda ? *lambda : std::map<Key,Vector>());
}
/*****************************************************************************/
VectorValues IterativeSolver::optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda)
{
return optimize(gfg, keyInfo, lambda, keyInfo.x0());
}
/****************************************************************************/
KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering)
: ordering_(ordering) {
initialize(fg);
}
/****************************************************************************/
KeyInfo::KeyInfo(const GaussianFactorGraph &fg)
: ordering_(Ordering::Natural(fg)) {
initialize(fg);
}
/****************************************************************************/
void KeyInfo::initialize(const GaussianFactorGraph &fg){
const map<Key, size_t> colspec = fg.getKeyDimMap();
const size_t n = ordering_.size();
size_t start = 0;
for ( size_t i = 0 ; i < n ; ++i ) {
const size_t key = ordering_[i];
const size_t dim = colspec.find(key)->second;
insert(make_pair<Key, KeyInfoEntry>(key, KeyInfoEntry(i, dim, start)));
start += dim ;
}
numCols_ = start;
}
/****************************************************************************/
vector<size_t> KeyInfo::colSpec() const {
std::vector<size_t> result(size(), 0);
BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) {
result[item.second.index()] = item.second.dim();
}
return result;
}
/****************************************************************************/
VectorValues KeyInfo::x0() const {
VectorValues result;
BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) {
result.insert(item.first, Vector::Zero(item.second.dim()));
}
return result;
}
/****************************************************************************/
Vector KeyInfo::x0vector() const {
return Vector::Zero(numCols_);
}
}

110
gtsam/linear/IterativeSolver.h
View File

@ -11,17 +11,27 @@
#pragma once
#include <gtsam/base/Vector.h>
#include <gtsam/global_includes.h>
#include <gtsam/inference/Ordering.h>
#include <boost/tuple/tuple.hpp>
#include <boost/optional/optional.hpp>
#include <boost/none.hpp>
#include <iosfwd>
#include <map>
#include <string>
#include <iostream>
#include <vector>
namespace gtsam {
// Forward declarations
class VectorValues;
class KeyInfo;
class KeyInfoEntry;
class GaussianFactorGraph;
class Values;
class VectorValues;
/************************************************************************************/
/**
* parameters for iterative linear solvers
*/
@ -30,55 +40,99 @@ namespace gtsam {
public:
typedef boost::shared_ptr<IterativeOptimizationParameters> shared_ptr;
enum Kernel { CG = 0 } kernel_ ; ///< Iterative Method Kernel
enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_;
public:
IterativeOptimizationParameters(const IterativeOptimizationParameters &p)
: kernel_(p.kernel_), verbosity_(p.verbosity_) {}
IterativeOptimizationParameters(const Kernel kernel = CG, const Verbosity verbosity = SILENT)
: kernel_(kernel), verbosity_(verbosity) {}
IterativeOptimizationParameters(Verbosity v = SILENT)
: verbosity_(v) {}
virtual ~IterativeOptimizationParameters() {}
/* general interface */
inline Kernel kernel() const { return kernel_; }
/* utility */
inline Verbosity verbosity() const { return verbosity_; }
/* matlab interface */
std::string getKernel() const ;
std::string getVerbosity() const;
void setKernel(const std::string &s) ;
void setVerbosity(const std::string &s) ;
virtual void print() const {
std::cout << "IterativeOptimizationParameters" << std::endl
<< "kernel: " << kernelTranslator(kernel_) << std::endl
<< "verbosity: " << verbosityTranslator(verbosity_) << std::endl;
}
/* matlab interface */
void print() const ;
/* virtual print function */
virtual void print(std::ostream &os) const ;
/* for serialization */
friend std::ostream& operator<<(std::ostream &os, const IterativeOptimizationParameters &p);
static Kernel kernelTranslator(const std::string &s);
static Verbosity verbosityTranslator(const std::string &s);
static std::string kernelTranslator(Kernel k);
static std::string verbosityTranslator(Verbosity v);
};
/************************************************************************************/
class GTSAM_EXPORT IterativeSolver {
public:
typedef boost::shared_ptr<IterativeSolver> shared_ptr;
IterativeSolver() {}
virtual ~IterativeSolver() {}
/* interface to the nonlinear optimizer */
virtual VectorValues optimize () = 0;
/* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
VectorValues optimize (
const GaussianFactorGraph &gfg,
boost::optional<const KeyInfo&> = boost::none,
boost::optional<const std::map<Key, Vector>&> lambda = boost::none
);
/* interface to the nonlinear optimizer */
virtual VectorValues optimize (const VectorValues &initial) = 0;
/* interface to the nonlinear optimizer, without initial estimate */
VectorValues optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda
);
/* interface to the nonlinear optimizer that the subclasses have to implement */
virtual VectorValues optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda,
const VectorValues &initial
) = 0;
/* update interface to the nonlinear optimizer */
virtual void replaceFactors(const boost::shared_ptr<GaussianFactorGraph> &factorGraph, const double lambda) {}
};
/************************************************************************************/
/* Handy data structure for iterative solvers
* key to (index, dimension, colstart) */
class GTSAM_EXPORT KeyInfoEntry : public boost::tuple<size_t, size_t, size_t> {
public:
typedef boost::tuple<Key,size_t,Key> Base;
KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {}
const size_t index() const { return this->get<0>(); }
const size_t dim() const { return this->get<1>(); }
const size_t colstart() const { return this->get<2>(); }
};
/************************************************************************************/
class GTSAM_EXPORT KeyInfo : public std::map<Key, KeyInfoEntry> {
public:
typedef std::map<Key, KeyInfoEntry> Base;
KeyInfo() : numCols_(0) {}
KeyInfo(const GaussianFactorGraph &fg);
KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering);
std::vector<size_t> colSpec() const ;
VectorValues x0() const;
Vector x0vector() const;
inline size_t numCols() const { return numCols_; }
inline const Ordering & ordering() const { return ordering_; }
protected:
void initialize(const GaussianFactorGraph &fg);
Ordering ordering_;
size_t numCols_;
};
}

201
gtsam/linear/PCGSolver.cpp Normal file
View File

@ -0,0 +1,201 @@
/*
* PCGSolver.cpp
*
* Created on: Feb 14, 2012
* Author: ydjian
*/
#include <gtsam/linear/GaussianFactorGraph.h>
//#include <gtsam/inference/FactorGraph-inst.h>
//#include <gtsam/linear/FactorGraphUtil-inl.h>
//#include <gtsam/linear/JacobianFactorGraph.h>
//#include <gtsam/linear/LSPCGSolver.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/Preconditioner.h>
//#include <gtsam/linear/SuiteSparseUtil.h>
//#include <gtsam/linear/ConjugateGradientMethod-inl.h>
//#include <gsp2/gtsam-interface-sbm.h>
//#include <ydjian/tool/ThreadSafeTimer.h>
#include <boost/algorithm/string.hpp>
#include <iostream>
#include <stdexcept>
using namespace std;
namespace gtsam {
/*****************************************************************************/
void PCGSolverParameters::print(ostream &os) const {
Base::print(os);
os << "PCGSolverParameters:" << endl;
preconditioner_->print(os);
}
/*****************************************************************************/
PCGSolver::PCGSolver(const PCGSolverParameters &p) {
preconditioner_ = createPreconditioner(p.preconditioner_);
}
/*****************************************************************************/
VectorValues PCGSolver::optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda,
const VectorValues &initial)
{
/* build preconditioner */
preconditioner_->build(gfg, keyInfo, lambda);
/* apply pcg */
const Vector sol = preconditionedConjugateGradient<GaussianFactorGraphSystem, Vector>(
GaussianFactorGraphSystem(gfg, *preconditioner_, keyInfo, lambda),
initial.vector(keyInfo.ordering()), parameters_);
return buildVectorValues(sol, keyInfo);
}
/*****************************************************************************/
GaussianFactorGraphSystem::GaussianFactorGraphSystem(
const GaussianFactorGraph &gfg,
const Preconditioner &preconditioner,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda)
: gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {}
/*****************************************************************************/
void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const {
/* implement b-Ax, assume x and r are pre-allocated */
/* reset r to b */
getb(r);
/* substract A*x */
Vector Ax = Vector::Zero(r.rows(), 1);
multiply(x, Ax);
r -= Ax ;
}
/*****************************************************************************/
void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const {
/* implement Ax, assume x and Ax are pre-allocated */
/* reset y */
Ax.setZero();
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
/* accumulate At A x*/
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
const Matrix Ai = jf->getA(it);
/* this map lookup should be replaced */
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
Ax.segment(entry.colstart(), entry.dim())
+= Ai.transpose() * (Ai * x.segment(entry.colstart(), entry.dim()));
}
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
/* accumulate H x */
/* use buffer to avoid excessive table lookups */
const size_t sz = hf->size();
vector<Vector> y;
y.reserve(sz);
for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) {
/* initialize y to zeros */
y.push_back(zero(hf->getDim(it)));
}
for (HessianFactor::const_iterator j = hf->begin(); j != hf->end(); j++ ) {
/* retrieve the key mapping */
const KeyInfoEntry &entry = keyInfo_.find(*j)->second;
// xj is the input vector
const Vector xj = x.segment(entry.colstart(), entry.dim());
size_t idx = 0;
for (HessianFactor::const_iterator i = hf->begin(); i != hf->end(); i++, idx++ ) {
if ( i == j ) y[idx] += hf->info(j, j).selfadjointView() * xj;
else y[idx] += hf->info(i, j).knownOffDiagonal() * xj;
}
}
/* accumulate to r */
for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) {
/* retrieve the key mapping */
const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second;
Ax.segment(entry.colstart(), entry.dim()) += y[i];
}
}
else {
throw invalid_argument("GaussianFactorGraphSystem::multiply gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
}
}
/*****************************************************************************/
void GaussianFactorGraphSystem::getb(Vector &b) const {
/* compute rhs, assume b pre-allocated */
/* reset */
b.setZero();
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
const Vector rhs = jf->getb();
/* accumulate At rhs */
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
/* this map lookup should be replaced */
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ;
}
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
/* accumulate g */
for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) {
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it);
}
}
else {
throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
}
}
/**********************************************************************************/
void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const
{ preconditioner_.transposeSolve(x, y); }
/**********************************************************************************/
void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const
{ preconditioner_.solve(x, y); }
/**********************************************************************************/
VectorValues buildVectorValues(const Vector &v,
const Ordering &ordering,
const map<Key, size_t> & dimensions) {
VectorValues result;
DenseIndex offset = 0;
for ( size_t i = 0 ; i < ordering.size() ; ++i ) {
const Key &key = ordering[i];
map<Key, size_t>::const_iterator it = dimensions.find(key);
if ( it == dimensions.end() ) {
throw invalid_argument("buildVectorValues: inconsistent ordering and dimensions");
}
const size_t dim = it->second;
result.insert(key, v.segment(offset, dim));
offset += dim;
}
return result;
}
/**********************************************************************************/
VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) {
VectorValues result;
BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) {
result.insert(item.first, v.segment(item.second.colstart(), item.second.dim()));
}
return result;
}
}

109
gtsam/linear/PCGSolver.h Normal file
View File

@ -0,0 +1,109 @@
/*
* PCGSolver.h
*
* Created on: Jan 31, 2012
* Author: Yong-Dian Jian
*/
#pragma once
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/shared_ptr.hpp>
#include <algorithm>
#include <iosfwd>
#include <map>
#include <string>
namespace gtsam {
class GaussianFactorGraph;
class KeyInfo;
class Preconditioner;
class PreconditionerParameters;
/*****************************************************************************/
struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters {
public:
typedef ConjugateGradientParameters Base;
typedef boost::shared_ptr<PCGSolverParameters> shared_ptr;
PCGSolverParameters() {}
virtual void print(std::ostream &os) const;
/* interface to preconditioner parameters */
inline const PreconditionerParameters& preconditioner() const {
return *preconditioner_;
}
boost::shared_ptr<PreconditionerParameters> preconditioner_;
};
/*****************************************************************************/
/* A virtual base class for the preconditioned conjugate gradient solver */
class GTSAM_EXPORT PCGSolver: public IterativeSolver {
public:
typedef IterativeSolver Base;
typedef boost::shared_ptr<PCGSolver> shared_ptr;
protected:
PCGSolverParameters parameters_;
boost::shared_ptr<Preconditioner> preconditioner_;
public:
/* Interface to initialize a solver without a problem */
PCGSolver(const PCGSolverParameters &p);
virtual ~PCGSolver() {}
using IterativeSolver::optimize;
virtual VectorValues optimize(const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
const VectorValues &initial);
};
/*****************************************************************************/
class GTSAM_EXPORT GaussianFactorGraphSystem {
public:
GaussianFactorGraphSystem(const GaussianFactorGraph &gfg,
const Preconditioner &preconditioner, const KeyInfo &info,
const std::map<Key, Vector> &lambda);
const GaussianFactorGraph &gfg_;
const Preconditioner &preconditioner_;
const KeyInfo &keyInfo_;
const std::map<Key, Vector> &lambda_;
void residual(const Vector &x, Vector &r) const;
void multiply(const Vector &x, Vector& y) const;
void leftPrecondition(const Vector &x, Vector &y) const;
void rightPrecondition(const Vector &x, Vector &y) const;
inline void scal(const double alpha, Vector &x) const {
x *= alpha;
}
inline double dot(const Vector &x, const Vector &y) const {
return x.dot(y);
}
inline void axpy(const double alpha, const Vector &x, Vector &y) const {
y += alpha * x;
}
void getb(Vector &b) const;
};
/* utility functions */
/**********************************************************************************/
VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
const std::map<Key, size_t> & dimensions);
/**********************************************************************************/
VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo);
}

214
gtsam/linear/Preconditioner.cpp Normal file
View File

@ -0,0 +1,214 @@
/*
* Preconditioner.cpp
*
* Created on: Jun 2, 2014
* Author: ydjian
*/
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/NoiseModel.h>
#include <boost/shared_ptr.hpp>
#include <boost/algorithm/string.hpp>
#include <iostream>
#include <vector>
using namespace std;
namespace gtsam {
/*****************************************************************************/
void PreconditionerParameters::print() const {
print(cout);
}
/***************************************************************************************/
void PreconditionerParameters::print(ostream &os) const {
os << "PreconditionerParameters" << endl
<< "kernel: " << kernelTranslator(kernel_) << endl
<< "verbosity: " << verbosityTranslator(verbosity_) << endl;
}
/*****************************************************************************/
ostream& operator<<(ostream &os, const PreconditionerParameters &p) {
p.print(os);
return os;
}
/***************************************************************************************/
PreconditionerParameters::Kernel PreconditionerParameters::kernelTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "GTSAM") return PreconditionerParameters::GTSAM;
else if (s == "CHOLMOD") return PreconditionerParameters::CHOLMOD;
/* default is cholmod */
else return PreconditionerParameters::CHOLMOD;
}
/***************************************************************************************/
PreconditionerParameters::Verbosity PreconditionerParameters::verbosityTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "SILENT") return PreconditionerParameters::SILENT;
else if (s == "COMPLEXITY") return PreconditionerParameters::COMPLEXITY;
else if (s == "ERROR") return PreconditionerParameters::ERROR;
/* default is default */
else return PreconditionerParameters::SILENT;
}
/***************************************************************************************/
std::string PreconditionerParameters::kernelTranslator(PreconditionerParameters::Kernel k) {
if ( k == GTSAM ) return "GTSAM";
if ( k == CHOLMOD ) return "CHOLMOD";
else return "UNKNOWN";
}
/***************************************************************************************/
std::string PreconditionerParameters::verbosityTranslator(PreconditionerParameters::Verbosity verbosity) {
if (verbosity == SILENT) return "SILENT";
else if (verbosity == COMPLEXITY) return "COMPLEXITY";
else if (verbosity == ERROR) return "ERROR";
else return "UNKNOWN";
}
/***************************************************************************************/
BlockJacobiPreconditioner::BlockJacobiPreconditioner()
: Base(), buffer_(0), bufferSize_(0), nnz_(0) {}
/***************************************************************************************/
BlockJacobiPreconditioner::~BlockJacobiPreconditioner() { clean(); }
/***************************************************************************************/
void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const {
const size_t n = dims_.size();
double *ptr = buffer_, *dst = x.data();
std::copy(y.data(), y.data() + y.rows(), x.data());
for ( size_t i = 0 ; i < n ; ++i ) {
const size_t d = dims_[i];
const size_t sz = d*d;
const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
R.triangularView<Eigen::Upper>().solveInPlace(b);
dst += d;
ptr += sz;
}
}
/***************************************************************************************/
void BlockJacobiPreconditioner::transposeSolve(const Vector& y, Vector& x) const {
const size_t n = dims_.size();
double *ptr = buffer_, *dst = x.data();
std::copy(y.data(), y.data() + y.rows(), x.data());
for ( size_t i = 0 ; i < n ; ++i ) {
const size_t d = dims_[i];
const size_t sz = d*d;
const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
R.transpose().triangularView<Eigen::Upper>().solveInPlace(b);
dst += d;
ptr += sz;
}
}
/***************************************************************************************/
void BlockJacobiPreconditioner::build(
const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
{
const size_t n = keyInfo.size();
dims_ = keyInfo.colSpec();
/* prepare the buffer of block diagonals */
std::vector<Matrix> blocks; blocks.reserve(n);
/* allocate memory for the factorization of block diagonals */
size_t nnz = 0;
for ( size_t i = 0 ; i < n ; ++i ) {
const size_t dim = dims_[i];
blocks.push_back(Matrix::Zero(dim, dim));
// nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ;
nnz += dim*dim;
}
/* compute the block diagonal by scanning over the factors */
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
const KeyInfoEntry &entry = keyInfo.find(*it)->second;
const Matrix &Ai = jf->getA(it);
blocks[entry.index()] += (Ai.transpose() * Ai);
}
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) {
const KeyInfoEntry &entry = keyInfo.find(*it)->second;
const Matrix &Hii = hf->info(it, it).selfadjointView();
blocks[entry.index()] += Hii;
}
}
else {
throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
}
/* if necessary, allocating the memory for cacheing the factorization results */
if ( nnz > bufferSize_ ) {
clean();
buffer_ = new double [nnz];
bufferSize_ = nnz;
}
nnz_ = nnz;
/* factorizing the blocks respectively */
double *ptr = buffer_;
for ( size_t i = 0 ; i < n ; ++i ) {
/* use eigen to decompose Di */
const Matrix R = blocks[i].llt().matrixL().transpose();
/* store the data in the buffer */
size_t sz = dims_[i]*dims_[i] ;
std::copy(R.data(), R.data() + sz, ptr);
/* advance the pointer */
ptr += sz;
}
}
/*****************************************************************************/
void BlockJacobiPreconditioner::clean() {
if ( buffer_ ) {
delete [] buffer_;
buffer_ = 0;
bufferSize_ = 0;
nnz_ = 0;
}
}
/***************************************************************************************/
boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters) {
if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast<DummyPreconditionerParameters>(parameters) ) {
return boost::make_shared<DummyPreconditioner>();
}
else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(parameters) ) {
return boost::make_shared<BlockJacobiPreconditioner>();
}
else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast<SubgraphPreconditionerParameters>(parameters) ) {
return boost::make_shared<SubgraphPreconditioner>(*subgraph);
}
throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type");
}
}

170
gtsam/linear/Preconditioner.h Normal file
View File

@ -0,0 +1,170 @@
/*
* Preconditioner.h
*
* Created on: Jun 2, 2014
* Author: ydjian
*/
#pragma once
#include <gtsam/base/Vector.h>
#include <boost/shared_ptr.hpp>
#include <iosfwd>
#include <map>
#include <string>
namespace gtsam {
class GaussianFactorGraph;
class KeyInfo;
class VectorValues;
/* parameters for the preconditioner */
struct GTSAM_EXPORT PreconditionerParameters {
typedef boost::shared_ptr<PreconditionerParameters> shared_ptr;
enum Kernel { /* Preconditioner Kernel */
GTSAM = 0,
CHOLMOD /* experimental */
} kernel_ ;
enum Verbosity {
SILENT = 0,
COMPLEXITY = 1,
ERROR = 2
} verbosity_ ;
PreconditionerParameters(): kernel_(GTSAM), verbosity_(SILENT) {}
PreconditionerParameters(const PreconditionerParameters &p) : kernel_(p.kernel_), verbosity_(p.verbosity_) {}
virtual ~PreconditionerParameters() {}
/* general interface */
inline Kernel kernel() const { return kernel_; }
inline Verbosity verbosity() const { return verbosity_; }
void print() const ;
virtual void print(std::ostream &os) const ;
static Kernel kernelTranslator(const std::string &s);
static Verbosity verbosityTranslator(const std::string &s);
static std::string kernelTranslator(Kernel k);
static std::string verbosityTranslator(Verbosity v);
/* for serialization */
friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p);
};
/* PCG aims to solve the problem: A x = b by reparametrizing it as
* S^t A S y = S^t b or M A x = M b, where A \approx S S, or A \approx M
* The goal of this class is to provide a general interface to all preconditioners */
class GTSAM_EXPORT Preconditioner {
public:
typedef boost::shared_ptr<Preconditioner> shared_ptr;
typedef std::vector<size_t> Dimensions;
/* Generic Constructor and Destructor */
Preconditioner() {}
virtual ~Preconditioner() {}
/* Computation Interfaces */
/* implement x = S^{-1} y */
virtual void solve(const Vector& y, Vector &x) const = 0;
// virtual void solve(const VectorValues& y, VectorValues &x) const = 0;
/* implement x = S^{-T} y */
virtual void transposeSolve(const Vector& y, Vector& x) const = 0;
// virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0;
// /* implement x = S^{-1} S^{-T} y */
// virtual void fullSolve(const Vector& y, Vector &x) const = 0;
// virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0;
/* build/factorize the preconditioner */
virtual void build(
const GaussianFactorGraph &gfg,
const KeyInfo &info,
const std::map<Key,Vector> &lambda
) = 0;
};
/*******************************************************************************************/
struct GTSAM_EXPORT DummyPreconditionerParameters : public PreconditionerParameters {
typedef PreconditionerParameters Base;
typedef boost::shared_ptr<DummyPreconditionerParameters> shared_ptr;
DummyPreconditionerParameters() : Base() {}
virtual ~DummyPreconditionerParameters() {}
};
/*******************************************************************************************/
class GTSAM_EXPORT DummyPreconditioner : public Preconditioner {
public:
typedef Preconditioner Base;
typedef boost::shared_ptr<DummyPreconditioner> shared_ptr;
public:
DummyPreconditioner() : Base() {}
virtual ~DummyPreconditioner() {}
/* Computation Interfaces for raw vector */
virtual void solve(const Vector& y, Vector &x) const { x = y; }
// virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; }
virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; }
// virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; }
// virtual void fullSolve(const Vector& y, Vector &x) const { x = y; }
// virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; }
virtual void build(
const GaussianFactorGraph &gfg,
const KeyInfo &info,
const std::map<Key,Vector> &lambda
) {}
};
/*******************************************************************************************/
struct GTSAM_EXPORT BlockJacobiPreconditionerParameters : public PreconditionerParameters {
typedef PreconditionerParameters Base;
BlockJacobiPreconditionerParameters() : Base() {}
virtual ~BlockJacobiPreconditionerParameters() {}
};
/*******************************************************************************************/
class GTSAM_EXPORT BlockJacobiPreconditioner : public Preconditioner {
public:
typedef Preconditioner Base;
BlockJacobiPreconditioner() ;
virtual ~BlockJacobiPreconditioner() ;
/* Computation Interfaces for raw vector */
virtual void solve(const Vector& y, Vector &x) const;
virtual void transposeSolve(const Vector& y, Vector& x) const ;
// virtual void fullSolve(const Vector& y, Vector &x) const ;
virtual void build(
const GaussianFactorGraph &gfg,
const KeyInfo &info,
const std::map<Key,Vector> &lambda
) ;
protected:
void clean() ;
std::vector<size_t> dims_;
double *buffer_;
size_t bufferSize_;
size_t nnz_;
};
/*********************************************************************************************/
/* factory method to create preconditioners */
boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters);
}

600
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -15,17 +15,29 @@
* @author: Frank Dellaert
*/
#include <gtsam/base/DSFVector.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <fstream>
#include <iostream>
#include <numeric>
#include <queue>
#include <set>
#include <utility>
#include <vector>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
/* ************************************************************************* */
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
@ -35,52 +47,445 @@ namespace gtsam {
result->push_back(jf);
}
return result;
}
/*****************************************************************************/
static std::vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
/* compute the sum of the weights */
const double sum = std::accumulate(weight.begin(), weight.end(), 0.0);
/* make a normalized and accumulated version of the weight vector */
const size_t m = weight.size();
vector<double> w; w.reserve(m);
for ( size_t i = 0 ; i < m ; ++i ) {
w.push_back(weight[i]/sum);
}
/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
const sharedBayesNet& Rc1, const sharedValues& xbar) :
Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))) {
vector<double> acc(m);
std::partial_sum(w.begin(),w.end(),acc.begin());
/* iid sample n times */
vector<size_t> result; result.reserve(n);
const double denominator = (double)RAND_MAX;
for ( size_t i = 0 ; i < n ; ++i ) {
const double value = rand() / denominator;
/* binary search the interval containing "value" */
vector<double>::iterator it = std::lower_bound(acc.begin(), acc.end(), value);
size_t idx = it - acc.begin();
result.push_back(idx);
}
return result;
}
/*****************************************************************************/
vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
const size_t m = weight.size();
if ( n > m ) throw std::invalid_argument("uniqueSampler: invalid input size");
vector<size_t> result;
size_t count = 0;
std::vector<bool> touched(m, false);
while ( count < n ) {
std::vector<size_t> localIndices; localIndices.reserve(n-count);
std::vector<double> localWeights; localWeights.reserve(n-count);
/* collect data */
for ( size_t i = 0 ; i < m ; ++i ) {
if ( !touched[i] ) {
localIndices.push_back(i);
localWeights.push_back(weight[i]);
}
}
/* ************************************************************************* */
// x = xbar + inv(R1)*y
VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
/* sampling and cache results */
vector<size_t> samples = iidSampler(localWeights, n-count);
BOOST_FOREACH ( const size_t &id, samples ) {
if ( touched[id] == false ) {
touched[id] = true ;
result.push_back(id);
if ( ++count >= n ) break;
}
}
}
return result;
}
/****************************************************************************/
Subgraph::Subgraph(const std::vector<size_t> &indices) {
edges_.reserve(indices.size());
BOOST_FOREACH ( const size_t &idx, indices ) {
edges_.push_back(SubgraphEdge(idx, 1.0));
}
}
/****************************************************************************/
std::vector<size_t> Subgraph::edgeIndices() const {
std::vector<size_t> eid; eid.reserve(size());
BOOST_FOREACH ( const SubgraphEdge &edge, edges_ ) {
eid.push_back(edge.index_);
}
return eid;
}
/****************************************************************************/
void Subgraph::save(const std::string &fn) const {
std::ofstream os(fn.c_str());
boost::archive::text_oarchive oa(os);
oa << *this;
os.close();
}
/****************************************************************************/
Subgraph::shared_ptr Subgraph::load(const std::string &fn) {
std::ifstream is(fn.c_str());
boost::archive::text_iarchive ia(is);
Subgraph::shared_ptr subgraph(new Subgraph());
ia >> *subgraph;
is.close();
return subgraph;
}
/****************************************************************************/
std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) {
if ( edge.weight() != 1.0 )
os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")";
else
os << edge.index() ;
return os;
}
/****************************************************************************/
std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) {
os << "Subgraph" << endl;
BOOST_FOREACH ( const SubgraphEdge &e, subgraph.edges() ) {
os << e << ", " ;
}
return os;
}
/*****************************************************************************/
void SubgraphBuilderParameters::print() const {
print(cout);
}
/***************************************************************************************/
void SubgraphBuilderParameters::print(ostream &os) const {
os << "SubgraphBuilderParameters" << endl
<< "skeleton: " << skeletonTranslator(skeleton_) << endl
<< "skeleton weight: " << skeletonWeightTranslator(skeletonWeight_) << endl
<< "augmentation weight: " << augmentationWeightTranslator(augmentationWeight_) << endl
;
}
/*****************************************************************************/
ostream& operator<<(ostream &os, const SubgraphBuilderParameters &p) {
p.print(os);
return os;
}
/*****************************************************************************/
SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslator(const std::string &src){
std::string s = src; boost::algorithm::to_upper(s);
if (s == "NATURALCHAIN") return NATURALCHAIN;
else if (s == "BFS") return BFS;
else if (s == "KRUSKAL") return KRUSKAL;
throw invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
return KRUSKAL;
}
/****************************************************************/
std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton w) {
if ( w == NATURALCHAIN )return "NATURALCHAIN";
else if ( w == BFS ) return "BFS";
else if ( w == KRUSKAL )return "KRUSKAL";
else return "UNKNOWN";
}
/****************************************************************/
SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "EQUAL") return EQUAL;
else if (s == "RHS") return RHS_2NORM;
else if (s == "LHS") return LHS_FNORM;
else if (s == "RANDOM") return RANDOM;
throw invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s);
return EQUAL;
}
/****************************************************************/
std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w) {
if ( w == EQUAL ) return "EQUAL";
else if ( w == RHS_2NORM ) return "RHS";
else if ( w == LHS_FNORM ) return "LHS";
else if ( w == RANDOM ) return "RANDOM";
else return "UNKNOWN";
}
/****************************************************************/
SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "SKELETON") return SKELETON;
// else if (s == "STRETCH") return STRETCH;
// else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH;
throw invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s);
return SKELETON;
}
/****************************************************************/
std::string SubgraphBuilderParameters::augmentationWeightTranslator(AugmentationWeight w) {
if ( w == SKELETON ) return "SKELETON";
// else if ( w == STRETCH ) return "STRETCH";
// else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH";
else return "UNKNOWN";
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
const SubgraphBuilderParameters &p = parameters_;
switch (p.skeleton_) {
case SubgraphBuilderParameters::NATURALCHAIN:
return natural_chain(gfg);
break;
case SubgraphBuilderParameters::BFS:
return bfs(gfg);
break;
case SubgraphBuilderParameters::KRUSKAL:
return kruskal(gfg, ordering, w);
break;
default:
cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
break;
}
return vector<size_t>();
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
std::vector<size_t> result ;
size_t idx = 0;
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
if ( gf->size() == 1 ) {
result.push_back(idx);
}
idx++;
}
return result;
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
std::vector<size_t> result ;
size_t idx = 0;
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
if ( gf->size() == 2 ) {
const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
if ( (k1-k0) == 1 || (k0-k1) == 1 )
result.push_back(idx);
}
idx++;
}
return result;
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
const VariableIndex variableIndex(gfg);
/* start from the first key of the first factor */
Key seed = gfg[0]->keys()[0];
const size_t n = variableIndex.size();
/* each vertex has self as the predecessor */
std::vector<size_t> result;
result.reserve(n-1);
/* Initialize */
std::queue<size_t> q;
q.push(seed);
std::set<size_t> flags;
flags.insert(seed);
/* traversal */
while ( !q.empty() ) {
const size_t head = q.front(); q.pop();
BOOST_FOREACH ( const size_t id, variableIndex[head] ) {
const GaussianFactor &gf = *gfg[id];
BOOST_FOREACH ( const size_t key, gf.keys() ) {
if ( flags.count(key) == 0 ) {
q.push(key);
flags.insert(key);
result.push_back(id);
}
}
}
}
return result;
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
const VariableIndex variableIndex(gfg);
const size_t n = variableIndex.size();
const vector<size_t> idx = sort_idx(w) ;
/* initialize buffer */
vector<size_t> result;
result.reserve(n-1);
// container for acsendingly sorted edges
DSFVector D(n) ;
size_t count = 0 ; double sum = 0.0 ;
BOOST_FOREACH (const size_t id, idx) {
const GaussianFactor &gf = *gfg[id];
if ( gf.keys().size() != 2 ) continue;
const size_t u = ordering.find(gf.keys()[0])->second,
u_root = D.find(u),
v = ordering.find(gf.keys()[1])->second,
v_root = D.find(v) ;
if ( u_root != v_root ) {
D.merge(u_root, v_root) ;
result.push_back(id) ;
sum += w[id] ;
if ( ++count == n-1 ) break ;
}
}
return result;
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::sample(const std::vector<double> &weights, const size_t t) const {
return uniqueSampler(weights, t);
}
/****************************************************************/
Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const {
const SubgraphBuilderParameters &p = parameters_;
const Ordering inverse_ordering = Ordering::Natural(gfg);
const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
const size_t n = inverse_ordering.size(), t = n * p.complexity_ ;
vector<double> w = weights(gfg);
const vector<size_t> tree = buildTree(gfg, forward_ordering, w);
/* sanity check */
if ( tree.size() != n-1 ) {
throw runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed ");
}
/* down weight the tree edges to zero */
BOOST_FOREACH ( const size_t id, tree ) {
w[id] = 0.0;
}
/* decide how many edges to augment */
std::vector<size_t> offTree = sample(w, t);
vector<size_t> subgraph = unary(gfg);
subgraph.insert(subgraph.end(), tree.begin(), tree.end());
subgraph.insert(subgraph.end(), offTree.begin(), offTree.end());
return boost::make_shared<Subgraph>(subgraph);
}
/****************************************************************/
SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg) const
{
const size_t m = gfg.size() ;
Weights weight; weight.reserve(m);
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg ) {
switch ( parameters_.skeletonWeight_ ) {
case SubgraphBuilderParameters::EQUAL:
weight.push_back(1.0);
break;
case SubgraphBuilderParameters::RHS_2NORM:
{
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
weight.push_back(jf->getb().norm());
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
weight.push_back(hf->linearTerm().norm());
}
}
break;
case SubgraphBuilderParameters::LHS_FNORM:
{
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
weight.push_back(std::sqrt(jf->getA().squaredNorm()));
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
weight.push_back(std::sqrt(hf->information().squaredNorm()));
}
}
break;
case SubgraphBuilderParameters::RANDOM:
weight.push_back(std::rand()%100 + 1.0);
break;
default:
throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme ");
break;
}
}
return weight;
}
/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) :
parameters_(p) {}
/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
const sharedBayesNet& Rc1, const sharedValues& xbar, const SubgraphPreconditionerParameters &p) :
Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar),
b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))), parameters_(p) {
}
/* ************************************************************************* */
// x = xbar + inv(R1)*y
VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
return *xbar_ + Rc1_->backSubstitute(y);
}
}
/* ************************************************************************* */
double SubgraphPreconditioner::error(const VectorValues& y) const {
/* ************************************************************************* */
double SubgraphPreconditioner::error(const VectorValues& y) const {
Errors e(y);
VectorValues x = this->x(y);
Errors e2 = Ab2()->gaussianErrors(x);
return 0.5 * (dot(e, e) + dot(e2,e2));
}
}
/* ************************************************************************* */
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const {
/* ************************************************************************* */
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const {
VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */
Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */
VectorValues v = VectorValues::Zero(x);
Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */
return y + Rc1()->backSubstituteTranspose(v);
}
}
/* ************************************************************************* */
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
/* ************************************************************************* */
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
Errors e(y);
VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */
Errors e2 = *Ab2() * x; /* A2*x */
e.splice(e.end(), e2);
return e;
}
}
/* ************************************************************************* */
// In-place version that overwrites e
void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
/* ************************************************************************* */
// In-place version that overwrites e
void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
Errors::iterator ei = e.begin();
for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) {
@ -90,11 +495,11 @@ namespace gtsam {
// Add A2 contribution
VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y
Ab2()->multiplyInPlace(x, ei); // use iterator version
}
}
/* ************************************************************************* */
// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {
/* ************************************************************************* */
// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {
Errors::const_iterator it = e.begin();
VectorValues y = zero();
@ -102,12 +507,12 @@ namespace gtsam {
y[i] = *it ;
transposeMultiplyAdd2(1.0,it,e.end(),y);
return y;
}
}
/* ************************************************************************* */
// y += alpha*A'*e
void SubgraphPreconditioner::transposeMultiplyAdd
(double alpha, const Errors& e, VectorValues& y) const {
/* ************************************************************************* */
// y += alpha*A'*e
void SubgraphPreconditioner::transposeMultiplyAdd
(double alpha, const Errors& e, VectorValues& y) const {
Errors::const_iterator it = e.begin();
for ( Key i = 0 ; i < y.size() ; ++i, ++it ) {
@ -115,11 +520,11 @@ namespace gtsam {
axpy(alpha, ei, y[i]);
}
transposeMultiplyAdd2(alpha, it, e.end(), y);
}
}
/* ************************************************************************* */
// y += alpha*inv(R1')*A2'*e2
void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
/* ************************************************************************* */
// y += alpha*inv(R1')*A2'*e2
void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const {
// create e2 with what's left of e
@ -130,11 +535,126 @@ namespace gtsam {
VectorValues x = VectorValues::Zero(y); // x = 0
Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2
axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x
}
}
/* ************************************************************************* */
void SubgraphPreconditioner::print(const std::string& s) const {
/* ************************************************************************* */
void SubgraphPreconditioner::print(const std::string& s) const {
cout << s << endl;
Ab2_->print();
}
/*****************************************************************************/
void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const
{
/* copy first */
std::copy(y.data(), y.data() + y.rows(), x.data());
/* in place back substitute */
BOOST_REVERSE_FOREACH(const boost::shared_ptr<GaussianConditional> & cg, *Rc1_) {
/* collect a subvector of x that consists of the parents of cg (S) */
const Vector xParent = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginParents(), cg->endParents()));
const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
/* compute the solution for the current pivot */
const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(rhsFrontal - cg->get_S() * xParent);
/* assign subvector of sol to the frontal variables */
setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
}
}
/*****************************************************************************/
void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
{
/* copy first */
std::copy(y.data(), y.data() + y.rows(), x.data());
/* in place back substitute */
BOOST_FOREACH(const boost::shared_ptr<GaussianConditional> & cg, *Rc1_) {
const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
// const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal);
const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal);
// Check for indeterminant solution
if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front());
/* assign subvector of sol to the frontal variables */
setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
/* substract from parent variables */
for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) {
KeyInfo::const_iterator it2 = keyInfo_.find(*it);
Eigen::Map<Vector> rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1);
rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal;
}
}
}
/*****************************************************************************/
void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
{
/* identify the subgraph structure */
const SubgraphBuilder builder(parameters_.builderParams_);
Subgraph::shared_ptr subgraph = builder(gfg);
keyInfo_ = keyInfo;
/* build factor subgraph */
GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, *subgraph, true);
/* factorize and cache BayesNet */
Rc1_ = gfg_subgraph->eliminateSequential();
}
/*****************************************************************************/
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys) {
/* a cache of starting index and dim */
typedef vector<pair<size_t, size_t> > Cache;
Cache cache;
/* figure out dimension by traversing the keys */
size_t d = 0;
BOOST_FOREACH ( const Key &key, keys ) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
cache.push_back(make_pair<size_t,size_t>(entry.colstart(), entry.dim()));
d += entry.dim();
}
/* use the cache to fill the result */
Vector result = Vector::Zero(d, 1);
size_t idx = 0;
BOOST_FOREACH ( const Cache::value_type &p, cache ) {
result.segment(idx, p.second) = src.segment(p.first, p.second) ;
idx += p.second;
}
return result;
}
/*****************************************************************************/
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst) {
/* use the cache */
size_t idx = 0;
BOOST_FOREACH ( const Key &key, keys ) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ;
idx += entry.dim();
}
}
/*****************************************************************************/
boost::shared_ptr<GaussianFactorGraph>
buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
result->reserve(subgraph.size());
BOOST_FOREACH ( const SubgraphEdge &e, subgraph ) {
const size_t idx = e.index();
if ( clone ) result->push_back(gfg[idx]->clone());
else result->push_back(gfg[idx]);
}
return result;
}
} // nsamespace gtsam

208
gtsam/linear/SubgraphPreconditioner.h
View File

@ -12,15 +12,16 @@
/**
* @file SubgraphPreconditioner.h
* @date Dec 31, 2009
* @author Frank Dellaert
* @author Frank Dellaert, Yong-Dian Jian
*/
#pragma once
#include <gtsam/global_includes.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/Errors.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/shared_ptr.hpp>
namespace gtsam {
@ -30,6 +31,146 @@ namespace gtsam {
class GaussianFactorGraph;
class VectorValues;
struct GTSAM_EXPORT SubgraphEdge {
size_t index_; /* edge id */
double weight_; /* edge weight */
SubgraphEdge() : index_(0), weight_(1.0) {}
SubgraphEdge(const SubgraphEdge &e) : index_(e.index()), weight_(e.weight()) {}
SubgraphEdge(const size_t index, const double weight = 1.0): index_(index), weight_(weight) {}
inline size_t index() const { return index_; }
inline double weight() const { return weight_; }
inline bool isUnitWeight() const { return (weight_ == 1.0); }
friend std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge);
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(index_);
ar & BOOST_SERIALIZATION_NVP(weight_);
}
};
/**************************************************************************/
class GTSAM_EXPORT Subgraph {
public:
typedef boost::shared_ptr<Subgraph> shared_ptr;
typedef std::vector<shared_ptr> vector_shared_ptr;
typedef std::vector<SubgraphEdge> Edges;
typedef std::vector<size_t> EdgeIndices;
typedef Edges::iterator iterator;
typedef Edges::const_iterator const_iterator;
protected:
Edges edges_; /* index to the factors */
public:
Subgraph() {}
Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {}
Subgraph(const Edges &edges) : edges_(edges) {}
Subgraph(const std::vector<size_t> &indices) ;
inline const Edges& edges() const { return edges_; }
inline const size_t size() const { return edges_.size(); }
EdgeIndices edgeIndices() const;
iterator begin() { return edges_.begin(); }
const_iterator begin() const { return edges_.begin(); }
iterator end() { return edges_.end(); }
const_iterator end() const { return edges_.end(); }
void save(const std::string &fn) const;
static shared_ptr load(const std::string &fn);
friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph);
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(edges_);
}
};
/****************************************************************************/
struct GTSAM_EXPORT SubgraphBuilderParameters {
public:
typedef boost::shared_ptr<SubgraphBuilderParameters> shared_ptr;
enum Skeleton {
/* augmented tree */
NATURALCHAIN = 0, /* natural ordering of the graph */
BFS, /* breadth-first search tree */
KRUSKAL, /* maximum weighted spanning tree */
} skeleton_ ;
enum SkeletonWeight { /* how to weigh the graph edges */
EQUAL = 0, /* every block edge has equal weight */
RHS_2NORM, /* use the 2-norm of the rhs */
LHS_FNORM, /* use the frobenius norm of the lhs */
RANDOM, /* bounded random edge weight */
} skeletonWeight_ ;
enum AugmentationWeight { /* how to weigh the graph edges */
SKELETON = 0, /* use the same weights in building the skeleton */
// STRETCH, /* stretch in the laplacian sense */
// GENERALIZED_STRETCH /* the generalized stretch defined in jian2013iros */
} augmentationWeight_ ;
double complexity_;
SubgraphBuilderParameters()
: skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {}
virtual ~SubgraphBuilderParameters() {}
/* for serialization */
void print() const ;
virtual void print(std::ostream &os) const ;
friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p);
static Skeleton skeletonTranslator(const std::string &s);
static std::string skeletonTranslator(Skeleton w);
static SkeletonWeight skeletonWeightTranslator(const std::string &s);
static std::string skeletonWeightTranslator(SkeletonWeight w);
static AugmentationWeight augmentationWeightTranslator(const std::string &s);
static std::string augmentationWeightTranslator(AugmentationWeight w);
};
/*****************************************************************************/
class GTSAM_EXPORT SubgraphBuilder {
public:
typedef SubgraphBuilder Base;
typedef boost::shared_ptr<SubgraphBuilder> shared_ptr;
typedef std::vector<double> Weights;
SubgraphBuilder(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
: parameters_(p) {}
virtual ~SubgraphBuilder() {}
virtual boost::shared_ptr<Subgraph> operator() (const GaussianFactorGraph &jfg) const ;
private:
std::vector<size_t> buildTree(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &weights) const ;
std::vector<size_t> unary(const GaussianFactorGraph &gfg) const ;
std::vector<size_t> natural_chain(const GaussianFactorGraph &gfg) const ;
std::vector<size_t> bfs(const GaussianFactorGraph &gfg) const ;
std::vector<size_t> kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const ;
std::vector<size_t> sample(const std::vector<double> &weights, const size_t t) const ;
Weights weights(const GaussianFactorGraph &gfg) const;
SubgraphBuilderParameters parameters_;
private:
SubgraphBuilder() {}
};
/*******************************************************************************************/
struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters {
typedef PreconditionerParameters Base;
typedef boost::shared_ptr<SubgraphPreconditionerParameters> shared_ptr;
SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
: Base(), builderParams_(p) {}
virtual ~SubgraphPreconditionerParameters() {}
SubgraphBuilderParameters builderParams_;
};
/**
* Subgraph conditioner class, as explained in the RSS 2010 submission.
* Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2
@ -37,7 +178,7 @@ namespace gtsam {
* To use the class, give the Bayes Net R1*x=c1 and Graph A2*x=b2.
* Then solve for yhat using CG, and solve for xhat = system.x(yhat).
*/
class GTSAM_EXPORT SubgraphPreconditioner {
class GTSAM_EXPORT SubgraphPreconditioner : public Preconditioner {
public:
typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
@ -52,9 +193,12 @@ namespace gtsam {
sharedValues xbar_; ///< A1 \ b1
sharedErrors b2bar_; ///< A2*xbar - b2
KeyInfo keyInfo_;
SubgraphPreconditionerParameters parameters_;
public:
SubgraphPreconditioner();
SubgraphPreconditioner(const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
/**
* Constructor
@ -62,7 +206,10 @@ namespace gtsam {
* @param Rc1: the Bayes Net R1*x=c1
* @param xbar: the solution to R1*x=c1
*/
SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar);
SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar,
const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
virtual ~SubgraphPreconditioner() {}
/** print the object */
void print(const std::string& s = "SubgraphPreconditioner") const;
@ -80,7 +227,6 @@ namespace gtsam {
* Add zero-mean i.i.d. Gaussian prior terms to each variable
* @param sigma Standard deviation of Gaussian
*/
// SubgraphPreconditioner add_priors(double sigma) const;
/* x = xbar + inv(R1)*y */
VectorValues x(const VectorValues& y) const;
@ -119,6 +265,54 @@ namespace gtsam {
* y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2]
*/
void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& y) const;
/*****************************************************************************/
/* implement virtual functions of Preconditioner */
/* Computation Interfaces for Vector */
virtual void solve(const Vector& y, Vector &x) const;
virtual void transposeSolve(const Vector& y, Vector& x) const ;
virtual void build(
const GaussianFactorGraph &gfg,
const KeyInfo &info,
const std::map<Key,Vector> &lambda
) ;
/*****************************************************************************/
};
/* get subvectors */
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys);
/* set subvectors */
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst);
/* build a factor subgraph, which is defined as a set of weighted edges (factors) */
boost::shared_ptr<GaussianFactorGraph>
buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone);
/* sort the container and return permutation index with default comparator */
template <typename Container>
std::vector<size_t> sort_idx(const Container &src)
{
typedef typename Container::value_type T;
const size_t n = src.size() ;
std::vector<std::pair<size_t,T> > tmp;
tmp.reserve(n);
for ( size_t i = 0 ; i < n ; i++ )
tmp.push_back(std::make_pair<size_t,T>(i, src[i]));
/* sort */
std::stable_sort(tmp.begin(), tmp.end()) ;
/* copy back */
std::vector<size_t> idx; idx.reserve(n);
for ( size_t i = 0 ; i < n ; i++ ) {
idx.push_back(tmp[i].first) ;
}
return idx;
}
} // namespace gtsam

8
gtsam/linear/SubgraphSolver.cpp
View File

@ -14,6 +14,7 @@
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph-inl.h>
@ -143,4 +144,11 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
return boost::tie(At, Ac);
}
/****************************************************************************/
VectorValues SubgraphSolver::optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda,
const VectorValues &initial
) { return VectorValues(); }
} // \namespace gtsam

21
gtsam/linear/SubgraphSolver.h
View File

@ -13,22 +13,22 @@
#include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/inference/Ordering.h>
#include <boost/tuple/tuple.hpp>
#include <iosfwd>
namespace gtsam {
// Forward declarations
class GaussianFactorGraph;
class GaussianBayesNet;
class SubgraphPreconditioner;
class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters {
public:
typedef ConjugateGradientParameters Base;
SubgraphSolverParameters() : Base() {}
virtual void print() const { Base::print(); }
virtual void print(std::ostream &os) const { Base::print(os); }
};
/**
@ -61,7 +61,7 @@ public:
protected:
Parameters parameters_;
Ordering ordering_;
SubgraphPreconditioner::shared_ptr pc_; ///< preconditioner object
boost::shared_ptr<SubgraphPreconditioner> pc_; ///< preconditioner object
public:
/* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */
@ -77,8 +77,17 @@ public:
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, const Ordering& ordering);
virtual ~SubgraphSolver() {}
virtual VectorValues optimize () ;
virtual VectorValues optimize (const VectorValues &initial) ;
VectorValues optimize () ;
VectorValues optimize (const VectorValues &initial) ;
/* interface to the nonlinear optimizer that the subclasses have to implement */
virtual VectorValues optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda,
const VectorValues &initial
) ;
protected:

2
gtsam/nonlinear/DoglegOptimizer.cpp
View File

@ -76,7 +76,7 @@ void DoglegOptimizer::iterate(void) {
result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION,
dx_u, dx_n, bn, graph_, state_.values, state_.error, dlVerbose);
}
else if ( params_.isCG() ) {
else if ( params_.isIterative() ) {
throw runtime_error("Dogleg is not currently compatible with the linear conjugate gradient solver");
}
else {

1
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/date_time/posix_time/posix_time.hpp>
class NonlinearOptimizerMoreOptimizationTest;

29
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -21,6 +21,7 @@
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
@ -106,23 +107,21 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg,
delta = gfg.optimize(*params.ordering, params.getEliminationFunction());
} else if (params.isSequential()) {
// Sequential QR or Cholesky (decided by params.getEliminationFunction())
delta = gfg.eliminateSequential(*params.ordering,
params.getEliminationFunction())->optimize();
} else if (params.isCG()) {
delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize();
} else if (params.isIterative()) {
// Conjugate Gradient -> needs params.iterativeParams
if (!params.iterativeParams)
throw std::runtime_error(
"NonlinearOptimizer::solve: cg parameter has to be assigned ...");
// the type of params.iterativeParams decides the type of CG solver
if (boost::dynamic_pointer_cast<SubgraphSolverParameters>(
params.iterativeParams)) {
SubgraphSolver solver(gfg,
dynamic_cast<const SubgraphSolverParameters&>(*params.iterativeParams),
*params.ordering);
delta = solver.optimize();
} else {
throw std::runtime_error(
"NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ...");
throw std::runtime_error("NonlinearOptimizer::solve: cg parameter has to be assigned ...");
if (boost::shared_ptr<PCGSolverParameters> pcg = boost::dynamic_pointer_cast<PCGSolverParameters>(params.iterativeParams) ) {
delta = PCGSolver(*pcg).optimize(gfg);
}
else if (boost::shared_ptr<SubgraphSolverParameters> spcg = boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) {
delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize();
}
else {
throw std::runtime_error("NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ...");
}
} else {
throw std::runtime_error(

12
gtsam/nonlinear/NonlinearOptimizerParams.cpp
View File

@ -99,8 +99,8 @@ void NonlinearOptimizerParams::print(const std::string& str) const {
case CHOLMOD:
std::cout << " linear solver type: CHOLMOD\n";
break;
case CONJUGATE_GRADIENT:
std::cout << " linear solver type: CONJUGATE GRADIENT\n";
case Iterative:
std::cout << " linear solver type: ITERATIVE\n";
break;
default:
std::cout << " linear solver type: (invalid)\n";
@ -127,8 +127,8 @@ std::string NonlinearOptimizerParams::linearSolverTranslator(
return "SEQUENTIAL_CHOLESKY";
case SEQUENTIAL_QR:
return "SEQUENTIAL_QR";
case CONJUGATE_GRADIENT:
return "CONJUGATE_GRADIENT";
case Iterative:
return "ITERATIVE";
case CHOLMOD:
return "CHOLMOD";
default:
@ -148,8 +148,8 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve
return SEQUENTIAL_CHOLESKY;
if (linearSolverType == "SEQUENTIAL_QR")
return SEQUENTIAL_QR;
if (linearSolverType == "CONJUGATE_GRADIENT")
return CONJUGATE_GRADIENT;
if (linearSolverType == "ITERATIVE")
return Iterative;
if (linearSolverType == "CHOLMOD")
return CHOLMOD;
throw std::invalid_argument(

6
gtsam/nonlinear/NonlinearOptimizerParams.h
View File

@ -99,7 +99,7 @@ public:
MULTIFRONTAL_QR,
SEQUENTIAL_CHOLESKY,
SEQUENTIAL_QR,
CONJUGATE_GRADIENT, /* Experimental Flag */
Iterative, /* Experimental Flag */
CHOLMOD, /* Experimental Flag */
};
@ -121,8 +121,8 @@ public:
return (linearSolverType == CHOLMOD);
}
inline bool isCG() const {
return (linearSolverType == CONJUGATE_GRADIENT);
inline bool isIterative() const {
return (linearSolverType == Iterative);
}
GaussianFactorGraph::Eliminate getEliminationFunction() const {

157
tests/testPCGSolver.cpp Normal file
View File

@ -0,0 +1,157 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testPCGSolver.cpp
* @brief Unit tests for PCGSolver class
* @author Yong-Dian Jian
*/
#include <tests/smallExample.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Matrix.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <iostream>
#include <fstream>
using namespace std;
using namespace gtsam;
const double tol = 1e-3;
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */
TEST( PCGSolver, llt ) {
Matrix R = (Matrix(3,3) <<
1., -1., -1.,
0., 2., -1.,
0., 0., 1.);
Matrix AtA = R.transpose() * R;
Vector Rvector = (Vector(9) << 1., -1., -1.,
0., 2., -1.,
0., 0., 1.);
// Vector Rvector = (Vector(6) << 1., -1., -1.,
// 2., -1.,
// 1.);
Vector b = (Vector(3) << 1., 2., 3.);
Vector x = (Vector(3) << 6.5, 2.5, 3.) ;
/* test cholesky */
Matrix Rhat = AtA.llt().matrixL().transpose();
EXPECT(assert_equal(R, Rhat, 1e-5));
/* test backward substitution */
Vector xhat = Rhat.triangularView<Eigen::Upper>().solve(b);
EXPECT(assert_equal(x, xhat, 1e-5));
/* test in-place back substitution */
xhat = b;
Rhat.triangularView<Eigen::Upper>().solveInPlace(xhat);
EXPECT(assert_equal(x, xhat, 1e-5));
/* test triangular matrix map */
Eigen::Map<Eigen::MatrixXd> Radapter(Rvector.data(), 3, 3);
xhat = Radapter.transpose().triangularView<Eigen::Upper>().solve(b);
EXPECT(assert_equal(x, xhat, 1e-5));
}
/* ************************************************************************* */
TEST( PCGSolver, dummy )
{
LevenbergMarquardtParams paramsPCG;
paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ = boost::make_shared<DummyPreconditionerParameters>();
paramsPCG.iterativeParams = pcg;
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
Point2 x0(10,10);
Values c0;
c0.insert(X(1), x0);
Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();
DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
}
/* ************************************************************************* */
TEST( PCGSolver, blockjacobi )
{
LevenbergMarquardtParams paramsPCG;
paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ = boost::make_shared<BlockJacobiPreconditionerParameters>();
paramsPCG.iterativeParams = pcg;
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
Point2 x0(10,10);
Values c0;
c0.insert(X(1), x0);
Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();
DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
}
/* ************************************************************************* */
TEST( PCGSolver, subgraph )
{
LevenbergMarquardtParams paramsPCG;
paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ = boost::make_shared<SubgraphPreconditionerParameters>();
paramsPCG.iterativeParams = pcg;
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
Point2 x0(10,10);
Values c0;
c0.insert(X(1), x0);
Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();
DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}