subgraph preconditioner revised
Yong-Dian Jian
parent
67398f0f13
commit
17426d0076
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -115,6 +115,7 @@ namespace gtsam {
|
|||
class KeyInfo : public std::map<Key, KeyInfoEntry> {
|
||||
public:
|
||||
typedef std::map<Key, KeyInfoEntry> 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() {}
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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, VectorValues>(system, initial, parameters_);
|
||||
////}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//void PCGSolver_SBM::convertKernel(const JacobianFactorGraph &jfg, const double lambda) {
|
||||
// linear_ = buildSparseLinearSystem(jfg, keyInfo_, true /* AtA */, lambda, false /* column major */,
|
||||
// parameters_.blas_kernel_, parameters_.rb_kernel_);
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//VectorValues PCGSolver_SBM::optimize_(const VectorValues &initial) {
|
||||
// System system(linear_, preconditioner_);
|
||||
// Vector solution = preconditionedConjugateGradient<System, Vector>(
|
||||
// system, initial.vector(keyInfo_.ordering()), parameters_);
|
||||
// return buildVectorValues(solution, keyInfo_);
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//ydjian::SparseLinearSystem::shared_ptr
|
||||
//buildSparseLinearSystem(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo,
|
||||
// const bool AtA, const double lambda, const bool colMajor,
|
||||
// const PCGSolverParameters::BLASKernel blas, const PCGSolverParameters::RegisterBlockKernel rbSrc) {
|
||||
//
|
||||
// std::map<PCGSolverParameters::RegisterBlockKernel, ydjian::SBM::Type> rbMap;
|
||||
// rbMap[PCGSolverParameters::RB_NONE] = ydjian::SBM::NAIVE;
|
||||
// rbMap[PCGSolverParameters::SSE_RB22] = ydjian::SBM::SSE_RB22;
|
||||
// rbMap[PCGSolverParameters::SSE_RB44] = ydjian::SBM::SSE_RB44;
|
||||
// rbMap[PCGSolverParameters::AVX_RB44] = ydjian::SBM::AVX_RB44;
|
||||
//
|
||||
// ydjian::SBM::Type rb;
|
||||
// SC_ASSERT( rbMap.find(rbSrc) != rbMap.end(), "buildSparseLinearSystem: rbSrc is not supported..");
|
||||
// rb = rbMap[rbSrc];
|
||||
//
|
||||
// ydjian::SparseLinearSystem::shared_ptr linear;
|
||||
//
|
||||
// switch (blas) {
|
||||
//
|
||||
// case PCGSolverParameters::SBM:
|
||||
// linear = gsp2::buildSparseLinearSystem(jfg, keyInfo, AtA, lambda, colMajor, false /* multithread */, rb);
|
||||
// break;
|
||||
//
|
||||
// case PCGSolverParameters::SBM_MT:
|
||||
// linear = gsp2::buildSparseLinearSystem(jfg, keyInfo, AtA, lambda, colMajor, true /* multithread */, rb);
|
||||
// break;
|
||||
//
|
||||
// default:
|
||||
// throw std::invalid_argument("createSparseLinearSystem: unsupported blas kernel");
|
||||
// break;
|
||||
// }
|
||||
//
|
||||
// return linear;
|
||||
//}
|
||||
//
|
||||
///*****************************************************************************/
|
||||
//PCGSolver::shared_ptr createPCGSolver(const PCGSolverParameters ¶meters){
|
||||
//
|
||||
// PCGSolver::shared_ptr solver;
|
||||
// switch ( parameters.pcg_kernel_ ) {
|
||||
// case PCGSolverParameters::PCG:
|
||||
// switch ( parameters.blas_kernel_ ) {
|
||||
//
|
||||
//// case PCGSolverParameters::GTSAM:
|
||||
//// solver.reset(new PCGSolver_FG(parameters));
|
||||
//// break;
|
||||
//
|
||||
// case PCGSolverParameters::SBM:
|
||||
// case PCGSolverParameters::SBM_MT:
|
||||
// solver.reset(new PCGSolver_SBM(parameters));
|
||||
// break;
|
||||
//
|
||||
// default:
|
||||
// throw std::invalid_argument("createPCGSolver: undefined blas_kernel for pcgsolver");
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// case PCGSolverParameters::LSPCG:
|
||||
// switch ( parameters.blas_kernel_ ) {
|
||||
// case PCGSolverParameters::GTSAM:
|
||||
// solver.reset(new LSPCGSolver_FG(parameters));
|
||||
// break;
|
||||
//
|
||||
// case PCGSolverParameters::SBM:
|
||||
// case PCGSolverParameters::SBM_MT:
|
||||
// solver.reset(new LSPCGSolver_SBM(parameters));
|
||||
// break;
|
||||
//
|
||||
// default:
|
||||
// throw std::invalid_argument("createPCGSolver: undefined blas_kernel for lspcg solver");
|
||||
// break;
|
||||
// }
|
||||
// break;
|
||||
//
|
||||
// default:
|
||||
// throw std::invalid_argument("createPCGSolver: undefined pcg_kernel");
|
||||
// break;
|
||||
// }
|
||||
// return solver;
|
||||
//}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<PCGSolverParameters> 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<PCGSolver> shared_ptr;
|
||||
|
|
@ -54,50 +54,21 @@ protected:
|
|||
PCGSolverParameters parameters_;
|
||||
boost::shared_ptr<Preconditioner> preconditioner_;
|
||||
|
||||
// /* cached local variables */
|
||||
// KeyInfo keyInfo_;
|
||||
//
|
||||
// /* whether the preconditioner has be built */
|
||||
// bool built_ ;
|
||||
|
||||
public:
|
||||
/* Interface to initialize a solver without a problem */
|
||||
PCGSolver(const PCGSolverParameters &p);
|
||||
virtual ~PCGSolver() {}
|
||||
|
||||
// /* update interface to the nonlinear optimizer */
|
||||
// virtual void replaceFactors(
|
||||
// const GaussianFactorGraph &factorGraph,
|
||||
// const Values &linearization_point,
|
||||
// const std::map<Key, double> lambda);
|
||||
|
||||
// /* build the preconditioner */
|
||||
// void buildPreconditioner();
|
||||
|
||||
// /* interface for the NonlinearOptimizer, with initial estimate equals to a zero vector */
|
||||
// virtual VectorValues optimize() ;
|
||||
//
|
||||
// /* interface for the NonlinearOptimizer, with a custom initial estimate*/
|
||||
// virtual VectorValues optimize(const VectorValues &initial);
|
||||
|
||||
using IterativeSolver::optimize;
|
||||
|
||||
virtual VectorValues optimize(const GaussianFactorGraph &gfg,
|
||||
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
|
||||
const VectorValues &initial);
|
||||
|
||||
//protected:
|
||||
|
||||
// /* convert jacobian factor graph to solver-specific kernel */
|
||||
// virtual void convertKernel(const JacobianFactorGraph &jfg, const double lambda = 0.0) = 0 ;
|
||||
//
|
||||
// /* do the actual optimization */
|
||||
// virtual VectorValues optimize_(const VectorValues &initial) = 0;
|
||||
|
||||
};
|
||||
|
||||
/*****************************************************************************/
|
||||
class GaussianFactorGraphSystem {
|
||||
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<PCGSolver_FG> shared_ptr ;
|
||||
//
|
||||
//protected:
|
||||
// JacobianFactorGraph::shared_ptr hfg_; // A'*A + lambda*I
|
||||
//
|
||||
//public:
|
||||
// PCGSolver_FG(const Parameters ¶meters) : Base(parameters) {}
|
||||
// virtual ~PCGSolver_FG() {}
|
||||
//
|
||||
//protected:
|
||||
// virtual void convertKernel(const JacobianFactorGraph &jfg, const double lambda = 0.0);
|
||||
// virtual VectorValues optimize_(const VectorValues &initial);
|
||||
//
|
||||
// /* interface to the preconditionedConjugateGradient function template */
|
||||
// class System {
|
||||
// public:
|
||||
// typedef Vector State;
|
||||
// typedef Vector Residual;
|
||||
//
|
||||
// protected:
|
||||
// const JacobianFactorGraph::shared_ptr hfg_;
|
||||
// const Preconditioner::shared_ptr preconditioner_;
|
||||
// const KeyInfo &keyInfo_;
|
||||
// const RowInfo &rowInfo_;
|
||||
//
|
||||
// public:
|
||||
// System(const JacobianFactorGraph::shared_ptr hfg,
|
||||
// const Preconditioner::shared_ptr preconditioner,
|
||||
// const KeyInfo &keyInfo, const RowInfo &rowInfo);
|
||||
//
|
||||
// inline void residual(const State &x, State &r) const { gtsam::residual(*hfg_, x, r); }
|
||||
// inline void multiply(const State &x, State& y) const { gtsam::multiply(*hfg_, x, y); }
|
||||
// inline void leftPrecondition(const State &x, State &y) const { preconditioner_->transposeSolve(x, y); }
|
||||
// inline void rightPrecondition(const State &x, State &y) const { preconditioner_->solve(x, y); }
|
||||
// inline void scal(const double alpha, State &x) const { x.vector() *= alpha; }
|
||||
// inline double dot(const State &x, const State &y) const { return x.vector().dot(y.vector()); }
|
||||
// inline void axpy(const double alpha, const State &x, State &y) const { y.vector() += alpha*x.vector(); }
|
||||
// } ;
|
||||
//};
|
||||
//
|
||||
///*****************************************************************************/
|
||||
///** an specialization of the PCGSolver using sbm and customized blas kernel **/
|
||||
//class PCGSolver_SBM : public PCGSolver {
|
||||
//
|
||||
//public:
|
||||
// typedef PCGSolver Base;
|
||||
// typedef boost::shared_ptr<PCGSolver_SBM> shared_ptr ;
|
||||
//
|
||||
//protected:
|
||||
// ydjian::SparseLinearSystem::shared_ptr linear_;
|
||||
//
|
||||
//public:
|
||||
// PCGSolver_SBM(const Parameters ¶meters) : Base(parameters) {}
|
||||
// virtual ~PCGSolver_SBM() {}
|
||||
//
|
||||
//protected:
|
||||
// /* virtual function of the PCGSolver */
|
||||
// virtual void convertKernel(const JacobianFactorGraph &jfg, const double lambda = 0.0);
|
||||
// virtual VectorValues optimize_(const VectorValues &initial);
|
||||
//
|
||||
// /* interface to the preconditionedConjugateGradient function template */
|
||||
// class System {
|
||||
//
|
||||
// public:
|
||||
// typedef Vector State;
|
||||
//
|
||||
// protected:
|
||||
// const ydjian::SparseLinearSystem::shared_ptr linear_;
|
||||
// const Preconditioner::shared_ptr preconditioner_;
|
||||
//
|
||||
// public:
|
||||
// System(const ydjian::SparseLinearSystem::shared_ptr linear, const Preconditioner::shared_ptr preconditioner)
|
||||
// : linear_(linear), preconditioner_(preconditioner) {}
|
||||
// inline void residual(const State &x, State &r) const { linear_->residual(x.data(), r.data(), false); }
|
||||
// inline void multiply(const State &x, State& y) const { linear_->matrix()->multiply(x.data(), y.data(), false); }
|
||||
// inline void leftPrecondition(const State &x, State &y) const { preconditioner_->transposeSolve(x, y); }
|
||||
// inline void rightPrecondition(const State &x, State &y) const { preconditioner_->solve(x, y); }
|
||||
// inline void scal(const double alpha, State &x) const { x *= alpha; }
|
||||
// inline double dot(const State &x, const State &y) const { return x.dot(y); }
|
||||
// inline void axpy(const double alpha, const State &x, State &y) const { y += alpha*x; }
|
||||
// } ;
|
||||
//};
|
||||
//
|
||||
///* a factory method to instantiate PCGSolvers and its derivatives */
|
||||
//PCGSolver::shared_ptr createPCGSolver(const PCGParameters ¶meters);
|
||||
//
|
||||
///* an utility function to create sparse linear system, for all cg solvers using the sbm kernel */
|
||||
//ydjian::SparseLinearSystem::shared_ptr
|
||||
//buildSparseLinearSystem(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo, const bool AtA,
|
||||
// const double lambda, const bool colMajor, const PCGParameters::BLASKernel blas,
|
||||
// const PCGParameters::RegisterBlockKernel rb);
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,27 +1,16 @@
|
|||
/*
|
||||
* Preconditioner.cpp
|
||||
*
|
||||
* Created on: Feb 2, 2012
|
||||
* Created on: Jun 2, 2014
|
||||
* Author: ydjian
|
||||
*/
|
||||
|
||||
|
||||
//#include <gtsam/linear/CombinatorialPreconditioner.h>
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
//#include <gtsam/linear/FactorGraphUtil-inl.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/PCGSolver.h>
|
||||
#include <gtsam/linear/Preconditioner.h>
|
||||
//#include <gtsam/linear/JacobianFactorGraph.h>
|
||||
//#include <gtsam/linear/JacobiPreconditioner.h>
|
||||
//#include <gtsam/linear/MIQRPreconditioner.h>
|
||||
#include <gtsam/linear/SubgraphPreconditioner.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
//#include <gtsam/linear/Subgraph.h>
|
||||
//#include <gtsam/linear/SubgraphBuilder-inl.h>
|
||||
//#include <gtsam/linear/SuiteSparseSolver.h>
|
||||
//#include <gtsam/linear/SuiteSparseUtil.h>
|
||||
//#include <ydjian/tool/ThreadSafeTimer.h>
|
||||
//#include <ydjian/tool/Timer.h>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
#include <iostream>
|
||||
|
|
@ -31,7 +20,6 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
|
||||
/*****************************************************************************/
|
||||
void PreconditionerParameters::print() const {
|
||||
print(cout);
|
||||
|
|
@ -214,110 +202,12 @@ boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<P
|
|||
else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(parameters) ) {
|
||||
return boost::make_shared<BlockJacobiPreconditioner>();
|
||||
}
|
||||
|
||||
|
||||
// BlockJacobiPreconditioner::Parameters::shared_ptr jacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditioner::Parameters>(parameters);
|
||||
// if ( jacobi ) {
|
||||
// BlockJacobiPreconditioner::shared_ptr p(new BlockJacobiPreconditioner(jacobi));
|
||||
// return p;
|
||||
// }
|
||||
//
|
||||
// MIQRPreconditioner::Parameters::shared_ptr miqr = boost::dynamic_pointer_cast<MIQRPreconditioner::Parameters>(parameters);
|
||||
// if ( miqr ) {
|
||||
// MIQRPreconditioner::shared_ptr p(new MIQRPreconditioner(miqr));
|
||||
// return p;
|
||||
// }
|
||||
//
|
||||
// CombinatorialPreconditioner::Parameters::shared_ptr combinatorial
|
||||
// = boost::dynamic_pointer_cast<CombinatorialPreconditioner::Parameters>(parameters);
|
||||
// if ( combinatorial ) {
|
||||
// return createCombinatorialPreconditioner(combinatorial);
|
||||
// }
|
||||
else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast<SubgraphPreconditionerParameters>(parameters) ) {
|
||||
return boost::make_shared<SubgraphPreconditioner>(*subgraph);
|
||||
}
|
||||
|
||||
throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type");
|
||||
}
|
||||
//
|
||||
///***************************************************************************************/
|
||||
//JacobianFactorGraph::shared_ptr
|
||||
//buildFactorSubgraph(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo,
|
||||
// const Subgraph &subgraph, const int hessian, const double lambda, const bool clone, const bool includeUnary){
|
||||
//
|
||||
// const Subgraph::Edges &edges = subgraph.edges();
|
||||
// const size_t m = jfg.size(), n = keyInfo.size();
|
||||
// const bool augment = (lambda!=0.0) ? true : false ;
|
||||
//
|
||||
// /* produce an edge weight table */
|
||||
// std::vector<double> weights(m, 0.0);
|
||||
// BOOST_FOREACH ( const Subgraph::Edge &edge, edges ) {
|
||||
// weights[edge.index()] = edge.weight();
|
||||
// }
|
||||
//
|
||||
// /* collect the number of dangling unary factors */
|
||||
// /* upper bound of the factors */
|
||||
// size_t extraUnary = 0;
|
||||
// if ( includeUnary ) {
|
||||
// for ( Index i = 0 ; i < m ; ++i ) {
|
||||
// if ( weights[i] == 0.0 && jfg[i]->size() == 1 ) {
|
||||
// weights[i] = 1.0;
|
||||
// ++extraUnary;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//
|
||||
// const size_t e = edges.size() + extraUnary;
|
||||
// const size_t sz = e + (augment ? n : 0) + (hessian ? 2*(m-e) : 0);
|
||||
//
|
||||
// JacobianFactorGraph::shared_ptr target(new JacobianFactorGraph());
|
||||
// target->reserve(sz);
|
||||
//
|
||||
// /* auxiliary variable */
|
||||
// size_t r = jfg[0]->rows();
|
||||
// SharedDiagonal sigma(noiseModel::Unit::Create(r));
|
||||
//
|
||||
// for ( Index i = 0 ; i < m ; ++i ) {
|
||||
//
|
||||
// const double w = weights[i];
|
||||
//
|
||||
// if ( w != 0.0 ) { /* subgraph edge */
|
||||
// if ( !clone && w == 1.0 ) {
|
||||
// target->push_back(jfg[i]);
|
||||
// }
|
||||
// else {
|
||||
// JacobianFactor::shared_ptr jf (new JacobianFactor(*jfg[i]));
|
||||
// scaleJacobianFactor(*jf, w);
|
||||
// target->push_back(jf);
|
||||
// }
|
||||
// }
|
||||
// else { /* non-subgraph edge */
|
||||
// if ( hessian ) {
|
||||
// const JacobianFactor &f = *jfg[i];
|
||||
// /* simple case: unary factor */
|
||||
// if ( f.size() == 1 ) {
|
||||
// if (!clone) target->push_back(jfg[i]);
|
||||
// else {
|
||||
// JacobianFactor::shared_ptr jf(new JacobianFactor(*jfg[i]));
|
||||
// target->push_back(jf);
|
||||
// }
|
||||
// }
|
||||
// else { /* general factor */
|
||||
// const size_t rj = f.rows();
|
||||
// if ( rj != r ) {
|
||||
// r = rj; sigma = noiseModel::Unit::Create(r);
|
||||
// }
|
||||
// for ( JacobianFactor::const_iterator j = f.begin() ; j != f.end() ; ++j ) {
|
||||
// JacobianFactor::shared_ptr jf(new JacobianFactor(*j, f.getA(j), Vector::Zero(r,1), sigma));
|
||||
// target->push_back(jf);
|
||||
// } /* for */
|
||||
// } /* factor arity */
|
||||
// } /* hessian */
|
||||
// } /* if w != 0.0 */
|
||||
// } /* for */
|
||||
//
|
||||
// if ( !augment ) return target ;
|
||||
//
|
||||
// return appendPriorFactors(*target, keyInfo, lambda, false /* clone */);
|
||||
//}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<PreconditionerParameters> 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<Preconditioner> shared_ptr;
|
||||
typedef std::vector<size_t> Dimensions;
|
||||
|
|
@ -93,7 +91,7 @@ public:
|
|||
};
|
||||
|
||||
/*******************************************************************************************/
|
||||
struct DummyPreconditionerParameters : public PreconditionerParameters {
|
||||
struct GTSAM_EXPORT DummyPreconditionerParameters : public PreconditionerParameters {
|
||||
typedef PreconditionerParameters Base;
|
||||
typedef boost::shared_ptr<DummyPreconditionerParameters> 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<DummyPreconditioner> 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<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters);
|
||||
|
||||
///* build a factor subgraph, which is defined as a set of weighted edges (factors).
|
||||
// * "hessian"={0,1} indicates whether the non-subgraph edges are split into multiple unary factors
|
||||
// * "lambda" indicates whether scaled identity matrix is augmented
|
||||
// * "clone" indicates whether factors are cloned into the subgraph if the edge has weight = 1.0
|
||||
// * "includeUnary" indicates whether the unary factors are forced to join the subgraph:
|
||||
// * useful for static subgraph preconditioner, because unary factors were imposed dynamically by LM */
|
||||
//boost::shared_ptr<JacobianFactorGraph>
|
||||
//buildFactorSubgraph(const JacobianFactorGraph &jfg, const KeyInfo &keyInfo,
|
||||
// const Subgraph &subgraph, const int hessian, const double lambda, const bool clone, const bool includeUnary);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -15,126 +15,646 @@
|
|||
* @author: Frank Dellaert
|
||||
*/
|
||||
|
||||
#include <gtsam/base/DSFVector.h>
|
||||
#include <gtsam/linear/SubgraphPreconditioner.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
#include <boost/archive/text_oarchive.hpp>
|
||||
#include <boost/archive/text_iarchive.hpp>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <numeric>
|
||||
#include <queue>
|
||||
#include <set>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
|
||||
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
|
||||
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
|
||||
if( !jf ) {
|
||||
jf = boost::make_shared<JacobianFactor>(*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<JacobianFactor>(gf);
|
||||
if( !jf ) {
|
||||
jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
|
||||
}
|
||||
result->push_back(jf);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
static std::vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
|
||||
|
||||
/* compute the sum of the weights */
|
||||
const double sum = std::accumulate(weight.begin(), weight.end(), 0.0);
|
||||
|
||||
/* make a normalized and accumulated version of the weight vector */
|
||||
const size_t m = weight.size();
|
||||
vector<double> w; w.reserve(m);
|
||||
for ( size_t i = 0 ; i < m ; ++i ) {
|
||||
w.push_back(weight[i]/sum);
|
||||
}
|
||||
|
||||
vector<double> acc(m);
|
||||
std::partial_sum(w.begin(),w.end(),acc.begin());
|
||||
|
||||
/* iid sample n times */
|
||||
vector<size_t> result; result.reserve(n);
|
||||
const double denominator = (double)RAND_MAX;
|
||||
for ( size_t i = 0 ; i < n ; ++i ) {
|
||||
const double value = rand() / denominator;
|
||||
/* binary search the interval containing "value" */
|
||||
vector<double>::iterator it = std::lower_bound(acc.begin(), acc.end(), value);
|
||||
size_t idx = it - acc.begin();
|
||||
result.push_back(idx);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
|
||||
|
||||
const size_t m = weight.size();
|
||||
if ( n > m ) throw std::invalid_argument("uniqueSampler: invalid input size");
|
||||
|
||||
vector<size_t> result;
|
||||
|
||||
size_t count = 0;
|
||||
std::vector<bool> touched(m, false);
|
||||
while ( count < n ) {
|
||||
std::vector<size_t> localIndices; localIndices.reserve(n-count);
|
||||
std::vector<double> localWeights; localWeights.reserve(n-count);
|
||||
|
||||
/* collect data */
|
||||
for ( size_t i = 0 ; i < m ; ++i ) {
|
||||
if ( !touched[i] ) {
|
||||
localIndices.push_back(i);
|
||||
localWeights.push_back(weight[i]);
|
||||
}
|
||||
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<size_t> samples = iidSampler(localWeights, n-count);
|
||||
BOOST_FOREACH ( const size_t &id, samples ) {
|
||||
if ( touched[id] == false ) {
|
||||
touched[id] = true ;
|
||||
result.push_back(id);
|
||||
if ( ++count >= n ) break;
|
||||
}
|
||||
}
|
||||
transposeMultiplyAdd2(alpha, it, e.end(), y);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
Subgraph::Subgraph(const std::vector<size_t> &indices) {
|
||||
edges_.reserve(indices.size());
|
||||
BOOST_FOREACH ( const size_t &idx, indices ) {
|
||||
edges_.push_back(SubgraphEdge(idx, 1.0));
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
std::vector<size_t> Subgraph::edgeIndices() const {
|
||||
std::vector<size_t> eid; eid.reserve(size());
|
||||
BOOST_FOREACH ( const SubgraphEdge &edge, edges_ ) {
|
||||
eid.push_back(edge.index_);
|
||||
}
|
||||
return eid;
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
void Subgraph::save(const std::string &fn) const {
|
||||
std::ofstream os(fn.c_str());
|
||||
boost::archive::text_oarchive oa(os);
|
||||
oa << *this;
|
||||
os.close();
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
Subgraph::shared_ptr Subgraph::load(const std::string &fn) {
|
||||
std::ifstream is(fn.c_str());
|
||||
boost::archive::text_iarchive ia(is);
|
||||
Subgraph::shared_ptr subgraph(new Subgraph());
|
||||
ia >> *subgraph;
|
||||
is.close();
|
||||
return subgraph;
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) {
|
||||
if ( edge.weight() != 1.0 )
|
||||
os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")";
|
||||
else
|
||||
os << edge.index() ;
|
||||
return os;
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) {
|
||||
os << "Subgraph" << endl;
|
||||
BOOST_FOREACH ( const SubgraphEdge &e, subgraph.edges() ) {
|
||||
os << e << ", " ;
|
||||
}
|
||||
return os;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
void SubgraphBuilderParameters::print() const {
|
||||
print(cout);
|
||||
}
|
||||
|
||||
/***************************************************************************************/
|
||||
void SubgraphBuilderParameters::print(ostream &os) const {
|
||||
os << "SubgraphBuilderParameters" << endl
|
||||
<< "skeleton: " << skeletonTranslator(skeleton_) << endl
|
||||
<< "skeleton weight: " << skeletonWeightTranslator(skeletonWeight_) << endl
|
||||
<< "augmentation weight: " << augmentationWeightTranslator(augmentationWeight_) << endl
|
||||
;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
ostream& operator<<(ostream &os, const SubgraphBuilderParameters &p) {
|
||||
p.print(os);
|
||||
return os;
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslator(const std::string &src){
|
||||
std::string s = src; boost::algorithm::to_upper(s);
|
||||
if (s == "NATURALCHAIN") return NATURALCHAIN;
|
||||
else if (s == "BFS") return BFS;
|
||||
else if (s == "KRUSKAL") return KRUSKAL;
|
||||
throw invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
|
||||
return KRUSKAL;
|
||||
}
|
||||
|
||||
/****************************************************************/
|
||||
std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton w) {
|
||||
if ( w == NATURALCHAIN )return "NATURALCHAIN";
|
||||
else if ( w == BFS ) return "BFS";
|
||||
else if ( w == KRUSKAL )return "KRUSKAL";
|
||||
else return "UNKNOWN";
|
||||
}
|
||||
|
||||
/****************************************************************/
|
||||
SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) {
|
||||
std::string s = src; boost::algorithm::to_upper(s);
|
||||
if (s == "EQUAL") return EQUAL;
|
||||
else if (s == "RHS") return RHS_2NORM;
|
||||
else if (s == "LHS") return LHS_FNORM;
|
||||
else if (s == "RANDOM") return RANDOM;
|
||||
throw invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s);
|
||||
return EQUAL;
|
||||
}
|
||||
|
||||
/****************************************************************/
|
||||
std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w) {
|
||||
if ( w == EQUAL ) return "EQUAL";
|
||||
else if ( w == RHS_2NORM ) return "RHS";
|
||||
else if ( w == LHS_FNORM ) return "LHS";
|
||||
else if ( w == RANDOM ) return "RANDOM";
|
||||
else return "UNKNOWN";
|
||||
}
|
||||
|
||||
/****************************************************************/
|
||||
SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator(const std::string &src) {
|
||||
std::string s = src; boost::algorithm::to_upper(s);
|
||||
if (s == "SKELETON") return SKELETON;
|
||||
// else if (s == "STRETCH") return STRETCH;
|
||||
// else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH;
|
||||
throw invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s);
|
||||
return SKELETON;
|
||||
}
|
||||
|
||||
/****************************************************************/
|
||||
std::string SubgraphBuilderParameters::augmentationWeightTranslator(AugmentationWeight w) {
|
||||
if ( w == SKELETON ) return "SKELETON";
|
||||
// else if ( w == STRETCH ) return "STRETCH";
|
||||
// else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH";
|
||||
else return "UNKNOWN";
|
||||
}
|
||||
|
||||
/****************************************************************/
|
||||
std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
|
||||
const SubgraphBuilderParameters &p = parameters_;
|
||||
switch (p.skeleton_) {
|
||||
case SubgraphBuilderParameters::NATURALCHAIN:
|
||||
return natural_chain(gfg);
|
||||
break;
|
||||
case SubgraphBuilderParameters::BFS:
|
||||
return bfs(gfg);
|
||||
break;
|
||||
case SubgraphBuilderParameters::KRUSKAL:
|
||||
return kruskal(gfg, ordering, w);
|
||||
break;
|
||||
default:
|
||||
cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
|
||||
break;
|
||||
}
|
||||
return vector<size_t>();
|
||||
}
|
||||
|
||||
/****************************************************************/
|
||||
std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
|
||||
std::vector<size_t> result ;
|
||||
size_t idx = 0;
|
||||
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
|
||||
if ( gf->size() == 1 ) {
|
||||
result.push_back(idx);
|
||||
}
|
||||
idx++;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/****************************************************************/
|
||||
std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
|
||||
std::vector<size_t> result ;
|
||||
size_t idx = 0;
|
||||
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
|
||||
if ( gf->size() == 2 ) {
|
||||
const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
|
||||
if ( (k1-k0) == 1 || (k0-k1) == 1 )
|
||||
result.push_back(idx);
|
||||
}
|
||||
idx++;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/****************************************************************/
|
||||
std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
|
||||
const VariableIndex variableIndex(gfg);
|
||||
/* start from the first key of the first factor */
|
||||
Key seed = gfg[0]->keys()[0];
|
||||
|
||||
const size_t n = variableIndex.size();
|
||||
|
||||
/* each vertex has self as the predecessor */
|
||||
std::vector<size_t> result;
|
||||
result.reserve(n-1);
|
||||
|
||||
/* Initialize */
|
||||
std::queue<size_t> q;
|
||||
q.push(seed);
|
||||
|
||||
std::set<size_t> flags;
|
||||
flags.insert(seed);
|
||||
|
||||
/* traversal */
|
||||
while ( !q.empty() ) {
|
||||
const size_t head = q.front(); q.pop();
|
||||
BOOST_FOREACH ( const size_t id, variableIndex[head] ) {
|
||||
const GaussianFactor &gf = *gfg[id];
|
||||
BOOST_FOREACH ( const size_t key, gf.keys() ) {
|
||||
if ( flags.count(key) == 0 ) {
|
||||
q.push(key);
|
||||
flags.insert(key);
|
||||
result.push_back(id);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/****************************************************************/
|
||||
std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
|
||||
const VariableIndex variableIndex(gfg);
|
||||
const size_t n = variableIndex.size();
|
||||
const vector<size_t> idx = sort_idx(w) ;
|
||||
|
||||
/* initialize buffer */
|
||||
vector<size_t> result;
|
||||
result.reserve(n-1);
|
||||
|
||||
// container for acsendingly sorted edges
|
||||
DSFVector D(n) ;
|
||||
|
||||
size_t count = 0 ; double sum = 0.0 ;
|
||||
BOOST_FOREACH (const size_t id, idx) {
|
||||
const GaussianFactor &gf = *gfg[id];
|
||||
if ( gf.keys().size() != 2 ) continue;
|
||||
const size_t u = ordering.find(gf.keys()[0])->second,
|
||||
u_root = D.find(u),
|
||||
v = ordering.find(gf.keys()[1])->second,
|
||||
v_root = D.find(v) ;
|
||||
if ( u_root != v_root ) {
|
||||
D.merge(u_root, v_root) ;
|
||||
result.push_back(id) ;
|
||||
sum += w[id] ;
|
||||
if ( ++count == n-1 ) break ;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/****************************************************************/
|
||||
std::vector<size_t> SubgraphBuilder::sample(const std::vector<double> &weights, const size_t t) const {
|
||||
return uniqueSampler(weights, t);
|
||||
}
|
||||
|
||||
/****************************************************************/
|
||||
Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const {
|
||||
|
||||
const SubgraphBuilderParameters &p = parameters_;
|
||||
const Ordering inverse_ordering = Ordering::Natural(gfg);
|
||||
const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
|
||||
const size_t n = inverse_ordering.size(), t = n * p.complexity_ ;
|
||||
|
||||
vector<double> w = weights(gfg);
|
||||
const vector<size_t> tree = buildTree(gfg, forward_ordering, w);
|
||||
|
||||
/* sanity check */
|
||||
if ( tree.size() != n-1 ) {
|
||||
throw runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed ");
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// 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<size_t> offTree = sample(w, t);
|
||||
|
||||
vector<size_t> subgraph = unary(gfg);
|
||||
subgraph.insert(subgraph.end(), tree.begin(), tree.end());
|
||||
subgraph.insert(subgraph.end(), offTree.begin(), offTree.end());
|
||||
|
||||
return boost::make_shared<Subgraph>(subgraph);
|
||||
}
|
||||
|
||||
/****************************************************************/
|
||||
SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg) const
|
||||
{
|
||||
const size_t m = gfg.size() ;
|
||||
Weights weight; weight.reserve(m);
|
||||
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg ) {
|
||||
switch ( parameters_.skeletonWeight_ ) {
|
||||
case SubgraphBuilderParameters::EQUAL:
|
||||
weight.push_back(1.0);
|
||||
break;
|
||||
case SubgraphBuilderParameters::RHS_2NORM:
|
||||
{
|
||||
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
|
||||
weight.push_back(jf->getb().norm());
|
||||
}
|
||||
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
|
||||
weight.push_back(hf->linearTerm().norm());
|
||||
}
|
||||
}
|
||||
break;
|
||||
case SubgraphBuilderParameters::LHS_FNORM:
|
||||
{
|
||||
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
|
||||
weight.push_back(std::sqrt(jf->getA().squaredNorm()));
|
||||
}
|
||||
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
|
||||
weight.push_back(std::sqrt(hf->information().squaredNorm()));
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case SubgraphBuilderParameters::RANDOM:
|
||||
weight.push_back(std::rand()%100 + 1.0);
|
||||
break;
|
||||
|
||||
default:
|
||||
throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme ");
|
||||
break;
|
||||
}
|
||||
}
|
||||
return weight;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) :
|
||||
parameters_(p) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
|
||||
const sharedBayesNet& Rc1, const sharedValues& xbar, const SubgraphPreconditionerParameters &p) :
|
||||
Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar),
|
||||
b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))), parameters_(p) {
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// x = xbar + inv(R1)*y
|
||||
VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
|
||||
return *xbar_ + Rc1_->backSubstitute(y);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
double SubgraphPreconditioner::error(const VectorValues& y) const {
|
||||
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<GaussianConditional> & cg, *Rc1_) {
|
||||
/* collect a subvector of x that consists of the parents of cg (S) */
|
||||
const Vector xParent = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginParents(), cg->endParents()));
|
||||
const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
|
||||
|
||||
/* compute the solution for the current pivot */
|
||||
const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(rhsFrontal - cg->get_S() * xParent);
|
||||
|
||||
/* assign subvector of sol to the frontal variables */
|
||||
setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
|
||||
{
|
||||
/* copy first */
|
||||
std::copy(y.data(), y.data() + y.rows(), x.data());
|
||||
|
||||
/* in place back substitute */
|
||||
BOOST_FOREACH(const boost::shared_ptr<GaussianConditional> & cg, *Rc1_) {
|
||||
const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
|
||||
// const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal);
|
||||
const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal);
|
||||
|
||||
// Check for indeterminant solution
|
||||
if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front());
|
||||
|
||||
/* assign subvector of sol to the frontal variables */
|
||||
setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
|
||||
|
||||
/* substract from parent variables */
|
||||
for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) {
|
||||
KeyInfo::const_iterator it2 = keyInfo_.find(*it);
|
||||
Eigen::Map<Vector> rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1);
|
||||
rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
|
||||
{
|
||||
/* identify the subgraph structure */
|
||||
const SubgraphBuilder builder(parameters_.builderParams_);
|
||||
Subgraph::shared_ptr subgraph = builder(gfg);
|
||||
|
||||
keyInfo_ = keyInfo;
|
||||
|
||||
/* build factor subgraph */
|
||||
GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, *subgraph, true);
|
||||
|
||||
/* factorize and cache BayesNet */
|
||||
Rc1_ = gfg_subgraph->eliminateSequential();
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys) {
|
||||
|
||||
/* a cache of starting index and dim */
|
||||
typedef vector<pair<size_t, size_t> > Cache;
|
||||
Cache cache;
|
||||
|
||||
/* figure out dimension by traversing the keys */
|
||||
size_t d = 0;
|
||||
BOOST_FOREACH ( const Key &key, keys ) {
|
||||
const KeyInfoEntry &entry = keyInfo.find(key)->second;
|
||||
cache.push_back(make_pair<size_t,size_t>(entry.colstart(), entry.dim()));
|
||||
d += entry.dim();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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<Key> &keys, Vector &dst) {
|
||||
/* use the cache */
|
||||
size_t idx = 0;
|
||||
BOOST_FOREACH ( const Key &key, keys ) {
|
||||
const KeyInfoEntry &entry = keyInfo.find(key)->second;
|
||||
dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ;
|
||||
idx += entry.dim();
|
||||
}
|
||||
}
|
||||
|
||||
/*****************************************************************************/
|
||||
boost::shared_ptr<GaussianFactorGraph>
|
||||
buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) {
|
||||
|
||||
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
|
||||
result->reserve(subgraph.size());
|
||||
BOOST_FOREACH ( const SubgraphEdge &e, subgraph ) {
|
||||
const size_t idx = e.index();
|
||||
if ( clone ) result->push_back(gfg[idx]->clone());
|
||||
else result->push_back(gfg[idx]);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
} // nsamespace gtsam
|
||||
|
|
|
|||
|
|
@ -12,15 +12,16 @@
|
|||
/**
|
||||
* @file SubgraphPreconditioner.h
|
||||
* @date Dec 31, 2009
|
||||
* @author Frank Dellaert
|
||||
* @author Frank Dellaert, Yong-Dian Jian
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/global_includes.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/linear/Errors.h>
|
||||
|
||||
#include <gtsam/linear/IterativeSolver.h>
|
||||
#include <gtsam/linear/Preconditioner.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <boost/shared_ptr.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -30,6 +31,146 @@ namespace gtsam {
|
|||
class GaussianFactorGraph;
|
||||
class VectorValues;
|
||||
|
||||
struct SubgraphEdge {
|
||||
size_t index_; /* edge id */
|
||||
double weight_; /* edge weight */
|
||||
SubgraphEdge() : index_(0), weight_(1.0) {}
|
||||
SubgraphEdge(const SubgraphEdge &e) : index_(e.index()), weight_(e.weight()) {}
|
||||
SubgraphEdge(const size_t index, const double weight = 1.0): index_(index), weight_(weight) {}
|
||||
inline size_t index() const { return index_; }
|
||||
inline double weight() const { return weight_; }
|
||||
inline bool isUnitWeight() const { return (weight_ == 1.0); }
|
||||
friend std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge);
|
||||
private:
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(index_);
|
||||
ar & BOOST_SERIALIZATION_NVP(weight_);
|
||||
}
|
||||
};
|
||||
|
||||
/**************************************************************************/
|
||||
class Subgraph {
|
||||
public:
|
||||
typedef boost::shared_ptr<Subgraph> shared_ptr;
|
||||
typedef std::vector<shared_ptr> vector_shared_ptr;
|
||||
typedef std::vector<SubgraphEdge> Edges;
|
||||
typedef std::vector<size_t> EdgeIndices;
|
||||
typedef Edges::iterator iterator;
|
||||
typedef Edges::const_iterator const_iterator;
|
||||
|
||||
protected:
|
||||
Edges edges_; /* index to the factors */
|
||||
|
||||
public:
|
||||
Subgraph() {}
|
||||
Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {}
|
||||
Subgraph(const Edges &edges) : edges_(edges) {}
|
||||
Subgraph(const std::vector<size_t> &indices) ;
|
||||
|
||||
inline const Edges& edges() const { return edges_; }
|
||||
inline const size_t size() const { return edges_.size(); }
|
||||
EdgeIndices edgeIndices() const;
|
||||
|
||||
iterator begin() { return edges_.begin(); }
|
||||
const_iterator begin() const { return edges_.begin(); }
|
||||
iterator end() { return edges_.end(); }
|
||||
const_iterator end() const { return edges_.end(); }
|
||||
|
||||
void save(const std::string &fn) const;
|
||||
static shared_ptr load(const std::string &fn);
|
||||
friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph);
|
||||
|
||||
private:
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(edges_);
|
||||
}
|
||||
};
|
||||
|
||||
/****************************************************************************/
|
||||
struct SubgraphBuilderParameters {
|
||||
public:
|
||||
typedef boost::shared_ptr<SubgraphBuilderParameters> shared_ptr;
|
||||
|
||||
enum Skeleton {
|
||||
/* augmented tree */
|
||||
NATURALCHAIN = 0, /* natural ordering of the graph */
|
||||
BFS, /* breadth-first search tree */
|
||||
KRUSKAL, /* maximum weighted spanning tree */
|
||||
} skeleton_ ;
|
||||
|
||||
enum SkeletonWeight { /* how to weigh the graph edges */
|
||||
EQUAL = 0, /* every block edge has equal weight */
|
||||
RHS_2NORM, /* use the 2-norm of the rhs */
|
||||
LHS_FNORM, /* use the frobenius norm of the lhs */
|
||||
RANDOM, /* bounded random edge weight */
|
||||
} skeletonWeight_ ;
|
||||
|
||||
enum AugmentationWeight { /* how to weigh the graph edges */
|
||||
SKELETON = 0, /* use the same weights in building the skeleton */
|
||||
// STRETCH, /* stretch in the laplacian sense */
|
||||
// GENERALIZED_STRETCH /* the generalized stretch defined in jian2013iros */
|
||||
} augmentationWeight_ ;
|
||||
|
||||
double complexity_;
|
||||
|
||||
SubgraphBuilderParameters()
|
||||
: skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {}
|
||||
virtual ~SubgraphBuilderParameters() {}
|
||||
|
||||
/* for serialization */
|
||||
void print() const ;
|
||||
virtual void print(std::ostream &os) const ;
|
||||
friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p);
|
||||
|
||||
static Skeleton skeletonTranslator(const std::string &s);
|
||||
static std::string skeletonTranslator(Skeleton w);
|
||||
static SkeletonWeight skeletonWeightTranslator(const std::string &s);
|
||||
static std::string skeletonWeightTranslator(SkeletonWeight w);
|
||||
static AugmentationWeight augmentationWeightTranslator(const std::string &s);
|
||||
static std::string augmentationWeightTranslator(AugmentationWeight w);
|
||||
};
|
||||
|
||||
/*****************************************************************************/
|
||||
class SubgraphBuilder {
|
||||
|
||||
public:
|
||||
typedef SubgraphBuilder Base;
|
||||
typedef boost::shared_ptr<SubgraphBuilder> shared_ptr;
|
||||
typedef std::vector<double> Weights;
|
||||
|
||||
SubgraphBuilder(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
|
||||
: parameters_(p) {}
|
||||
virtual ~SubgraphBuilder() {}
|
||||
virtual boost::shared_ptr<Subgraph> operator() (const GaussianFactorGraph &jfg) const ;
|
||||
|
||||
private:
|
||||
std::vector<size_t> buildTree(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &weights) const ;
|
||||
std::vector<size_t> unary(const GaussianFactorGraph &gfg) const ;
|
||||
std::vector<size_t> natural_chain(const GaussianFactorGraph &gfg) const ;
|
||||
std::vector<size_t> bfs(const GaussianFactorGraph &gfg) const ;
|
||||
std::vector<size_t> kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const ;
|
||||
std::vector<size_t> sample(const std::vector<double> &weights, const size_t t) const ;
|
||||
Weights weights(const GaussianFactorGraph &gfg) const;
|
||||
SubgraphBuilderParameters parameters_;
|
||||
|
||||
private:
|
||||
SubgraphBuilder() {}
|
||||
};
|
||||
|
||||
/*******************************************************************************************/
|
||||
struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters {
|
||||
typedef PreconditionerParameters Base;
|
||||
typedef boost::shared_ptr<SubgraphPreconditionerParameters> shared_ptr;
|
||||
SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
|
||||
: Base(), builderParams_(p) {}
|
||||
virtual ~SubgraphPreconditionerParameters() {}
|
||||
SubgraphBuilderParameters builderParams_;
|
||||
};
|
||||
|
||||
/**
|
||||
* Subgraph conditioner class, as explained in the RSS 2010 submission.
|
||||
* Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2
|
||||
|
|
@ -37,7 +178,7 @@ namespace gtsam {
|
|||
* To use the class, give the Bayes Net R1*x=c1 and Graph A2*x=b2.
|
||||
* Then solve for yhat using CG, and solve for xhat = system.x(yhat).
|
||||
*/
|
||||
class GTSAM_EXPORT SubgraphPreconditioner {
|
||||
class GTSAM_EXPORT SubgraphPreconditioner : public Preconditioner {
|
||||
|
||||
public:
|
||||
typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
|
||||
|
|
@ -52,9 +193,12 @@ namespace gtsam {
|
|||
sharedValues xbar_; ///< A1 \ b1
|
||||
sharedErrors b2bar_; ///< A2*xbar - b2
|
||||
|
||||
KeyInfo keyInfo_;
|
||||
SubgraphPreconditionerParameters parameters_;
|
||||
|
||||
public:
|
||||
|
||||
SubgraphPreconditioner();
|
||||
SubgraphPreconditioner(const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
|
|
@ -62,7 +206,10 @@ namespace gtsam {
|
|||
* @param Rc1: the Bayes Net R1*x=c1
|
||||
* @param xbar: the solution to R1*x=c1
|
||||
*/
|
||||
SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar);
|
||||
SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar,
|
||||
const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
|
||||
|
||||
virtual ~SubgraphPreconditioner() {}
|
||||
|
||||
/** print the object */
|
||||
void print(const std::string& s = "SubgraphPreconditioner") const;
|
||||
|
|
@ -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<Key,Vector> &lambda
|
||||
) ;
|
||||
/*****************************************************************************/
|
||||
};
|
||||
|
||||
/* get subvectors */
|
||||
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys);
|
||||
|
||||
/* set subvectors */
|
||||
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst);
|
||||
|
||||
|
||||
/* build a factor subgraph, which is defined as a set of weighted edges (factors) */
|
||||
boost::shared_ptr<GaussianFactorGraph>
|
||||
buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone);
|
||||
|
||||
|
||||
/* sort the container and return permutation index with default comparator */
|
||||
template <typename Container>
|
||||
std::vector<size_t> sort_idx(const Container &src)
|
||||
{
|
||||
typedef typename Container::value_type T;
|
||||
const size_t n = src.size() ;
|
||||
std::vector<std::pair<size_t,T> > tmp;
|
||||
tmp.reserve(n);
|
||||
for ( size_t i = 0 ; i < n ; i++ )
|
||||
tmp.push_back(std::make_pair<size_t,T>(i, src[i]));
|
||||
|
||||
/* sort */
|
||||
std::stable_sort(tmp.begin(), tmp.end()) ;
|
||||
|
||||
/* copy back */
|
||||
std::vector<size_t> idx; idx.reserve(n);
|
||||
for ( size_t i = 0 ; i < n ; i++ ) {
|
||||
idx.push_back(tmp[i].first) ;
|
||||
}
|
||||
return idx;
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
#include <gtsam/linear/iterative-inl.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/SubgraphPreconditioner.h>
|
||||
#include <gtsam/linear/SubgraphSolver.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/inference/graph-inl.h>
|
||||
|
|
@ -143,4 +144,11 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
|
|||
return boost::tie(At, Ac);
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
VectorValues SubgraphSolver::optimize (
|
||||
const GaussianFactorGraph &gfg,
|
||||
const KeyInfo &keyInfo,
|
||||
const std::map<Key, Vector> &lambda,
|
||||
const VectorValues &initial
|
||||
) { return VectorValues(); }
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
|
|
@ -13,7 +13,6 @@
|
|||
|
||||
|
||||
#include <gtsam/linear/ConjugateGradientSolver.h>
|
||||
#include <gtsam/linear/SubgraphPreconditioner.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <iosfwd>
|
||||
|
|
@ -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<SubgraphPreconditioner> 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<Key, Vector> &lambda,
|
||||
const VectorValues &initial
|
||||
) { return VectorValues(); }
|
||||
) ;
|
||||
|
||||
protected:
|
||||
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <boost/date_time/posix_time/posix_time.hpp>
|
||||
|
||||
class NonlinearOptimizerMoreOptimizationTest;
|
||||
|
|
|
|||
|
|
@ -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 ...");
|
||||
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@
|
|||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/PCGSolver.h>
|
||||
#include <gtsam/linear/Preconditioner.h>
|
||||
#include <gtsam/linear/SubgraphPreconditioner.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
|
|
@ -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<PCGSolverParameters>();
|
||||
pcg->preconditioner_ = boost::make_shared<SubgraphPreconditionerParameters>();
|
||||
paramsPCG.iterativeParams = pcg;
|
||||
|
||||
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
|
||||
|
||||
Point2 x0(10,10);
|
||||
Values c0;
|
||||
c0.insert(X(1), x0);
|
||||
|
||||
Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();
|
||||
|
||||
DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue