diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index ea2ad7eda..7b1a2bb2e 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -17,9 +17,11 @@ #pragma once +#include #include #include +#include #include #include #include @@ -135,6 +137,15 @@ namespace gtsam { static GTSAM_EXPORT Ordering COLAMDConstrained(const VariableIndex& variableIndex, const FastMap& groups); + /// Return a natural Ordering. Typically used by iterative solvers + template + static Ordering Natural(const FactorGraph &fg) { + FastSet src = fg.keys(); + std::vector keys(src.begin(), src.end()); + std::stable_sort(keys.begin(), keys.end()); + return Ordering(keys); + } + /// @} /// @name Testable @{ diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp index 4072c5080..0505f6c01 100644 --- a/gtsam/linear/ConjugateGradientSolver.cpp +++ b/gtsam/linear/ConjugateGradientSolver.cpp @@ -6,10 +6,24 @@ */ #include +#include #include +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; - } } diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index ce27b7a96..47ec4b405 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -12,13 +12,13 @@ #pragma once #include +#include 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 +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; +} + + } diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index d6836783b..f10309d76 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -54,6 +54,20 @@ namespace gtsam { return keys; } + /* ************************************************************************* */ + std::map GaussianFactorGraph::getKeyDimMap() const { + map spec; + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, *this ) { + for ( GaussianFactor::const_iterator it = gf->begin() ; it != gf->end() ; it++ ) { + map::iterator it2 = spec.find(*it); + if ( it2 == spec.end() ) { + spec.insert(make_pair(*it, gf->getDim(it))); + } + } + } + return spec; + } + /* ************************************************************************* */ vector GaussianFactorGraph::getkeydim() const { // First find dimensions of each variable diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 692310f26..910b25d1e 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -138,6 +138,9 @@ namespace gtsam { typedef FastSet Keys; Keys keys() const; + /* return a map of (Key, dimension) */ + std::map getKeyDimMap() const; + std::vector getkeydim() const; /** unnormalized error */ diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index b022a26d8..36178b191 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -6,26 +6,42 @@ */ #include +#include +#include #include +#include #include +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 keyInfo, + boost::optional&> lambda) +{ + return optimize( + gfg, + keyInfo ? *keyInfo : KeyInfo(gfg), + lambda ? *lambda : std::map()); +} + +/*****************************************************************************/ +VectorValues IterativeSolver::optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &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 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(i, dim, start))); + start += dim ; + } + numCols_ = start; +} + +/****************************************************************************/ +vector KeyInfo::colSpec() const { + std::vector 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_); +} + } diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index daf5960e7..66006875b 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -11,16 +11,28 @@ #pragma once +#include #include - +#include +#include +#include +#include +#include +#include #include +#include 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 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 = boost::none, + boost::optional&> lambda = boost::none + ); + + /* interface to the nonlinear optimizer, without initial estimate */ + VectorValues optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda + ); + + /* interface to the nonlinear optimizer that the subclasses have to implement */ + virtual VectorValues optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &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 { + public: + typedef boost::tuple 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 { + public: + typedef std::map Base; + KeyInfo(const GaussianFactorGraph &fg); + KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); + + std::vector 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() {} + + }; + + } diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp new file mode 100644 index 000000000..6bfa389fb --- /dev/null +++ b/gtsam/linear/PCGSolver.cpp @@ -0,0 +1,437 @@ +/* + * PCGSolver.cpp + * + * Created on: Feb 14, 2012 + * Author: ydjian + */ + +#include +//#include +//#include +//#include +//#include +#include +#include +//#include +//#include +//#include +//#include +#include +#include +#include + +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 &lambda, + const VectorValues &initial) +{ + /* build preconditioner */ + preconditioner_->build(gfg, keyInfo, lambda); + + /* apply pcg */ + const Vector sol = preconditionedConjugateGradient( + 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 &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(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(gf) ) { + /* accumulate H x */ + + /* use buffer to avoid excessive table lookups */ + const size_t sz = hf->size(); + 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(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(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 & dimensions) { + VectorValues result; + + DenseIndex offset = 0; + for ( size_t i = 0 ; i < ordering.size() ; ++i ) { + const Key &key = ordering[i]; + map::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, 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, 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 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; +//} +} diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h new file mode 100644 index 000000000..034352c50 --- /dev/null +++ b/gtsam/linear/PCGSolver.h @@ -0,0 +1,236 @@ +/* + * PCGSolver.h + * + * Created on: Jan 31, 2012 + * Author: Yong-Dian Jian + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gtsam { + +class GaussianFactorGraph; +class KeyInfo; +class Preconditioner; +class PreconditionerParameters; + +/*****************************************************************************/ +struct PCGSolverParameters: public ConjugateGradientParameters { +public: + typedef ConjugateGradientParameters Base; + typedef boost::shared_ptr shared_ptr; + + PCGSolverParameters() {} + + virtual void print(std::ostream &os) const; + + /* interface to preconditioner parameters */ + inline const PreconditionerParameters& preconditioner() const { + return *preconditioner_; + } + + boost::shared_ptr preconditioner_; +}; + +/*****************************************************************************/ +/* A virtual base class for the preconditioned conjugate gradient solver */ +class PCGSolver: public IterativeSolver { +public: + typedef IterativeSolver Base; + typedef boost::shared_ptr shared_ptr; + +protected: + + PCGSolverParameters parameters_; + boost::shared_ptr 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 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 &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 &lambda); + + const GaussianFactorGraph &gfg_; + const Preconditioner &preconditioner_; + const KeyInfo &keyInfo_; + const std::map &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 & 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 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 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); + +} + diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp new file mode 100644 index 000000000..c233c431b --- /dev/null +++ b/gtsam/linear/Preconditioner.cpp @@ -0,0 +1,212 @@ +/* + * Preconditioner.cpp + * + * Created on: Feb 2, 2012 + * Author: ydjian + */ + + +//#include +#include +//#include +#include +#include +//#include +//#include +//#include +#include +//#include +//#include +//#include +//#include +//#include +//#include +#include +#include +#include +#include + +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 createPreconditioner(const boost::shared_ptr parameters) { + + DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast(parameters); + if ( dummy ) { + return boost::make_shared(); + } + +// BlockJacobiPreconditioner::Parameters::shared_ptr jacobi = boost::dynamic_pointer_cast(parameters); +// if ( jacobi ) { +// BlockJacobiPreconditioner::shared_ptr p(new BlockJacobiPreconditioner(jacobi)); +// return p; +// } +// +// MIQRPreconditioner::Parameters::shared_ptr miqr = boost::dynamic_pointer_cast(parameters); +// if ( miqr ) { +// MIQRPreconditioner::shared_ptr p(new MIQRPreconditioner(miqr)); +// return p; +// } +// +// CombinatorialPreconditioner::Parameters::shared_ptr combinatorial +// = boost::dynamic_pointer_cast(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 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 */); +//} + + +} + + diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h new file mode 100644 index 000000000..5ca5ec70b --- /dev/null +++ b/gtsam/linear/Preconditioner.h @@ -0,0 +1,173 @@ +/* + * Preconditioner.h + * + * Created on: Feb 1, 2012 + * Author: ydjian + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + +class GaussianFactorGraph; +class KeyInfo; +class VectorValues; +//class Subgraph; + +/* parameters for the preconditioner */ +struct PreconditionerParameters { + + typedef boost::shared_ptr 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 shared_ptr; + typedef std::vector 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 &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 shared_ptr; + DummyPreconditionerParameters() : Base() {} + virtual ~DummyPreconditionerParameters() {} +}; + +/*******************************************************************************************/ +class DummyPreconditioner : public Preconditioner { +public: + typedef Preconditioner Base; + typedef boost::shared_ptr 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 &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 createPreconditioner(const boost::shared_ptr 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 +//buildFactorSubgraph(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo, +// const Subgraph &subgraph, const int hessian, const double lambda, const bool clone, const bool includeUnary); + +} + + diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index f6557a2c2..a66182281 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -15,8 +15,8 @@ #include #include #include - #include +#include 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 &Rc1, const boost::shared_ptr &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 &lambda, + const VectorValues &initial + ) { return VectorValues(); } protected: diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 678cac6b4..2a6eb5887 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -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( - params.iterativeParams)) { - SubgraphSolver solver(gfg, - dynamic_cast(*params.iterativeParams), - *params.ordering); - delta = solver.optimize(); - } else { + throw std::runtime_error("NonlinearOptimizer::solve: cg parameter has to be assigned ..."); + + if (boost::shared_ptr pcg = boost::dynamic_pointer_cast(params.iterativeParams) ) { + delta = PCGSolver(*pcg).optimize(gfg); + } + else if (boost::shared_ptr spcg = boost::dynamic_pointer_cast(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 ..."); } diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp new file mode 100644 index 000000000..ea9b5ec6c --- /dev/null +++ b/tests/testPCGSolver.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include // for operator += +using namespace boost::assign; + +#include +#include + +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(); + pcg->preconditioner_ = boost::make_shared(); + 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); +} +