diff --git a/gtsam/linear/Makefile.am b/gtsam/linear/Makefile.am index 446332d03..9fa46b7f9 100644 --- a/gtsam/linear/Makefile.am +++ b/gtsam/linear/Makefile.am @@ -37,9 +37,9 @@ check_PROGRAMS += tests/testKalmanFilter # Iterative Methods headers += iterative-inl.h -sources += iterative.cpp SubgraphPreconditioner.cpp +sources += iterative.cpp SubgraphPreconditioner.cpp SubgraphSolver.cpp headers += IterativeSolver.h IterativeOptimizationParameters.h -headers += SubgraphSolver.h SubgraphSolver-inl.h +headers += SubgraphSolver-inl.h # Timing tests noinst_PROGRAMS = tests/timeGaussianFactor tests/timeFactorOverhead tests/timeSLAMlike diff --git a/gtsam/linear/SubgraphSolver-inl.h b/gtsam/linear/SubgraphSolver-inl.h index ca13fc42c..6bf36cc32 100644 --- a/gtsam/linear/SubgraphSolver-inl.h +++ b/gtsam/linear/SubgraphSolver-inl.h @@ -11,17 +11,9 @@ #pragma once -#include - #include -#include #include -#include -#include -#include -#include -#include #include #include @@ -29,39 +21,6 @@ using namespace std; namespace gtsam { -/* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */ -bool split(const std::map &M, - const GaussianFactorGraph &Ab, - GaussianFactorGraph &Ab1, - GaussianFactorGraph &Ab2) { - - Ab1 = GaussianFactorGraph(); - Ab2 = GaussianFactorGraph(); - - for ( size_t i = 0 ; i < Ab.size() ; ++i ) { - - boost::shared_ptr factor = Ab[i] ; - - if (factor->keys().size() > 2) - throw(invalid_argument("split: only support factors with at most two keys")); - if (factor->keys().size() == 1) { - Ab1.push_back(factor); - Ab2.push_back(factor); - continue; - } - Index key1 = factor->keys()[0]; - Index key2 = factor->keys()[1]; - - if ((M.find(key1) != M.end() && M.find(key1)->second == key2) || - (M.find(key2) != M.end() && M.find(key2)->second == key1)) - Ab1.push_back(factor); - else - Ab2.push_back(factor); - } - return true ; -} - - template void SubgraphSolver::replaceFactors(const typename LINEAR::shared_ptr &graph) { @@ -85,7 +44,7 @@ void SubgraphSolver::replaceFactors(const typename LINEAR:: SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1); pc_ = boost::make_shared( - Ab1->dynamicCastFactors >(), Ab2->dynamicCastFactors >(),Rc1,xbar); + Ab1->dynamicCastFactors >(), Ab2->dynamicCastFactors >(),Rc1,xbar); } template @@ -94,7 +53,7 @@ VectorValues::shared_ptr SubgraphSolver::optimize() const { // preconditioned conjugate gradient VectorValues zeros = pc_->zero(); VectorValues ybar = conjugateGradients - (*pc_, zeros, *parameters_); + (*pc_, zeros, *parameters_); boost::shared_ptr xbar = boost::make_shared() ; *xbar = pc_->x(ybar); @@ -103,29 +62,21 @@ VectorValues::shared_ptr SubgraphSolver::optimize() const { template void SubgraphSolver::initialize(const GRAPH& G, const VALUES& theta0) { - // generate spanning tree - PredecessorMap tree_ = gtsam::findMinimumSpanningTree(G); + // generate spanning tree + PredecessorMap tree_ = gtsam::findMinimumSpanningTree(G); - // make the ordering - list keys = predecessorMap2Keys(tree_); - ordering_ = boost::make_shared(list(keys.begin(), keys.end())); + // make the ordering + list keys = predecessorMap2Keys(tree_); + ordering_ = boost::make_shared(list(keys.begin(), keys.end())); - // build factor pairs - pairs_.clear(); - typedef pair EG ; - BOOST_FOREACH( const EG &eg, tree_ ) { - Symbol key1 = Symbol(eg.first), - key2 = Symbol(eg.second) ; - pairs_.insert(pair((*ordering_)[key1], (*ordering_)[key2])) ; - } + // build factor pairs + pairs_.clear(); + typedef pair EG ; + BOOST_FOREACH( const EG &eg, tree_ ) { + Symbol key1 = Symbol(eg.first), + key2 = Symbol(eg.second) ; + pairs_.insert(pair((*ordering_)[key1], (*ordering_)[key2])) ; } - - - - - - - - - } + +} // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp new file mode 100644 index 000000000..ce0934ffe --- /dev/null +++ b/gtsam/linear/SubgraphSolver.cpp @@ -0,0 +1,50 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +#include + +using namespace std; + +namespace gtsam { + +/* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */ +bool split(const std::map &M, + const GaussianFactorGraph &Ab, + GaussianFactorGraph &Ab1, + GaussianFactorGraph &Ab2) { + + Ab1 = GaussianFactorGraph(); + Ab2 = GaussianFactorGraph(); + + for ( size_t i = 0 ; i < Ab.size() ; ++i ) { + + boost::shared_ptr factor = Ab[i] ; + + if (factor->keys().size() > 2) + throw(invalid_argument("split: only support factors with at most two keys")); + if (factor->keys().size() == 1) { + Ab1.push_back(factor); + Ab2.push_back(factor); + continue; + } + Index key1 = factor->keys()[0]; + Index key2 = factor->keys()[1]; + + if ((M.find(key1) != M.end() && M.find(key1)->second == key2) || + (M.find(key2) != M.end() && M.find(key2)->second == key1)) + Ab1.push_back(factor); + else + Ab2.push_back(factor); + } + return true ; +} + +} // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index 40bfb47c0..c644e6f77 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -11,80 +11,88 @@ #pragma once +#include +#include #include #include namespace gtsam { - /** - * A nonlinear system solver using subgraph preconditioning conjugate gradient - * Concept NonLinearSolver implements - * linearize: G * T -> L - * solve : L -> VectorValues - */ - template - class SubgraphSolver : public IterativeSolver { +/* split the gaussian factor graph Ab into Ab1 and Ab2 according to the map */ +bool split(const std::map &M, + const GaussianFactorGraph &Ab, + GaussianFactorGraph &Ab1, + GaussianFactorGraph &Ab2); - private: - typedef typename VALUES::Key Key; - typedef typename GRAPH::Pose Pose; - typedef typename GRAPH::Constraint Constraint; +/** + * A nonlinear system solver using subgraph preconditioning conjugate gradient + * Concept NonLinearSolver implements + * linearize: G * T -> L + * solve : L -> VectorValues + */ +template +class SubgraphSolver : public IterativeSolver { - typedef boost::shared_ptr shared_ptr ; - typedef boost::shared_ptr shared_ordering ; - typedef boost::shared_ptr shared_graph ; - typedef boost::shared_ptr shared_linear ; - typedef boost::shared_ptr shared_values ; - typedef boost::shared_ptr shared_preconditioner ; - typedef std::map mapPairIndex ; +private: + typedef typename VALUES::Key Key; + typedef typename GRAPH::Pose Pose; + typedef typename GRAPH::Constraint Constraint; - /* the ordering derived from the spanning tree */ - shared_ordering ordering_; + typedef boost::shared_ptr shared_ptr ; + typedef boost::shared_ptr shared_ordering ; + typedef boost::shared_ptr shared_graph ; + typedef boost::shared_ptr shared_linear ; + typedef boost::shared_ptr shared_values ; + typedef boost::shared_ptr shared_preconditioner ; + typedef std::map mapPairIndex ; - /* the indice of two vertices in the gaussian factor graph */ - mapPairIndex pairs_; + /* the ordering derived from the spanning tree */ + shared_ordering ordering_; - /* preconditioner */ - shared_preconditioner pc_; + /* the indice of two vertices in the gaussian factor graph */ + mapPairIndex pairs_; - /* flag for direct solver - either QR or LDL */ - bool useQR_; + /* preconditioner */ + shared_preconditioner pc_; - public: + /* flag for direct solver - either QR or LDL */ + bool useQR_; - SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters ¶meters = Parameters(), bool useQR = false): - IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); } +public: - SubgraphSolver(const LINEAR& GFG) { - std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl; - throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported"); - } + SubgraphSolver(const GRAPH& G, const VALUES& theta0, const Parameters ¶meters = Parameters(), bool useQR = false): + IterativeSolver(parameters), useQR_(useQR) { initialize(G,theta0); } - SubgraphSolver(const shared_linear& GFG, const boost::shared_ptr& structure, bool useQR = false) { - std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl; - throw std::runtime_error("SubgraphSolver: gaussian factor graph and variable index initialization not supported"); - } + SubgraphSolver(const LINEAR& GFG) { + std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl; + throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported"); + } - SubgraphSolver(const SubgraphSolver& solver) : - IterativeSolver(solver), ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_), useQR_(solver.useQR_) {} + SubgraphSolver(const shared_linear& GFG, const boost::shared_ptr& structure, bool useQR = false) { + std::cout << "[SubgraphSolver] Unexpected usage.." << std::endl; + throw std::runtime_error("SubgraphSolver: gaussian factor graph and variable index initialization not supported"); + } - SubgraphSolver(shared_ordering ordering, - mapPairIndex pairs, - shared_preconditioner pc, - sharedParameters parameters = boost::make_shared(), - bool useQR = true) : - IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc), useQR_(useQR) {} + SubgraphSolver(const SubgraphSolver& solver) : + IterativeSolver(solver), ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_), useQR_(solver.useQR_) {} - void replaceFactors(const typename LINEAR::shared_ptr &graph); - VectorValues::shared_ptr optimize() const ; - shared_ordering ordering() const { return ordering_; } + SubgraphSolver(shared_ordering ordering, + mapPairIndex pairs, + shared_preconditioner pc, + sharedParameters parameters = boost::make_shared(), + bool useQR = true) : + IterativeSolver(parameters), ordering_(ordering), pairs_(pairs), pc_(pc), useQR_(useQR) {} - protected: - void initialize(const GRAPH& G, const VALUES& theta0); + void replaceFactors(const typename LINEAR::shared_ptr &graph); + VectorValues::shared_ptr optimize() const ; + shared_ordering ordering() const { return ordering_; } - private: - SubgraphSolver():IterativeSolver(){} - }; +protected: + void initialize(const GRAPH& G, const VALUES& theta0); + +private: + SubgraphSolver():IterativeSolver(){} +}; } // nsamespace gtsam diff --git a/gtsam/nonlinear/NonlinearOptimization-inl.h b/gtsam/nonlinear/NonlinearOptimization-inl.h index f04cdc04d..178ef646d 100644 --- a/gtsam/nonlinear/NonlinearOptimization-inl.h +++ b/gtsam/nonlinear/NonlinearOptimization-inl.h @@ -20,9 +20,10 @@ #include #include +#include + #include #include -#include using namespace std; diff --git a/gtsam/nonlinear/NonlinearOptimizationParameters.h b/gtsam/nonlinear/NonlinearOptimizationParameters.h index 3edd92435..0de56bd86 100644 --- a/gtsam/nonlinear/NonlinearOptimizationParameters.h +++ b/gtsam/nonlinear/NonlinearOptimizationParameters.h @@ -20,6 +20,7 @@ #include #include +#include namespace gtsam {