add new generic pcgsolver and preconditioner classes
add a unit test for the PCGSolver classrelease/4.3a0
Yong-Dian Jian
parent
c844ab19e3
commit
e8d3809917
|
|
@ -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 @{
|
||||
|
|
|
|||
|
|
@ -6,10 +6,24 @@
|
|||
*/
|
||||
|
||||
#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;
|
||||
|
|
@ -29,18 +43,6 @@ ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTransla
|
|||
return ConjugateGradientParameters::GTSAM;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
|
||||
/*****************************************************************************/
|
||||
void ConjugateGradientParameters::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;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -12,13 +12,13 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/linear/IterativeSolver.h>
|
||||
#include <iosfwd>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* parameters for the conjugate gradient method
|
||||
*/
|
||||
|
||||
class ConjugateGradientParameters : public IterativeOptimizationParameters {
|
||||
|
||||
public:
|
||||
|
|
@ -71,10 +71,79 @@ public:
|
|||
inline void setEpsilon_rel(double value) { epsilon_rel_ = value; }
|
||||
inline void setEpsilon_abs(double value) { epsilon_abs_ = value; }
|
||||
|
||||
virtual void print(std::ostream &os) const;
|
||||
|
||||
static std::string blasTranslator(const BLASKernel k) ;
|
||||
static BLASKernel blasTranslator(const std::string &s) ;
|
||||
|
||||
virtual void print() const;
|
||||
};
|
||||
|
||||
/*********************************************************************************************/
|
||||
/*
|
||||
* 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 ¶meters) {
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -6,26 +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::getVerbosity() const { return verbosityTranslator(verbosity_); }
|
||||
string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); }
|
||||
|
||||
/*****************************************************************************/
|
||||
void IterativeOptimizationParameters::setVerbosity(const std::string &src) { verbosity_ = verbosityTranslator(src); }
|
||||
void IterativeOptimizationParameters::setVerbosity(const string &src) { verbosity_ = verbosityTranslator(src); }
|
||||
|
||||
/*****************************************************************************/
|
||||
void IterativeOptimizationParameters::print() const {
|
||||
std::cout << "IterativeOptimizationParameters" << std::endl
|
||||
<< "verbosity: " << verbosityTranslator(verbosity_) << std::endl;
|
||||
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;
|
||||
|
|
@ -34,13 +50,84 @@ IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verb
|
|||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
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_);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -11,16 +11,28 @@
|
|||
|
||||
#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 <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
class VectorValues;
|
||||
class KeyInfo;
|
||||
class KeyInfoEntry;
|
||||
class Ordering;
|
||||
class GaussianFactorGraph;
|
||||
class Values;
|
||||
class VectorValues;
|
||||
|
||||
/************************************************************************************/
|
||||
/**
|
||||
* parameters for iterative linear solvers
|
||||
*/
|
||||
|
|
@ -33,38 +45,97 @@ namespace gtsam {
|
|||
|
||||
public:
|
||||
|
||||
IterativeOptimizationParameters(const IterativeOptimizationParameters &p)
|
||||
: verbosity_(p.verbosity_) {}
|
||||
|
||||
IterativeOptimizationParameters(const Verbosity verbosity = SILENT)
|
||||
: verbosity_(verbosity) {}
|
||||
IterativeOptimizationParameters(Verbosity v = SILENT)
|
||||
: verbosity_(v) {}
|
||||
|
||||
virtual ~IterativeOptimizationParameters() {}
|
||||
|
||||
/* general interface */
|
||||
/* utility */
|
||||
inline Verbosity verbosity() const { return verbosity_; }
|
||||
|
||||
/* matlab interface */
|
||||
std::string getVerbosity() const;
|
||||
void setVerbosity(const std::string &s) ;
|
||||
|
||||
virtual void print() const ;
|
||||
/* 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 Verbosity verbosityTranslator(const std::string &s);
|
||||
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, 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;
|
||||
|
||||
/* interface to the nonlinear optimizer */
|
||||
virtual VectorValues optimize (const VectorValues &initial) = 0;
|
||||
};
|
||||
|
||||
/************************************************************************************/
|
||||
/* Handy data structure for iterative solvers
|
||||
* key to (index, dimension, colstart) */
|
||||
class 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 KeyInfo : public std::map<Key, KeyInfoEntry> {
|
||||
public:
|
||||
typedef std::map<Key, KeyInfoEntry> Base;
|
||||
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_;
|
||||
|
||||
private:
|
||||
KeyInfo() {}
|
||||
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,437 @@
|
|||
/*
|
||||
* 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;
|
||||
}
|
||||
|
||||
///*****************************************************************************/
|
||||
//std::string PCGSolverParameters::pcgTranslator(const PCGKernel value) {
|
||||
// std::string s;
|
||||
// switch (value) {
|
||||
// case PCGSolverParameters::PCG: s = "PCG"; break;
|
||||
// case PCGSolverParameters::LSPCG: s = "LSPCG"; break;
|
||||
// case PCGSolverParameters::SPCG: s = "SPCG"; break;
|
||||
// default: s = "UNDEFINED"; break;
|
||||
// }
|
||||
// return s;
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//PCGSolverParameters::PCGKernel PCGSolverParameters::pcgTranslator(const std::string &src) {
|
||||
// std::string s = src; boost::algorithm::to_upper(s);
|
||||
// if (s == "PCG") return PCGSolverParameters::PCG;
|
||||
// if (s == "LSPCG") return PCGSolverParameters::LSPCG;
|
||||
// if (s == "SPCG") return PCGSolverParameters::SPCG;
|
||||
//
|
||||
// /* default is LSPCG */
|
||||
// return PCGSolverParameters::LSPCG;
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//std::string PCGSolverParameters::blasTranslator(const BLASKernel value) {
|
||||
// std::string s;
|
||||
// switch (value) {
|
||||
// case PCGSolverParameters::GTSAM: s = "GTSAM" ; break;
|
||||
// case PCGSolverParameters::SBM: s = "SBM" ; break;
|
||||
// case PCGSolverParameters::SBM_MT: s = "SBM-MT" ; break;
|
||||
// default: s = "UNDEFINED" ; break;
|
||||
// }
|
||||
// return s;
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//PCGSolverParameters::BLASKernel PCGSolverParameters::blasTranslator(const std::string &src) {
|
||||
// std::string s = src; boost::algorithm::to_upper(s);
|
||||
// if (s == "GTSAM") return PCGSolverParameters::GTSAM;
|
||||
// if (s == "SBM") return PCGSolverParameters::SBM;
|
||||
// if (s == "SBM-MT") return PCGSolverParameters::SBM_MT;
|
||||
//
|
||||
// /* default is SBM */
|
||||
// return PCGSolverParameters::SBM;
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//std::string PCGSolverParameters::rbTranslator(const RegisterBlockKernel k) {
|
||||
// std::string s;
|
||||
// switch (k) {
|
||||
// case PCGSolverParameters::RB_NONE: s = "RB_NONE" ; break;
|
||||
// case PCGSolverParameters::SSE_RB22: s = "SSE_RB22" ; break;
|
||||
// case PCGSolverParameters::SSE_RB44: s = "SSE_RB44" ; break;
|
||||
// case PCGSolverParameters::AVX_RB44: s = "AVX_RB44" ; break;
|
||||
// default: s = "UNDEFINED" ; break;
|
||||
// }
|
||||
// return s;
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//PCGSolverParameters::RegisterBlockKernel PCGSolverParameters::rbTranslator(const std::string &src) {
|
||||
// std::string s = src; boost::algorithm::to_upper(s);
|
||||
// if (s == "") return PCGSolverParameters::RB_NONE;
|
||||
// if (s == "SSE_RB22") return PCGSolverParameters::SSE_RB22;
|
||||
// if (s == "SSE_RB44") return PCGSolverParameters::SSE_RB44;
|
||||
// if (s == "AVX_RB44") return PCGSolverParameters::AVX_RB44;
|
||||
//
|
||||
// /* default is SBM */
|
||||
// return PCGSolverParameters::RB_NONE;
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//PCGSolver::PCGSolver(const PCGSolverParameters &p) : Base(), parameters_(p), built_(false) {
|
||||
// preconditioner_ = createPreconditioner(p.preconditioner_);
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//void PCGSolver::replaceFactors(
|
||||
// const Values &linearization_point,
|
||||
// const GaussianFactorGraph &gfg,
|
||||
// const double lambda)
|
||||
//{
|
||||
// const JacobianFactorGraph jfg(gfg);
|
||||
//
|
||||
// /* prepare the column structure */
|
||||
// if ( keyInfo_.size() == 0 ) {
|
||||
// keyInfo_ = KeyInfo(jfg, *orderingNatural(jfg));
|
||||
// }
|
||||
//
|
||||
// /* implemented by subclass */
|
||||
// safe_tic_();
|
||||
// convertKernel(jfg, lambda);
|
||||
// safe_toc_("convert-kernel");
|
||||
//
|
||||
// /* update the preconditioner as well */
|
||||
// preconditioner_->replaceFactors(jfg, lambda);
|
||||
// built_ = false;
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//void PCGSolver::buildPreconditioner() {
|
||||
// if ( built_ == false ) {
|
||||
// safe_tic_();
|
||||
// preconditioner_->buildPreconditioner();
|
||||
// built_ = true;
|
||||
// safe_toc_("factorize");
|
||||
// }
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//VectorValues PCGSolver::optimize() {
|
||||
//
|
||||
// buildPreconditioner();
|
||||
//
|
||||
// VectorValues zero;
|
||||
// BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo_ ) {
|
||||
// zero.insert(item.first, Vector::Zero(item.second.dim()));
|
||||
// }
|
||||
//
|
||||
// return optimize(zero);
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//VectorValues PCGSolver::optimize(const VectorValues &initial) {
|
||||
// safe_tic_();
|
||||
// VectorValues result = optimize_(initial);
|
||||
// safe_toc_("pcg");
|
||||
// return result ;
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
////void PCGSolver_FG::convertKernel(const JacobianFactorGraph &jfg, const double lambda) {
|
||||
//// hfg_ = buildOuterProductJacobianFactorGraph(jfg, VariableIndex(jfg), *x0_, lambda);
|
||||
////}
|
||||
////
|
||||
////VectorValues PCGSolver_FG::optimize_(const VectorValues &initial) {
|
||||
//// System system(hfg_, preconditioner_);
|
||||
//// return preconditionedConjugateGradient<System, VectorValues>(system, initial, parameters_);
|
||||
////}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//void PCGSolver_SBM::convertKernel(const JacobianFactorGraph &jfg, const double lambda) {
|
||||
// linear_ = buildSparseLinearSystem(jfg, keyInfo_, true /* AtA */, lambda, false /* column major */,
|
||||
// parameters_.blas_kernel_, parameters_.rb_kernel_);
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//VectorValues PCGSolver_SBM::optimize_(const VectorValues &initial) {
|
||||
// System system(linear_, preconditioner_);
|
||||
// Vector solution = preconditionedConjugateGradient<System, Vector>(
|
||||
// system, initial.vector(keyInfo_.ordering()), parameters_);
|
||||
// return buildVectorValues(solution, keyInfo_);
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//ydjian::SparseLinearSystem::shared_ptr
|
||||
//buildSparseLinearSystem(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo,
|
||||
// const bool AtA, const double lambda, const bool colMajor,
|
||||
// const PCGSolverParameters::BLASKernel blas, const PCGSolverParameters::RegisterBlockKernel rbSrc) {
|
||||
//
|
||||
// std::map<PCGSolverParameters::RegisterBlockKernel, ydjian::SBM::Type> rbMap;
|
||||
// rbMap[PCGSolverParameters::RB_NONE] = ydjian::SBM::NAIVE;
|
||||
// rbMap[PCGSolverParameters::SSE_RB22] = ydjian::SBM::SSE_RB22;
|
||||
// rbMap[PCGSolverParameters::SSE_RB44] = ydjian::SBM::SSE_RB44;
|
||||
// rbMap[PCGSolverParameters::AVX_RB44] = ydjian::SBM::AVX_RB44;
|
||||
//
|
||||
// ydjian::SBM::Type rb;
|
||||
// SC_ASSERT( rbMap.find(rbSrc) != rbMap.end(), "buildSparseLinearSystem: rbSrc is not supported..");
|
||||
// rb = rbMap[rbSrc];
|
||||
//
|
||||
// ydjian::SparseLinearSystem::shared_ptr linear;
|
||||
//
|
||||
// switch (blas) {
|
||||
//
|
||||
// case PCGSolverParameters::SBM:
|
||||
// linear = gsp2::buildSparseLinearSystem(jfg, keyInfo, AtA, lambda, colMajor, false /* multithread */, rb);
|
||||
// break;
|
||||
//
|
||||
// case PCGSolverParameters::SBM_MT:
|
||||
// linear = gsp2::buildSparseLinearSystem(jfg, keyInfo, AtA, lambda, colMajor, true /* multithread */, rb);
|
||||
// break;
|
||||
//
|
||||
// default:
|
||||
// throw std::invalid_argument("createSparseLinearSystem: unsupported blas kernel");
|
||||
// break;
|
||||
// }
|
||||
//
|
||||
// return linear;
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//PCGSolver::shared_ptr createPCGSolver(const PCGSolverParameters ¶meters){
|
||||
//
|
||||
// PCGSolver::shared_ptr solver;
|
||||
// switch ( parameters.pcg_kernel_ ) {
|
||||
// case PCGSolverParameters::PCG:
|
||||
// switch ( parameters.blas_kernel_ ) {
|
||||
//
|
||||
//// case PCGSolverParameters::GTSAM:
|
||||
//// solver.reset(new PCGSolver_FG(parameters));
|
||||
//// break;
|
||||
//
|
||||
// case PCGSolverParameters::SBM:
|
||||
// case PCGSolverParameters::SBM_MT:
|
||||
// solver.reset(new PCGSolver_SBM(parameters));
|
||||
// break;
|
||||
//
|
||||
// default:
|
||||
// throw std::invalid_argument("createPCGSolver: undefined blas_kernel for pcgsolver");
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case PCGSolverParameters::LSPCG:
|
||||
// switch ( parameters.blas_kernel_ ) {
|
||||
// case PCGSolverParameters::GTSAM:
|
||||
// solver.reset(new LSPCGSolver_FG(parameters));
|
||||
// break;
|
||||
//
|
||||
// case PCGSolverParameters::SBM:
|
||||
// case PCGSolverParameters::SBM_MT:
|
||||
// solver.reset(new LSPCGSolver_SBM(parameters));
|
||||
// break;
|
||||
//
|
||||
// default:
|
||||
// throw std::invalid_argument("createPCGSolver: undefined blas_kernel for lspcg solver");
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// default:
|
||||
// throw std::invalid_argument("createPCGSolver: undefined pcg_kernel");
|
||||
// break;
|
||||
// }
|
||||
// return solver;
|
||||
//}
|
||||
}
|
||||
|
|
@ -0,0 +1,236 @@
|
|||
/*
|
||||
* 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 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 PCGSolver: public IterativeSolver {
|
||||
public:
|
||||
typedef IterativeSolver Base;
|
||||
typedef boost::shared_ptr<PCGSolver> shared_ptr;
|
||||
|
||||
protected:
|
||||
|
||||
PCGSolverParameters parameters_;
|
||||
boost::shared_ptr<Preconditioner> preconditioner_;
|
||||
|
||||
// /* cached local variables */
|
||||
// KeyInfo keyInfo_;
|
||||
//
|
||||
// /* whether the preconditioner has be built */
|
||||
// bool built_ ;
|
||||
|
||||
public:
|
||||
/* Interface to initialize a solver without a problem */
|
||||
PCGSolver(const PCGSolverParameters &p);
|
||||
virtual ~PCGSolver() {}
|
||||
|
||||
// /* update interface to the nonlinear optimizer */
|
||||
// virtual void replaceFactors(
|
||||
// const GaussianFactorGraph &factorGraph,
|
||||
// const Values &linearization_point,
|
||||
// const std::map<Key, double> lambda);
|
||||
|
||||
// /* build the preconditioner */
|
||||
// void buildPreconditioner();
|
||||
|
||||
// /* interface for the NonlinearOptimizer, with initial estimate equals to a zero vector */
|
||||
// virtual VectorValues optimize() ;
|
||||
//
|
||||
// /* interface for the NonlinearOptimizer, with a custom initial estimate*/
|
||||
// virtual VectorValues optimize(const VectorValues &initial);
|
||||
|
||||
using IterativeSolver::optimize;
|
||||
|
||||
virtual VectorValues optimize(const GaussianFactorGraph &gfg,
|
||||
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
|
||||
const VectorValues &initial);
|
||||
|
||||
//protected:
|
||||
|
||||
// /* convert jacobian factor graph to solver-specific kernel */
|
||||
// virtual void convertKernel(const JacobianFactorGraph &jfg, const double lambda = 0.0) = 0 ;
|
||||
//
|
||||
// /* do the actual optimization */
|
||||
// virtual VectorValues optimize_(const VectorValues &initial) = 0;
|
||||
|
||||
};
|
||||
|
||||
/*****************************************************************************/
|
||||
class 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);
|
||||
|
||||
///*****************************************************************************/
|
||||
///* an specialization of the PCGSolver using gtsam's factor graph and linear algebra facility */
|
||||
//class PCGSolver_FG : public PCGSolver {
|
||||
//
|
||||
//public:
|
||||
// typedef PCGSolver Base;
|
||||
// typedef boost::shared_ptr<PCGSolver_FG> shared_ptr ;
|
||||
//
|
||||
//protected:
|
||||
// JacobianFactorGraph::shared_ptr hfg_; // A'*A + lambda*I
|
||||
//
|
||||
//public:
|
||||
// PCGSolver_FG(const Parameters ¶meters) : Base(parameters) {}
|
||||
// virtual ~PCGSolver_FG() {}
|
||||
//
|
||||
//protected:
|
||||
// virtual void convertKernel(const JacobianFactorGraph &jfg, const double lambda = 0.0);
|
||||
// virtual VectorValues optimize_(const VectorValues &initial);
|
||||
//
|
||||
// /* interface to the preconditionedConjugateGradient function template */
|
||||
// class System {
|
||||
// public:
|
||||
// typedef Vector State;
|
||||
// typedef Vector Residual;
|
||||
//
|
||||
// protected:
|
||||
// const JacobianFactorGraph::shared_ptr hfg_;
|
||||
// const Preconditioner::shared_ptr preconditioner_;
|
||||
// const KeyInfo &keyInfo_;
|
||||
// const RowInfo &rowInfo_;
|
||||
//
|
||||
// public:
|
||||
// System(const JacobianFactorGraph::shared_ptr hfg,
|
||||
// const Preconditioner::shared_ptr preconditioner,
|
||||
// const KeyInfo &keyInfo, const RowInfo &rowInfo);
|
||||
//
|
||||
// inline void residual(const State &x, State &r) const { gtsam::residual(*hfg_, x, r); }
|
||||
// inline void multiply(const State &x, State& y) const { gtsam::multiply(*hfg_, x, y); }
|
||||
// inline void leftPrecondition(const State &x, State &y) const { preconditioner_->transposeSolve(x, y); }
|
||||
// inline void rightPrecondition(const State &x, State &y) const { preconditioner_->solve(x, y); }
|
||||
// inline void scal(const double alpha, State &x) const { x.vector() *= alpha; }
|
||||
// inline double dot(const State &x, const State &y) const { return x.vector().dot(y.vector()); }
|
||||
// inline void axpy(const double alpha, const State &x, State &y) const { y.vector() += alpha*x.vector(); }
|
||||
// } ;
|
||||
//};
|
||||
//
|
||||
///*****************************************************************************/
|
||||
///** an specialization of the PCGSolver using sbm and customized blas kernel **/
|
||||
//class PCGSolver_SBM : public PCGSolver {
|
||||
//
|
||||
//public:
|
||||
// typedef PCGSolver Base;
|
||||
// typedef boost::shared_ptr<PCGSolver_SBM> shared_ptr ;
|
||||
//
|
||||
//protected:
|
||||
// ydjian::SparseLinearSystem::shared_ptr linear_;
|
||||
//
|
||||
//public:
|
||||
// PCGSolver_SBM(const Parameters ¶meters) : Base(parameters) {}
|
||||
// virtual ~PCGSolver_SBM() {}
|
||||
//
|
||||
//protected:
|
||||
// /* virtual function of the PCGSolver */
|
||||
// virtual void convertKernel(const JacobianFactorGraph &jfg, const double lambda = 0.0);
|
||||
// virtual VectorValues optimize_(const VectorValues &initial);
|
||||
//
|
||||
// /* interface to the preconditionedConjugateGradient function template */
|
||||
// class System {
|
||||
//
|
||||
// public:
|
||||
// typedef Vector State;
|
||||
//
|
||||
// protected:
|
||||
// const ydjian::SparseLinearSystem::shared_ptr linear_;
|
||||
// const Preconditioner::shared_ptr preconditioner_;
|
||||
//
|
||||
// public:
|
||||
// System(const ydjian::SparseLinearSystem::shared_ptr linear, const Preconditioner::shared_ptr preconditioner)
|
||||
// : linear_(linear), preconditioner_(preconditioner) {}
|
||||
// inline void residual(const State &x, State &r) const { linear_->residual(x.data(), r.data(), false); }
|
||||
// inline void multiply(const State &x, State& y) const { linear_->matrix()->multiply(x.data(), y.data(), false); }
|
||||
// inline void leftPrecondition(const State &x, State &y) const { preconditioner_->transposeSolve(x, y); }
|
||||
// inline void rightPrecondition(const State &x, State &y) const { preconditioner_->solve(x, y); }
|
||||
// inline void scal(const double alpha, State &x) const { x *= alpha; }
|
||||
// inline double dot(const State &x, const State &y) const { return x.dot(y); }
|
||||
// inline void axpy(const double alpha, const State &x, State &y) const { y += alpha*x; }
|
||||
// } ;
|
||||
//};
|
||||
//
|
||||
///* a factory method to instantiate PCGSolvers and its derivatives */
|
||||
//PCGSolver::shared_ptr createPCGSolver(const PCGParameters ¶meters);
|
||||
//
|
||||
///* an utility function to create sparse linear system, for all cg solvers using the sbm kernel */
|
||||
//ydjian::SparseLinearSystem::shared_ptr
|
||||
//buildSparseLinearSystem(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo, const bool AtA,
|
||||
// const double lambda, const bool colMajor, const PCGParameters::BLASKernel blas,
|
||||
// const PCGParameters::RegisterBlockKernel rb);
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,212 @@
|
|||
/*
|
||||
* Preconditioner.cpp
|
||||
*
|
||||
* Created on: Feb 2, 2012
|
||||
* Author: ydjian
|
||||
*/
|
||||
|
||||
|
||||
//#include <gtsam/linear/CombinatorialPreconditioner.h>
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
//#include <gtsam/linear/FactorGraphUtil-inl.h>
|
||||
#include <gtsam/linear/PCGSolver.h>
|
||||
#include <gtsam/linear/Preconditioner.h>
|
||||
//#include <gtsam/linear/JacobianFactorGraph.h>
|
||||
//#include <gtsam/linear/JacobiPreconditioner.h>
|
||||
//#include <gtsam/linear/MIQRPreconditioner.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
//#include <gtsam/linear/Subgraph.h>
|
||||
//#include <gtsam/linear/SubgraphBuilder-inl.h>
|
||||
//#include <gtsam/linear/SuiteSparseSolver.h>
|
||||
//#include <gtsam/linear/SuiteSparseUtil.h>
|
||||
//#include <ydjian/tool/ThreadSafeTimer.h>
|
||||
//#include <ydjian/tool/Timer.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";
|
||||
}
|
||||
|
||||
///***************************************************************************************/
|
||||
//void Preconditioner::replaceFactors(const JacobianFactorGraph &jfg, const double lambda)
|
||||
//{
|
||||
// const Parameters &p = *parameters_;
|
||||
//
|
||||
// if ( keyInfo_.size() == 0 ) {
|
||||
// keyInfo_ = KeyInfo(jfg, *orderingNatural(jfg));
|
||||
// }
|
||||
//
|
||||
// if ( p.verbosity() >= Parameters::COMPLEXITY )
|
||||
// cout << "Preconditioner::replaceFactors with a jfg of " << jfg.size() << " factors."<< endl;
|
||||
//}
|
||||
//
|
||||
/***************************************************************************************/
|
||||
boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters) {
|
||||
|
||||
DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast<DummyPreconditionerParameters>(parameters);
|
||||
if ( dummy ) {
|
||||
return boost::make_shared<DummyPreconditioner>();
|
||||
}
|
||||
|
||||
// BlockJacobiPreconditioner::Parameters::shared_ptr jacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditioner::Parameters>(parameters);
|
||||
// if ( jacobi ) {
|
||||
// BlockJacobiPreconditioner::shared_ptr p(new BlockJacobiPreconditioner(jacobi));
|
||||
// return p;
|
||||
// }
|
||||
//
|
||||
// MIQRPreconditioner::Parameters::shared_ptr miqr = boost::dynamic_pointer_cast<MIQRPreconditioner::Parameters>(parameters);
|
||||
// if ( miqr ) {
|
||||
// MIQRPreconditioner::shared_ptr p(new MIQRPreconditioner(miqr));
|
||||
// return p;
|
||||
// }
|
||||
//
|
||||
// CombinatorialPreconditioner::Parameters::shared_ptr combinatorial
|
||||
// = boost::dynamic_pointer_cast<CombinatorialPreconditioner::Parameters>(parameters);
|
||||
// if ( combinatorial ) {
|
||||
// return createCombinatorialPreconditioner(combinatorial);
|
||||
// }
|
||||
|
||||
throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type");
|
||||
}
|
||||
//
|
||||
///***************************************************************************************/
|
||||
//JacobianFactorGraph::shared_ptr
|
||||
//buildFactorSubgraph(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo,
|
||||
// const Subgraph &subgraph, const int hessian, const double lambda, const bool clone, const bool includeUnary){
|
||||
//
|
||||
// const Subgraph::Edges &edges = subgraph.edges();
|
||||
// const size_t m = jfg.size(), n = keyInfo.size();
|
||||
// const bool augment = (lambda!=0.0) ? true : false ;
|
||||
//
|
||||
// /* produce an edge weight table */
|
||||
// std::vector<double> weights(m, 0.0);
|
||||
// BOOST_FOREACH ( const Subgraph::Edge &edge, edges ) {
|
||||
// weights[edge.index()] = edge.weight();
|
||||
// }
|
||||
//
|
||||
// /* collect the number of dangling unary factors */
|
||||
// /* upper bound of the factors */
|
||||
// size_t extraUnary = 0;
|
||||
// if ( includeUnary ) {
|
||||
// for ( Index i = 0 ; i < m ; ++i ) {
|
||||
// if ( weights[i] == 0.0 && jfg[i]->size() == 1 ) {
|
||||
// weights[i] = 1.0;
|
||||
// ++extraUnary;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// const size_t e = edges.size() + extraUnary;
|
||||
// const size_t sz = e + (augment ? n : 0) + (hessian ? 2*(m-e) : 0);
|
||||
//
|
||||
// JacobianFactorGraph::shared_ptr target(new JacobianFactorGraph());
|
||||
// target->reserve(sz);
|
||||
//
|
||||
// /* auxiliary variable */
|
||||
// size_t r = jfg[0]->rows();
|
||||
// SharedDiagonal sigma(noiseModel::Unit::Create(r));
|
||||
//
|
||||
// for ( Index i = 0 ; i < m ; ++i ) {
|
||||
//
|
||||
// const double w = weights[i];
|
||||
//
|
||||
// if ( w != 0.0 ) { /* subgraph edge */
|
||||
// if ( !clone && w == 1.0 ) {
|
||||
// target->push_back(jfg[i]);
|
||||
// }
|
||||
// else {
|
||||
// JacobianFactor::shared_ptr jf (new JacobianFactor(*jfg[i]));
|
||||
// scaleJacobianFactor(*jf, w);
|
||||
// target->push_back(jf);
|
||||
// }
|
||||
// }
|
||||
// else { /* non-subgraph edge */
|
||||
// if ( hessian ) {
|
||||
// const JacobianFactor &f = *jfg[i];
|
||||
// /* simple case: unary factor */
|
||||
// if ( f.size() == 1 ) {
|
||||
// if (!clone) target->push_back(jfg[i]);
|
||||
// else {
|
||||
// JacobianFactor::shared_ptr jf(new JacobianFactor(*jfg[i]));
|
||||
// target->push_back(jf);
|
||||
// }
|
||||
// }
|
||||
// else { /* general factor */
|
||||
// const size_t rj = f.rows();
|
||||
// if ( rj != r ) {
|
||||
// r = rj; sigma = noiseModel::Unit::Create(r);
|
||||
// }
|
||||
// for ( JacobianFactor::const_iterator j = f.begin() ; j != f.end() ; ++j ) {
|
||||
// JacobianFactor::shared_ptr jf(new JacobianFactor(*j, f.getA(j), Vector::Zero(r,1), sigma));
|
||||
// target->push_back(jf);
|
||||
// } /* for */
|
||||
// } /* factor arity */
|
||||
// } /* hessian */
|
||||
// } /* if w != 0.0 */
|
||||
// } /* for */
|
||||
//
|
||||
// if ( !augment ) return target ;
|
||||
//
|
||||
// return appendPriorFactors(*target, keyInfo, lambda, false /* clone */);
|
||||
//}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,173 @@
|
|||
/*
|
||||
* Preconditioner.h
|
||||
*
|
||||
* Created on: Feb 1, 2012
|
||||
* 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;
|
||||
//class Subgraph;
|
||||
|
||||
/* parameters for the preconditioner */
|
||||
struct 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 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;
|
||||
|
||||
// /* complexity index */
|
||||
// virtual size_t complexity() const = 0;
|
||||
//
|
||||
// /* is the preconditioner dependent to data */
|
||||
// virtual bool isStatic() const = 0;
|
||||
//
|
||||
// /* is the preconditioner kind of spanning subgraph preconditioner */
|
||||
// virtual bool isSubgraph() const = 0;
|
||||
//
|
||||
// /* return A\b */
|
||||
// virtual void xstar(Vector &result) const = 0 ;
|
||||
//
|
||||
//protected:
|
||||
// Parameters::shared_ptr parameters_;
|
||||
// KeyInfo keyInfo_;
|
||||
|
||||
};
|
||||
|
||||
/*******************************************************************************************/
|
||||
struct DummyPreconditionerParameters : public PreconditionerParameters {
|
||||
typedef PreconditionerParameters Base;
|
||||
typedef boost::shared_ptr<DummyPreconditionerParameters> shared_ptr;
|
||||
DummyPreconditionerParameters() : Base() {}
|
||||
virtual ~DummyPreconditionerParameters() {}
|
||||
};
|
||||
|
||||
/*******************************************************************************************/
|
||||
class 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
|
||||
) {}
|
||||
|
||||
// virtual void replaceFactors(const JacobianFactorGraph &jfg, const double lambda = 0.0) {
|
||||
// Base::replaceFactors(jfg,lambda);
|
||||
// }
|
||||
// virtual void buildPreconditioner() {}
|
||||
// virtual size_t complexity() const { return 0; }
|
||||
// virtual bool isStatic() const { return true; }
|
||||
// virtual bool isSubgraph() const { return false; }
|
||||
// virtual void xstar(Vector &result) const {}
|
||||
};
|
||||
|
||||
/* factory method to create preconditioners */
|
||||
boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters);
|
||||
|
||||
///* build a factor subgraph, which is defined as a set of weighted edges (factors).
|
||||
// * "hessian"={0,1} indicates whether the non-subgraph edges are split into multiple unary factors
|
||||
// * "lambda" indicates whether scaled identity matrix is augmented
|
||||
// * "clone" indicates whether factors are cloned into the subgraph if the edge has weight = 1.0
|
||||
// * "includeUnary" indicates whether the unary factors are forced to join the subgraph:
|
||||
// * useful for static subgraph preconditioner, because unary factors were imposed dynamically by LM */
|
||||
//boost::shared_ptr<JacobianFactorGraph>
|
||||
//buildFactorSubgraph(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo,
|
||||
// const Subgraph &subgraph, const int hessian, const double lambda, const bool clone, const bool includeUnary);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -15,8 +15,8 @@
|
|||
#include <gtsam/linear/ConjugateGradientSolver.h>
|
||||
#include <gtsam/linear/SubgraphPreconditioner.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <iosfwd>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -28,7 +28,7 @@ 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); }
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -77,8 +77,17 @@ public:
|
|||
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters ¶meters, 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
|
||||
) { return VectorValues(); }
|
||||
|
||||
protected:
|
||||
|
||||
|
|
|
|||
|
|
@ -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>
|
||||
|
||||
|
|
@ -100,19 +101,18 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg,
|
|||
if (params.isMultifrontal()) {
|
||||
delta = gfg.optimize(*params.ordering, params.getEliminationFunction());
|
||||
} else if (params.isSequential()) {
|
||||
delta = gfg.eliminateSequential(*params.ordering,
|
||||
params.getEliminationFunction())->optimize();
|
||||
delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize();
|
||||
} else if (params.isIterative()) {
|
||||
if (!params.iterativeParams)
|
||||
throw std::runtime_error(
|
||||
"NonlinearOptimizer::solve: cg parameter has to be assigned ...");
|
||||
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: 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 ...");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,77 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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/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( NonlinearOptimizer, optimization_method )
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue