From 17426d0076dfbb35742c413b1947f8ba90fc2fda Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Sun, 15 Jun 2014 23:14:06 -0400 Subject: [PATCH] subgraph preconditioner revised --- gtsam/linear/ConjugateGradientSolver.h | 2 +- gtsam/linear/IterativeSolver.h | 4 +- gtsam/linear/PCGSolver.cpp | 236 ------ gtsam/linear/PCGSolver.h | 133 +--- gtsam/linear/Preconditioner.cpp | 120 +-- gtsam/linear/Preconditioner.h | 26 +- gtsam/linear/SubgraphPreconditioner.cpp | 722 +++++++++++++++--- gtsam/linear/SubgraphPreconditioner.h | 207 ++++- gtsam/linear/SubgraphSolver.cpp | 8 + gtsam/linear/SubgraphSolver.h | 6 +- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 1 + gtsam/nonlinear/NonlinearOptimizer.cpp | 1 + tests/testPCGSolver.cpp | 22 +- 13 files changed, 873 insertions(+), 615 deletions(-) diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index 47ec4b405..2382a0eb1 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -19,7 +19,7 @@ namespace gtsam { /** * parameters for the conjugate gradient method */ -class ConjugateGradientParameters : public IterativeOptimizationParameters { +class GTSAM_EXPORT ConjugateGradientParameters : public IterativeOptimizationParameters { public: typedef IterativeOptimizationParameters Base; diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 66006875b..e02d92e3a 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -115,6 +115,7 @@ namespace gtsam { class KeyInfo : public std::map { public: typedef std::map Base; + KeyInfo() {} KeyInfo(const GaussianFactorGraph &fg); KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); @@ -132,9 +133,6 @@ namespace gtsam { Ordering ordering_; size_t numCols_; - private: - KeyInfo() {} - }; diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp index 6bfa389fb..27eb57b44 100644 --- a/gtsam/linear/PCGSolver.cpp +++ b/gtsam/linear/PCGSolver.cpp @@ -198,240 +198,4 @@ VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { 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 index 034352c50..8d48da149 100644 --- a/gtsam/linear/PCGSolver.h +++ b/gtsam/linear/PCGSolver.h @@ -25,7 +25,7 @@ class Preconditioner; class PreconditionerParameters; /*****************************************************************************/ -struct PCGSolverParameters: public ConjugateGradientParameters { +struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; typedef boost::shared_ptr shared_ptr; @@ -44,7 +44,7 @@ public: /*****************************************************************************/ /* A virtual base class for the preconditioned conjugate gradient solver */ -class PCGSolver: public IterativeSolver { +class GTSAM_EXPORT PCGSolver: public IterativeSolver { public: typedef IterativeSolver Base; typedef boost::shared_ptr shared_ptr; @@ -54,50 +54,21 @@ 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 { +class GTSAM_EXPORT GaussianFactorGraphSystem { public: GaussianFactorGraphSystem(const GaussianFactorGraph &gfg, @@ -134,103 +105,5 @@ VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, /**********************************************************************************/ 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 index 42ea56e68..c180f1160 100644 --- a/gtsam/linear/Preconditioner.cpp +++ b/gtsam/linear/Preconditioner.cpp @@ -1,27 +1,16 @@ /* * Preconditioner.cpp * - * Created on: Feb 2, 2012 + * Created on: Jun 2, 2014 * Author: ydjian */ - -//#include #include -//#include #include #include #include -//#include -//#include -//#include +#include #include -//#include -//#include -//#include -//#include -//#include -//#include #include #include #include @@ -31,7 +20,6 @@ using namespace std; namespace gtsam { - /*****************************************************************************/ void PreconditionerParameters::print() const { print(cout); @@ -214,110 +202,12 @@ boost::shared_ptr createPreconditioner(const boost::shared_ptr

(parameters) ) { 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); -// } + else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast(parameters) ) { + return boost::make_shared(*subgraph); + } 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 index 17c12446f..10ceb78a9 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -1,7 +1,7 @@ /* * Preconditioner.h * - * Created on: Feb 1, 2012 + * Created on: Jun 2, 2014 * Author: ydjian */ @@ -20,7 +20,7 @@ class KeyInfo; class VectorValues; /* parameters for the preconditioner */ -struct PreconditionerParameters { +struct GTSAM_EXPORT PreconditionerParameters { typedef boost::shared_ptr shared_ptr; @@ -54,14 +54,12 @@ struct PreconditionerParameters { /* 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 { +class GTSAM_EXPORT Preconditioner { public: typedef boost::shared_ptr shared_ptr; typedef std::vector Dimensions; @@ -93,7 +91,7 @@ public: }; /*******************************************************************************************/ -struct DummyPreconditionerParameters : public PreconditionerParameters { +struct GTSAM_EXPORT DummyPreconditionerParameters : public PreconditionerParameters { typedef PreconditionerParameters Base; typedef boost::shared_ptr shared_ptr; DummyPreconditionerParameters() : Base() {} @@ -101,7 +99,7 @@ struct DummyPreconditionerParameters : public PreconditionerParameters { }; /*******************************************************************************************/ -class DummyPreconditioner : public Preconditioner { +class GTSAM_EXPORT DummyPreconditioner : public Preconditioner { public: typedef Preconditioner Base; typedef boost::shared_ptr shared_ptr; @@ -129,14 +127,14 @@ public: }; /*******************************************************************************************/ -struct BlockJacobiPreconditionerParameters : public PreconditionerParameters { +struct GTSAM_EXPORT BlockJacobiPreconditionerParameters : public PreconditionerParameters { typedef PreconditionerParameters Base; BlockJacobiPreconditionerParameters() : Base() {} virtual ~BlockJacobiPreconditionerParameters() {} }; /*******************************************************************************************/ -class BlockJacobiPreconditioner : public Preconditioner { +class GTSAM_EXPORT BlockJacobiPreconditioner : public Preconditioner { public: typedef Preconditioner Base; BlockJacobiPreconditioner() ; @@ -167,16 +165,6 @@ protected: /* 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/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 44cb8c146..20b434b9f 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -15,126 +15,646 @@ * @author: Frank Dellaert */ +#include #include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include using namespace std; namespace gtsam { - /* ************************************************************************* */ - static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { - GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); - BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { - JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); - if( !jf ) { - jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) +/* ************************************************************************* */ +static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { + GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); + BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { + JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); + if( !jf ) { + jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + } + result->push_back(jf); + } + return result; +} + +/*****************************************************************************/ +static std::vector iidSampler(const vector &weight, const size_t n) { + + /* compute the sum of the weights */ + const double sum = std::accumulate(weight.begin(), weight.end(), 0.0); + + /* make a normalized and accumulated version of the weight vector */ + const size_t m = weight.size(); + vector w; w.reserve(m); + for ( size_t i = 0 ; i < m ; ++i ) { + w.push_back(weight[i]/sum); + } + + vector acc(m); + std::partial_sum(w.begin(),w.end(),acc.begin()); + + /* iid sample n times */ + vector result; result.reserve(n); + const double denominator = (double)RAND_MAX; + for ( size_t i = 0 ; i < n ; ++i ) { + const double value = rand() / denominator; + /* binary search the interval containing "value" */ + vector::iterator it = std::lower_bound(acc.begin(), acc.end(), value); + size_t idx = it - acc.begin(); + result.push_back(idx); + } + return result; +} + +/*****************************************************************************/ +vector uniqueSampler(const vector &weight, const size_t n) { + + const size_t m = weight.size(); + if ( n > m ) throw std::invalid_argument("uniqueSampler: invalid input size"); + + vector result; + + size_t count = 0; + std::vector touched(m, false); + while ( count < n ) { + std::vector localIndices; localIndices.reserve(n-count); + std::vector localWeights; localWeights.reserve(n-count); + + /* collect data */ + for ( size_t i = 0 ; i < m ; ++i ) { + if ( !touched[i] ) { + localIndices.push_back(i); + localWeights.push_back(weight[i]); } - result->push_back(jf); - } - return result; - } - - /* ************************************************************************* */ - SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, - const sharedBayesNet& Rc1, const sharedValues& xbar) : - Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))) { - } - - /* ************************************************************************* */ - // x = xbar + inv(R1)*y - VectorValues SubgraphPreconditioner::x(const VectorValues& y) const { - return *xbar_ + Rc1_->backSubstitute(y); - } - - /* ************************************************************************* */ - double SubgraphPreconditioner::error(const VectorValues& y) const { - Errors e(y); - VectorValues x = this->x(y); - Errors e2 = Ab2()->gaussianErrors(x); - return 0.5 * (dot(e, e) + dot(e2,e2)); - } - - /* ************************************************************************* */ - // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), - VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const { - VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */ - Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */ - VectorValues v = VectorValues::Zero(x); - Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ - return y + Rc1()->backSubstituteTranspose(v); - } - - /* ************************************************************************* */ - // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] - Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { - - Errors e(y); - VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */ - Errors e2 = *Ab2() * x; /* A2*x */ - e.splice(e.end(), e2); - return e; - } - - /* ************************************************************************* */ - // In-place version that overwrites e - void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { - - Errors::iterator ei = e.begin(); - for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) { - *ei = y[i]; } - // Add A2 contribution - VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y - Ab2()->multiplyInPlace(x, ei); // use iterator version - } - - /* ************************************************************************* */ - // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 - VectorValues SubgraphPreconditioner::operator^(const Errors& e) const { - - Errors::const_iterator it = e.begin(); - VectorValues y = zero(); - for ( Key i = 0 ; i < y.size() ; ++i, ++it ) - y[i] = *it ; - transposeMultiplyAdd2(1.0,it,e.end(),y); - return y; - } - - /* ************************************************************************* */ - // y += alpha*A'*e - void SubgraphPreconditioner::transposeMultiplyAdd - (double alpha, const Errors& e, VectorValues& y) const { - - Errors::const_iterator it = e.begin(); - for ( Key i = 0 ; i < y.size() ; ++i, ++it ) { - const Vector& ei = *it; - axpy(alpha, ei, y[i]); + /* sampling and cache results */ + vector samples = iidSampler(localWeights, n-count); + BOOST_FOREACH ( const size_t &id, samples ) { + if ( touched[id] == false ) { + touched[id] = true ; + result.push_back(id); + if ( ++count >= n ) break; + } } - transposeMultiplyAdd2(alpha, it, e.end(), y); + } + return result; +} + +/****************************************************************************/ +Subgraph::Subgraph(const std::vector &indices) { + edges_.reserve(indices.size()); + BOOST_FOREACH ( const size_t &idx, indices ) { + edges_.push_back(SubgraphEdge(idx, 1.0)); + } +} + +/****************************************************************************/ +std::vector Subgraph::edgeIndices() const { + std::vector eid; eid.reserve(size()); + BOOST_FOREACH ( const SubgraphEdge &edge, edges_ ) { + eid.push_back(edge.index_); + } + return eid; +} + +/****************************************************************************/ +void Subgraph::save(const std::string &fn) const { + std::ofstream os(fn.c_str()); + boost::archive::text_oarchive oa(os); + oa << *this; + os.close(); +} + +/****************************************************************************/ +Subgraph::shared_ptr Subgraph::load(const std::string &fn) { + std::ifstream is(fn.c_str()); + boost::archive::text_iarchive ia(is); + Subgraph::shared_ptr subgraph(new Subgraph()); + ia >> *subgraph; + is.close(); + return subgraph; +} + +/****************************************************************************/ +std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) { + if ( edge.weight() != 1.0 ) + os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")"; + else + os << edge.index() ; + return os; +} + +/****************************************************************************/ +std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) { + os << "Subgraph" << endl; + BOOST_FOREACH ( const SubgraphEdge &e, subgraph.edges() ) { + os << e << ", " ; + } + return os; +} + +/*****************************************************************************/ +void SubgraphBuilderParameters::print() const { + print(cout); +} + +/***************************************************************************************/ +void SubgraphBuilderParameters::print(ostream &os) const { + os << "SubgraphBuilderParameters" << endl + << "skeleton: " << skeletonTranslator(skeleton_) << endl + << "skeleton weight: " << skeletonWeightTranslator(skeletonWeight_) << endl + << "augmentation weight: " << augmentationWeightTranslator(augmentationWeight_) << endl + ; +} + +/*****************************************************************************/ +ostream& operator<<(ostream &os, const SubgraphBuilderParameters &p) { + p.print(os); + return os; +} + +/*****************************************************************************/ +SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslator(const std::string &src){ + std::string s = src; boost::algorithm::to_upper(s); + if (s == "NATURALCHAIN") return NATURALCHAIN; + else if (s == "BFS") return BFS; + else if (s == "KRUSKAL") return KRUSKAL; + throw invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s); + return KRUSKAL; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton w) { + if ( w == NATURALCHAIN )return "NATURALCHAIN"; + else if ( w == BFS ) return "BFS"; + else if ( w == KRUSKAL )return "KRUSKAL"; + else return "UNKNOWN"; +} + +/****************************************************************/ +SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "EQUAL") return EQUAL; + else if (s == "RHS") return RHS_2NORM; + else if (s == "LHS") return LHS_FNORM; + else if (s == "RANDOM") return RANDOM; + throw invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s); + return EQUAL; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w) { + if ( w == EQUAL ) return "EQUAL"; + else if ( w == RHS_2NORM ) return "RHS"; + else if ( w == LHS_FNORM ) return "LHS"; + else if ( w == RANDOM ) return "RANDOM"; + else return "UNKNOWN"; +} + +/****************************************************************/ +SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "SKELETON") return SKELETON; +// else if (s == "STRETCH") return STRETCH; +// else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH; + throw invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s); + return SKELETON; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::augmentationWeightTranslator(AugmentationWeight w) { + if ( w == SKELETON ) return "SKELETON"; +// else if ( w == STRETCH ) return "STRETCH"; +// else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH"; + else return "UNKNOWN"; +} + +/****************************************************************/ +std::vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const { + const SubgraphBuilderParameters &p = parameters_; + switch (p.skeleton_) { + case SubgraphBuilderParameters::NATURALCHAIN: + return natural_chain(gfg); + break; + case SubgraphBuilderParameters::BFS: + return bfs(gfg); + break; + case SubgraphBuilderParameters::KRUSKAL: + return kruskal(gfg, ordering, w); + break; + default: + cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl; + break; + } + return vector(); +} + +/****************************************************************/ +std::vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { + std::vector result ; + size_t idx = 0; + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + if ( gf->size() == 1 ) { + result.push_back(idx); + } + idx++; + } + return result; +} + +/****************************************************************/ +std::vector SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const { + std::vector result ; + size_t idx = 0; + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + if ( gf->size() == 2 ) { + const Key k0 = gf->keys()[0], k1 = gf->keys()[1]; + if ( (k1-k0) == 1 || (k0-k1) == 1 ) + result.push_back(idx); + } + idx++; + } + return result; +} + +/****************************************************************/ +std::vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { + const VariableIndex variableIndex(gfg); + /* start from the first key of the first factor */ + Key seed = gfg[0]->keys()[0]; + + const size_t n = variableIndex.size(); + + /* each vertex has self as the predecessor */ + std::vector result; + result.reserve(n-1); + + /* Initialize */ + std::queue q; + q.push(seed); + + std::set flags; + flags.insert(seed); + + /* traversal */ + while ( !q.empty() ) { + const size_t head = q.front(); q.pop(); + BOOST_FOREACH ( const size_t id, variableIndex[head] ) { + const GaussianFactor &gf = *gfg[id]; + BOOST_FOREACH ( const size_t key, gf.keys() ) { + if ( flags.count(key) == 0 ) { + q.push(key); + flags.insert(key); + result.push_back(id); + } + } + } + } + return result; +} + +/****************************************************************/ +std::vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const { + const VariableIndex variableIndex(gfg); + const size_t n = variableIndex.size(); + const vector idx = sort_idx(w) ; + + /* initialize buffer */ + vector result; + result.reserve(n-1); + + // container for acsendingly sorted edges + DSFVector D(n) ; + + size_t count = 0 ; double sum = 0.0 ; + BOOST_FOREACH (const size_t id, idx) { + const GaussianFactor &gf = *gfg[id]; + if ( gf.keys().size() != 2 ) continue; + const size_t u = ordering.find(gf.keys()[0])->second, + u_root = D.find(u), + v = ordering.find(gf.keys()[1])->second, + v_root = D.find(v) ; + if ( u_root != v_root ) { + D.merge(u_root, v_root) ; + result.push_back(id) ; + sum += w[id] ; + if ( ++count == n-1 ) break ; + } + } + return result; +} + +/****************************************************************/ +std::vector SubgraphBuilder::sample(const std::vector &weights, const size_t t) const { + return uniqueSampler(weights, t); +} + +/****************************************************************/ +Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const { + + const SubgraphBuilderParameters &p = parameters_; + const Ordering inverse_ordering = Ordering::Natural(gfg); + const FastMap forward_ordering = inverse_ordering.invert(); + const size_t n = inverse_ordering.size(), t = n * p.complexity_ ; + + vector w = weights(gfg); + const vector tree = buildTree(gfg, forward_ordering, w); + + /* sanity check */ + if ( tree.size() != n-1 ) { + throw runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed "); } - /* ************************************************************************* */ - // y += alpha*inv(R1')*A2'*e2 - void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha, + /* down weight the tree edges to zero */ + BOOST_FOREACH ( const size_t id, tree ) { + w[id] = 0.0; + } + + /* decide how many edges to augment */ + std::vector offTree = sample(w, t); + + vector subgraph = unary(gfg); + subgraph.insert(subgraph.end(), tree.begin(), tree.end()); + subgraph.insert(subgraph.end(), offTree.begin(), offTree.end()); + + return boost::make_shared(subgraph); +} + +/****************************************************************/ +SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg) const +{ + const size_t m = gfg.size() ; + Weights weight; weight.reserve(m); + + BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg ) { + switch ( parameters_.skeletonWeight_ ) { + case SubgraphBuilderParameters::EQUAL: + weight.push_back(1.0); + break; + case SubgraphBuilderParameters::RHS_2NORM: + { + if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { + weight.push_back(jf->getb().norm()); + } + else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { + weight.push_back(hf->linearTerm().norm()); + } + } + break; + case SubgraphBuilderParameters::LHS_FNORM: + { + if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { + weight.push_back(std::sqrt(jf->getA().squaredNorm())); + } + else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { + weight.push_back(std::sqrt(hf->information().squaredNorm())); + } + } + break; + + case SubgraphBuilderParameters::RANDOM: + weight.push_back(std::rand()%100 + 1.0); + break; + + default: + throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme "); + break; + } + } + return weight; +} + +/* ************************************************************************* */ +SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) : + parameters_(p) {} + +/* ************************************************************************* */ +SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, + const sharedBayesNet& Rc1, const sharedValues& xbar, const SubgraphPreconditionerParameters &p) : + Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), + b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))), parameters_(p) { +} + +/* ************************************************************************* */ +// x = xbar + inv(R1)*y +VectorValues SubgraphPreconditioner::x(const VectorValues& y) const { + return *xbar_ + Rc1_->backSubstitute(y); +} + +/* ************************************************************************* */ +double SubgraphPreconditioner::error(const VectorValues& y) const { + Errors e(y); + VectorValues x = this->x(y); + Errors e2 = Ab2()->gaussianErrors(x); + return 0.5 * (dot(e, e) + dot(e2,e2)); +} + +/* ************************************************************************* */ +// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), +VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const { + VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */ + Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */ + VectorValues v = VectorValues::Zero(x); + Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ + return y + Rc1()->backSubstituteTranspose(v); +} + +/* ************************************************************************* */ +// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] +Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { + + Errors e(y); + VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */ + Errors e2 = *Ab2() * x; /* A2*x */ + e.splice(e.end(), e2); + return e; +} + +/* ************************************************************************* */ +// In-place version that overwrites e +void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { + + Errors::iterator ei = e.begin(); + for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) { + *ei = y[i]; + } + + // Add A2 contribution + VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y + Ab2()->multiplyInPlace(x, ei); // use iterator version +} + +/* ************************************************************************* */ +// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 +VectorValues SubgraphPreconditioner::operator^(const Errors& e) const { + + Errors::const_iterator it = e.begin(); + VectorValues y = zero(); + for ( Key i = 0 ; i < y.size() ; ++i, ++it ) + y[i] = *it ; + transposeMultiplyAdd2(1.0,it,e.end(),y); + return y; +} + +/* ************************************************************************* */ +// y += alpha*A'*e +void SubgraphPreconditioner::transposeMultiplyAdd +(double alpha, const Errors& e, VectorValues& y) const { + + Errors::const_iterator it = e.begin(); + for ( Key i = 0 ; i < y.size() ; ++i, ++it ) { + const Vector& ei = *it; + axpy(alpha, ei, y[i]); + } + transposeMultiplyAdd2(alpha, it, e.end(), y); +} + +/* ************************************************************************* */ +// y += alpha*inv(R1')*A2'*e2 +void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha, Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const { - // create e2 with what's left of e - // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ? - Errors e2; - while (it != end) e2.push_back(*(it++)); + // create e2 with what's left of e + // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ? + Errors e2; + while (it != end) e2.push_back(*(it++)); - VectorValues x = VectorValues::Zero(y); // x = 0 - Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2 - axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x + VectorValues x = VectorValues::Zero(y); // x = 0 + Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2 + axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x +} + +/* ************************************************************************* */ +void SubgraphPreconditioner::print(const std::string& s) const { + cout << s << endl; + Ab2_->print(); +} + +/*****************************************************************************/ +void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const +{ + /* copy first */ + std::copy(y.data(), y.data() + y.rows(), x.data()); + + /* in place back substitute */ + BOOST_REVERSE_FOREACH(const boost::shared_ptr & cg, *Rc1_) { + /* collect a subvector of x that consists of the parents of cg (S) */ + const Vector xParent = getSubvector(x, keyInfo_, FastVector(cg->beginParents(), cg->endParents())); + const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals())); + + /* compute the solution for the current pivot */ + const Vector solFrontal = cg->get_R().triangularView().solve(rhsFrontal - cg->get_S() * xParent); + + /* assign subvector of sol to the frontal variables */ + setSubvector(solFrontal, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals()), x); + } +} + +/*****************************************************************************/ +void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const +{ + /* copy first */ + std::copy(y.data(), y.data() + y.rows(), x.data()); + + /* in place back substitute */ + BOOST_FOREACH(const boost::shared_ptr & cg, *Rc1_) { + const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals())); +// const Vector solFrontal = cg->get_R().triangularView().transpose().solve(rhsFrontal); + const Vector solFrontal = cg->get_R().transpose().triangularView().solve(rhsFrontal); + + // Check for indeterminant solution + if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front()); + + /* assign subvector of sol to the frontal variables */ + setSubvector(solFrontal, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals()), x); + + /* substract from parent variables */ + for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) { + KeyInfo::const_iterator it2 = keyInfo_.find(*it); + Eigen::Map rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1); + rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal; + } + } +} + +/*****************************************************************************/ +void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) +{ + /* identify the subgraph structure */ + const SubgraphBuilder builder(parameters_.builderParams_); + Subgraph::shared_ptr subgraph = builder(gfg); + + keyInfo_ = keyInfo; + + /* build factor subgraph */ + GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, *subgraph, true); + + /* factorize and cache BayesNet */ + Rc1_ = gfg_subgraph->eliminateSequential(); +} + +/*****************************************************************************/ +Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys) { + + /* a cache of starting index and dim */ + typedef vector > Cache; + Cache cache; + + /* figure out dimension by traversing the keys */ + size_t d = 0; + BOOST_FOREACH ( const Key &key, keys ) { + const KeyInfoEntry &entry = keyInfo.find(key)->second; + cache.push_back(make_pair(entry.colstart(), entry.dim())); + d += entry.dim(); } - /* ************************************************************************* */ - void SubgraphPreconditioner::print(const std::string& s) const { - cout << s << endl; - Ab2_->print(); + /* use the cache to fill the result */ + Vector result = Vector::Zero(d, 1); + size_t idx = 0; + BOOST_FOREACH ( const Cache::value_type &p, cache ) { + result.segment(idx, p.second) = src.segment(p.first, p.second) ; + idx += p.second; } + + return result; +} + +/*****************************************************************************/ +void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys, Vector &dst) { + /* use the cache */ + size_t idx = 0; + BOOST_FOREACH ( const Key &key, keys ) { + const KeyInfoEntry &entry = keyInfo.find(key)->second; + dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ; + idx += entry.dim(); + } +} + +/*****************************************************************************/ +boost::shared_ptr +buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) { + + GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); + result->reserve(subgraph.size()); + BOOST_FOREACH ( const SubgraphEdge &e, subgraph ) { + const size_t idx = e.index(); + if ( clone ) result->push_back(gfg[idx]->clone()); + else result->push_back(gfg[idx]); + } + return result; +} + } // nsamespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index d9d7524b6..e237b5853 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -12,15 +12,16 @@ /** * @file SubgraphPreconditioner.h * @date Dec 31, 2009 - * @author Frank Dellaert + * @author Frank Dellaert, Yong-Dian Jian */ #pragma once #include -#include #include - +#include +#include +#include #include namespace gtsam { @@ -30,6 +31,146 @@ namespace gtsam { class GaussianFactorGraph; class VectorValues; + struct SubgraphEdge { + size_t index_; /* edge id */ + double weight_; /* edge weight */ + SubgraphEdge() : index_(0), weight_(1.0) {} + SubgraphEdge(const SubgraphEdge &e) : index_(e.index()), weight_(e.weight()) {} + SubgraphEdge(const size_t index, const double weight = 1.0): index_(index), weight_(weight) {} + inline size_t index() const { return index_; } + inline double weight() const { return weight_; } + inline bool isUnitWeight() const { return (weight_ == 1.0); } + friend std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge); + private: + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(index_); + ar & BOOST_SERIALIZATION_NVP(weight_); + } + }; + + /**************************************************************************/ + class Subgraph { + public: + typedef boost::shared_ptr shared_ptr; + typedef std::vector vector_shared_ptr; + typedef std::vector Edges; + typedef std::vector EdgeIndices; + typedef Edges::iterator iterator; + typedef Edges::const_iterator const_iterator; + + protected: + Edges edges_; /* index to the factors */ + + public: + Subgraph() {} + Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {} + Subgraph(const Edges &edges) : edges_(edges) {} + Subgraph(const std::vector &indices) ; + + inline const Edges& edges() const { return edges_; } + inline const size_t size() const { return edges_.size(); } + EdgeIndices edgeIndices() const; + + iterator begin() { return edges_.begin(); } + const_iterator begin() const { return edges_.begin(); } + iterator end() { return edges_.end(); } + const_iterator end() const { return edges_.end(); } + + void save(const std::string &fn) const; + static shared_ptr load(const std::string &fn); + friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph); + + private: + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(edges_); + } + }; + + /****************************************************************************/ + struct SubgraphBuilderParameters { + public: + typedef boost::shared_ptr shared_ptr; + + enum Skeleton { + /* augmented tree */ + NATURALCHAIN = 0, /* natural ordering of the graph */ + BFS, /* breadth-first search tree */ + KRUSKAL, /* maximum weighted spanning tree */ + } skeleton_ ; + + enum SkeletonWeight { /* how to weigh the graph edges */ + EQUAL = 0, /* every block edge has equal weight */ + RHS_2NORM, /* use the 2-norm of the rhs */ + LHS_FNORM, /* use the frobenius norm of the lhs */ + RANDOM, /* bounded random edge weight */ + } skeletonWeight_ ; + + enum AugmentationWeight { /* how to weigh the graph edges */ + SKELETON = 0, /* use the same weights in building the skeleton */ +// STRETCH, /* stretch in the laplacian sense */ +// GENERALIZED_STRETCH /* the generalized stretch defined in jian2013iros */ + } augmentationWeight_ ; + + double complexity_; + + SubgraphBuilderParameters() + : skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {} + virtual ~SubgraphBuilderParameters() {} + + /* for serialization */ + void print() const ; + virtual void print(std::ostream &os) const ; + friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p); + + static Skeleton skeletonTranslator(const std::string &s); + static std::string skeletonTranslator(Skeleton w); + static SkeletonWeight skeletonWeightTranslator(const std::string &s); + static std::string skeletonWeightTranslator(SkeletonWeight w); + static AugmentationWeight augmentationWeightTranslator(const std::string &s); + static std::string augmentationWeightTranslator(AugmentationWeight w); + }; + + /*****************************************************************************/ + class SubgraphBuilder { + + public: + typedef SubgraphBuilder Base; + typedef boost::shared_ptr shared_ptr; + typedef std::vector Weights; + + SubgraphBuilder(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) + : parameters_(p) {} + virtual ~SubgraphBuilder() {} + virtual boost::shared_ptr operator() (const GaussianFactorGraph &jfg) const ; + + private: + std::vector buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &weights) const ; + std::vector unary(const GaussianFactorGraph &gfg) const ; + std::vector natural_chain(const GaussianFactorGraph &gfg) const ; + std::vector bfs(const GaussianFactorGraph &gfg) const ; + std::vector kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const ; + std::vector sample(const std::vector &weights, const size_t t) const ; + Weights weights(const GaussianFactorGraph &gfg) const; + SubgraphBuilderParameters parameters_; + + private: + SubgraphBuilder() {} + }; + + /*******************************************************************************************/ + struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters { + typedef PreconditionerParameters Base; + typedef boost::shared_ptr shared_ptr; + SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) + : Base(), builderParams_(p) {} + virtual ~SubgraphPreconditionerParameters() {} + SubgraphBuilderParameters builderParams_; + }; + /** * Subgraph conditioner class, as explained in the RSS 2010 submission. * Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2 @@ -37,7 +178,7 @@ namespace gtsam { * To use the class, give the Bayes Net R1*x=c1 and Graph A2*x=b2. * Then solve for yhat using CG, and solve for xhat = system.x(yhat). */ - class GTSAM_EXPORT SubgraphPreconditioner { + class GTSAM_EXPORT SubgraphPreconditioner : public Preconditioner { public: typedef boost::shared_ptr shared_ptr; @@ -52,9 +193,12 @@ namespace gtsam { sharedValues xbar_; ///< A1 \ b1 sharedErrors b2bar_; ///< A2*xbar - b2 + KeyInfo keyInfo_; + SubgraphPreconditionerParameters parameters_; + public: - SubgraphPreconditioner(); + SubgraphPreconditioner(const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters()); /** * Constructor @@ -62,7 +206,10 @@ namespace gtsam { * @param Rc1: the Bayes Net R1*x=c1 * @param xbar: the solution to R1*x=c1 */ - SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar); + SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar, + const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters()); + + virtual ~SubgraphPreconditioner() {} /** print the object */ void print(const std::string& s = "SubgraphPreconditioner") const; @@ -119,6 +266,54 @@ namespace gtsam { * y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2] */ void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& y) const; + + /*****************************************************************************/ + /* implement virtual functions of Preconditioner */ + + /* Computation Interfaces for Vector */ + virtual void solve(const Vector& y, Vector &x) const; + virtual void transposeSolve(const Vector& y, Vector& x) const ; + + virtual void build( + const GaussianFactorGraph &gfg, + const KeyInfo &info, + const std::map &lambda + ) ; + /*****************************************************************************/ }; + /* get subvectors */ + Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys); + + /* set subvectors */ + void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys, Vector &dst); + + + /* build a factor subgraph, which is defined as a set of weighted edges (factors) */ + boost::shared_ptr + buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone); + + + /* sort the container and return permutation index with default comparator */ + template + std::vector sort_idx(const Container &src) + { + typedef typename Container::value_type T; + const size_t n = src.size() ; + std::vector > tmp; + tmp.reserve(n); + for ( size_t i = 0 ; i < n ; i++ ) + tmp.push_back(std::make_pair(i, src[i])); + + /* sort */ + std::stable_sort(tmp.begin(), tmp.end()) ; + + /* copy back */ + std::vector idx; idx.reserve(n); + for ( size_t i = 0 ; i < n ; i++ ) { + idx.push_back(tmp[i].first) ; + } + return idx; + } + } // namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 034fdeeaf..6f10d04ad 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -143,4 +144,11 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { return boost::tie(At, Ac); } +/****************************************************************************/ +VectorValues SubgraphSolver::optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda, + const VectorValues &initial +) { return VectorValues(); } } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index a66182281..e4d3f50f3 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -13,7 +13,6 @@ #include -#include #include #include #include @@ -23,6 +22,7 @@ namespace gtsam { // Forward declarations class GaussianFactorGraph; class GaussianBayesNet; + class SubgraphPreconditioner; class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters { public: @@ -61,7 +61,7 @@ public: protected: Parameters parameters_; Ordering ordering_; - SubgraphPreconditioner::shared_ptr pc_; ///< preconditioner object + boost::shared_ptr pc_; ///< preconditioner object public: /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ @@ -87,7 +87,7 @@ public: const KeyInfo &keyInfo, const std::map &lambda, const VectorValues &initial - ) { return VectorValues(); } + ) ; protected: diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index f365fc524..5e8e3923c 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include class NonlinearOptimizerMoreOptimizationTest; diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 2a6eb5887..88bece4e1 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -103,6 +103,7 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg, } else if (params.isSequential()) { 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 ..."); diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp index ef74ad1ef..38a40521a 100644 --- a/tests/testPCGSolver.cpp +++ b/tests/testPCGSolver.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -88,7 +89,6 @@ TEST( PCGSolver, llt ) { } - /* ************************************************************************* */ TEST( PCGSolver, dummy ) { @@ -129,6 +129,26 @@ TEST( PCGSolver, blockjacobi ) DOUBLES_EQUAL(0,fg.error(actualPCG),tol); } +/* ************************************************************************* */ +TEST( PCGSolver, subgraph ) +{ + 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;