From 3bb1f269161318330e343facdc1eab78115b579e Mon Sep 17 00:00:00 2001 From: Yong-Dian Jian Date: Sat, 23 Oct 2010 05:47:29 +0000 Subject: [PATCH] adapt spcg to new optimization interface --- examples/Makefile.am | 4 +- examples/Pose2SLAMwSPCG_advanced.cpp | 17 +++- linear/GaussianFactorGraph.cpp | 30 ++++++ linear/GaussianFactorGraph.h | 7 ++ linear/GaussianMultifrontalSolver.cpp | 2 +- linear/GaussianSequentialSolver.cpp | 2 +- linear/SubgraphPreconditioner.cpp | 6 +- linear/SubgraphPreconditioner.h | 3 +- linear/SubgraphSolver-inl.h | 69 +------------- linear/SubgraphSolver.h | 131 ++++++++++++++++++-------- nonlinear/NonlinearOptimization-inl.h | 39 ++++---- nonlinear/NonlinearOptimizer-inl.h | 18 ++++ nonlinear/NonlinearOptimizer.h | 7 ++ 13 files changed, 197 insertions(+), 138 deletions(-) diff --git a/examples/Makefile.am b/examples/Makefile.am index 3178a55bd..f128cc40f 100644 --- a/examples/Makefile.am +++ b/examples/Makefile.am @@ -16,8 +16,8 @@ noinst_PROGRAMS += PlanarSLAMExample_easy # Solves SLAM example from tutorial noinst_PROGRAMS += PlanarSLAMSelfContained_advanced # Solves SLAM example from tutorial with all typedefs in the script noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface -#noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver -#noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver +noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver +noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver #SUBDIRS = vSLAMexample # does not compile.... #---------------------------------------------------------------------------------------------------- # rules to build local programs diff --git a/examples/Pose2SLAMwSPCG_advanced.cpp b/examples/Pose2SLAMwSPCG_advanced.cpp index 26b6bbdb0..9ea0cf0ab 100644 --- a/examples/Pose2SLAMwSPCG_advanced.cpp +++ b/examples/Pose2SLAMwSPCG_advanced.cpp @@ -25,9 +25,16 @@ using namespace std; using namespace gtsam; using namespace pose2SLAM; -typedef boost::shared_ptr sharedGraph; -typedef boost::shared_ptr sharedValue; -typedef NonlinearOptimizer > SPCGOptimizer; +typedef boost::shared_ptr sharedGraph ; +typedef boost::shared_ptr sharedValue ; +//typedef NonlinearOptimizer > SPCGOptimizer; + + +typedef SubgraphSolver Solver; + +typedef boost::shared_ptr sharedSolver ; + +typedef NonlinearOptimizer SPCGOptimizer; sharedGraph graph; sharedValue initial; @@ -44,8 +51,8 @@ int main(void) { graph->print("full graph") ; initial->print("initial estimate") ; - SPCGOptimizer::shared_solver solver(new SPCGOptimizer::solver(*graph, *initial)) ; - SPCGOptimizer optimizer(graph, initial, solver) ; + sharedSolver solver(new Solver(*graph, *initial)) ; + SPCGOptimizer optimizer(graph, initial, solver->ordering(), solver) ; cout << "before optimization, sum of error is " << optimizer.error() << endl; NonlinearOptimizationParameters parameter; diff --git a/linear/GaussianFactorGraph.cpp b/linear/GaussianFactorGraph.cpp index b96f936a0..277339c46 100644 --- a/linear/GaussianFactorGraph.cpp +++ b/linear/GaussianFactorGraph.cpp @@ -163,4 +163,34 @@ GaussianFactorGraph GaussianFactorGraph::add_priors(double sigma, const vector &M, GaussianFactorGraph &Ab1, GaussianFactorGraph &Ab2) const { + + typedef sharedFactor F ; + + Ab1 = GaussianFactorGraph(); + Ab2 = GaussianFactorGraph(); + + BOOST_FOREACH(const F& factor, *this) { + + 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/linear/GaussianFactorGraph.h b/linear/GaussianFactorGraph.h index 2800d1c67..cfb37912e 100644 --- a/linear/GaussianFactorGraph.h +++ b/linear/GaussianFactorGraph.h @@ -152,6 +152,13 @@ namespace gtsam { */ GaussianFactorGraph add_priors(double sigma, const std::vector& dimensions) const; + + /** + * Split a Gaussian factor graph into two, according to M + * M keeps the vertex indices of edges of A1. The others belong to A2. + */ + bool split(const std::map &M, GaussianFactorGraph &A1, GaussianFactorGraph &A2) const ; + }; } // namespace gtsam diff --git a/linear/GaussianMultifrontalSolver.cpp b/linear/GaussianMultifrontalSolver.cpp index 6086c67cd..ae0ffcc29 100644 --- a/linear/GaussianMultifrontalSolver.cpp +++ b/linear/GaussianMultifrontalSolver.cpp @@ -59,7 +59,7 @@ GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginal(Index j) const { std::pair GaussianMultifrontalSolver::marginalStandard(Index j) const { GaussianConditional::shared_ptr conditional = Base::marginal(j)->eliminateFirst(); Matrix R = conditional->get_R(); - return make_pair(conditional->get_d(), inverse(trans(R)*R)); + return make_pair(conditional->get_d(), inverse(prod(trans(R),R))); } } diff --git a/linear/GaussianSequentialSolver.cpp b/linear/GaussianSequentialSolver.cpp index 2a9f8fec2..2fca9ed27 100644 --- a/linear/GaussianSequentialSolver.cpp +++ b/linear/GaussianSequentialSolver.cpp @@ -75,7 +75,7 @@ GaussianFactor::shared_ptr GaussianSequentialSolver::marginal(Index j) const { std::pair GaussianSequentialSolver::marginalStandard(Index j) const { GaussianConditional::shared_ptr conditional = Base::marginal(j)->eliminateFirst(); Matrix R = conditional->get_R(); - return make_pair(conditional->get_d(), inverse(trans(R)*R)); + return make_pair(conditional->get_d(), inverse(prod(trans(R),R))); } diff --git a/linear/SubgraphPreconditioner.cpp b/linear/SubgraphPreconditioner.cpp index 097b0f814..84a138e36 100644 --- a/linear/SubgraphPreconditioner.cpp +++ b/linear/SubgraphPreconditioner.cpp @@ -23,8 +23,8 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - SubgraphPreconditioner::SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, - sharedBayesNet& Rc1, sharedValues& xbar) : + SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, + const sharedBayesNet& Rc1, const sharedValues& xbar) : Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(Ab2_->errors_(*xbar)) { } @@ -50,8 +50,6 @@ namespace gtsam { /* ************************************************************************* */ double SubgraphPreconditioner::error(const VectorValues& y) const { - - Errors e(y); VectorValues x = this->x(y); Errors e2 = Ab2_->errors(x); diff --git a/linear/SubgraphPreconditioner.h b/linear/SubgraphPreconditioner.h index e5c9ff7ff..d47805660 100644 --- a/linear/SubgraphPreconditioner.h +++ b/linear/SubgraphPreconditioner.h @@ -46,6 +46,7 @@ namespace gtsam { public: + SubgraphPreconditioner(); /** * Constructor * @param Ab1: the Graph A1*x=b1 @@ -53,7 +54,7 @@ namespace gtsam { * @param Rc1: the Bayes Net R1*x=c1 * @param xbar: the solution to R1*x=c1 */ - SubgraphPreconditioner(sharedFG& Ab1, sharedFG& Ab2, sharedBayesNet& Rc1, sharedValues& xbar); + SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar); /** diff --git a/linear/SubgraphSolver-inl.h b/linear/SubgraphSolver-inl.h index 22b452b40..e5a1890e5 100644 --- a/linear/SubgraphSolver-inl.h +++ b/linear/SubgraphSolver-inl.h @@ -19,75 +19,8 @@ #pragma once -#include #include -#include -#include -#include -#include using namespace std; -namespace gtsam { - - /* ************************************************************************* */ - template - SubgraphSolver::SubgraphSolver(const GRAPH& G, const VALUES& theta0) { - initialize(G,theta0); - } - - /* ************************************************************************* */ - template - void SubgraphSolver::initialize(const GRAPH& G, const VALUES& theta0) { - - // generate spanning tree - PredecessorMap tree = gtsam::findMinimumSpanningTree(G); - - // split the graph - if (verbose_) cout << "generating spanning tree and split the graph ..."; - gtsam::split(G, tree, T_, C_) ; - if (verbose_) cout << ",with " << T_.size() << " and " << C_.size() << " factors" << endl; - - // make the ordering - list keys = predecessorMap2Keys(tree); - ordering_ = boost::shared_ptr(new Ordering(list(keys.begin(), keys.end()))); - - // Add a HardConstraint to the root, otherwise the root will be singular - Key root = keys.back(); - T_.addHardConstraint(root, theta0[root]); - - // compose the approximate solution - theta_bar_ = composePoses (T_, tree, theta0[root]); - } - - /* ************************************************************************* */ - template - boost::shared_ptr SubgraphSolver::linearize(const GRAPH& G, const VALUES& theta_bar) const { - SubgraphPreconditioner::sharedFG Ab1 = T_.linearize(theta_bar, *ordering_); - SubgraphPreconditioner::sharedFG Ab2 = C_.linearize(theta_bar, *ordering_); -#ifdef TIMING - SubgraphPreconditioner::sharedBayesNet Rc1; - SubgraphPreconditioner::sharedValues xbar; -#else - GaussianFactorGraph sacrificialAb1 = *Ab1; // duplicate !!!!! - SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree::Create(sacrificialAb1)->eliminate(); - SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1); -#endif - // TODO: there does not seem to be a good reason to have Ab1_ - // It seems only be used to provide an ordering for creating sparse matrices - return boost::shared_ptr(new SubgraphPreconditioner(Ab1, Ab2, Rc1, xbar)); - } - - /* ************************************************************************* */ - template - VectorValues SubgraphSolver::optimize(SubgraphPreconditioner& system) const { - VectorValues zeros = system.zero(); - - // Solve the subgraph PCG - VectorValues ybar = conjugateGradients (system, zeros, verbose_, epsilon_, epsilon_abs_, maxIterations_); - VectorValues xbar = system.x(ybar); - return xbar; - } - -} +namespace gtsam {} diff --git a/linear/SubgraphSolver.h b/linear/SubgraphSolver.h index 74bcffd3d..da763a785 100644 --- a/linear/SubgraphSolver.h +++ b/linear/SubgraphSolver.h @@ -9,17 +9,19 @@ * -------------------------------------------------------------------------- */ -/* - * SubgraphSolver.h - * Created on: Dec 31, 2009 - * @author: Frank Dellaert - */ - #pragma once +#include +#include +#include +#include + +#include +#include #include #include #include +#include #include namespace gtsam { @@ -30,13 +32,21 @@ namespace gtsam { * linearize: G * T -> L * solve : L -> VectorValues */ - template + template class SubgraphSolver { private: typedef typename VALUES::Key Key; - typedef typename GRAPH::Constraint Constraint; typedef typename GRAPH::Pose Pose; + typedef typename GRAPH::Constraint Constraint; + + 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 ; // TODO not hardcode static const size_t maxIterations_=100; @@ -44,50 +54,95 @@ namespace gtsam { static const bool verbose_=true; /* the ordering derived from the spanning tree */ - boost::shared_ptr ordering_; + shared_ordering ordering_; - /* the solution computed from the first subgraph */ - boost::shared_ptr theta_bar_; + /* the indice of two vertices in the gaussian factor graph */ + mapPairIndex pairs_; - GRAPH T_, C_; + /* preconditioner */ + shared_preconditioner pc_; public: - SubgraphSolver() {} + SubgraphSolver(){} - SubgraphSolver(const GRAPH& G, const VALUES& theta0); + SubgraphSolver(const LINEAR &GFG) { + throw std::runtime_error("SubgraphSolver: gaussian factor graph initialization not supported"); + } - void initialize(const GRAPH& G, const VALUES& theta0); + SubgraphSolver(const SubgraphSolver& solver) : + ordering_(solver.ordering_), pairs_(solver.pairs_), pc_(solver.pc_){} - boost::shared_ptr ordering() const { return ordering_; } - - boost::shared_ptr theta_bar() const { return theta_bar_; } - - /** - * linearize the non-linear graph around the current config and build the subgraph preconditioner systme - */ - boost::shared_ptr linearize(const GRAPH& G, const VALUES& theta_bar) const; + SubgraphSolver(shared_ordering ordering, + mapPairIndex pairs, + shared_preconditioner pc) : + ordering_(ordering), pairs_(pairs), pc_(pc) {} - /** - * solve for the optimal displacement in the tangent space, and then solve - * the resulted linear system - */ - VectorValues optimize(SubgraphPreconditioner& system) const; + SubgraphSolver(const GRAPH& G, const VALUES& theta0) { initialize(G,theta0); } - boost::shared_ptr prepareLinear(const SubgraphPreconditioner& fg) const { - return boost::shared_ptr(new SubgraphSolver(*this)); + shared_ptr update(const LINEAR &graph) const { + + shared_linear Ab1 = boost::make_shared(), + Ab2 = boost::make_shared(); + + if (verbose_) cout << "split the graph ..."; + graph.split(pairs_, *Ab1, *Ab2) ; + if (verbose_) cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl; + + // // Add a HardConstra int to the root, otherwise the root will be singular + // Key root = keys.back(); + // T_.addHardConstraint(root, theta0[root]); + // + // // compose the approximate solution + // theta_bar_ = composePoses (T_, tree, theta0[root]); + + LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!! + SubgraphPreconditioner::sharedBayesNet Rc1 = EliminationTree::Create(sacrificialAb1)->eliminate(); + SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1); + + shared_preconditioner pc = boost::make_shared(Ab1,Ab2,Rc1,xbar); + return boost::make_shared(ordering_, pairs_, pc) ; } + VectorValues::shared_ptr optimize() const { + + // preconditioned conjugate gradient + VectorValues zeros = pc_->zero(); + VectorValues ybar = conjugateGradients (*pc_, zeros, verbose_, epsilon_, epsilon_abs_, maxIterations_); + + boost::shared_ptr xbar = boost::make_shared() ; + *xbar = pc_->x(ybar); + return xbar; + } + + shared_ordering ordering() const { return ordering_; } + + protected: + + void initialize(const GRAPH& G, const VALUES& theta0) { + // generate spanning tree + PredecessorMap tree_ = gtsam::findMinimumSpanningTree(G); + + // 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])) ; + } + } - /** expmap the Values given the stored Ordering */ - VALUES expmap(const VALUES& config, const VectorValues& delta) const { - return config.expmap(delta, *ordering_); - } }; - template const size_t SubgraphSolver::maxIterations_; - template const bool SubgraphSolver::verbose_; - template const double SubgraphSolver::epsilon_; - template const double SubgraphSolver::epsilon_abs_; + template const size_t SubgraphSolver::maxIterations_; + template const bool SubgraphSolver::verbose_; + template const double SubgraphSolver::epsilon_; + template const double SubgraphSolver::epsilon_abs_; } // nsamespace gtsam diff --git a/nonlinear/NonlinearOptimization-inl.h b/nonlinear/NonlinearOptimization-inl.h index 677f4af61..c2e243ca5 100644 --- a/nonlinear/NonlinearOptimization-inl.h +++ b/nonlinear/NonlinearOptimization-inl.h @@ -67,24 +67,27 @@ namespace gtsam { return *result.values(); } -// /** -// * The sparse preconditioned conjucate gradient solver -// */ -// template -// T optimizeSPCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) { -// -// // initial optimization state is the same in both cases tested -// typedef NonlinearOptimizer > SPCGOptimizer; -// typename SPCGOptimizer::shared_solver solver(new SubgraphSolver(graph, initialEstimate)); -// SPCGOptimizer optimizer( -// boost::make_shared(graph), -// boost::make_shared(initialEstimate), -// solver); -// -// // Levenberg-Marquardt -// SPCGOptimizer result = optimizer.levenbergMarquardt(parameters); -// return *result.values(); -// } + /** + * The sparse preconditioned conjucate gradient solver + */ + template + T optimizeSPCG(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters()) { + + // initial optimization state is the same in both cases tested + typedef SubgraphSolver Solver; + typedef boost::shared_ptr shared_Solver; + typedef NonlinearOptimizer SPCGOptimizer; + shared_Solver solver = boost::make_shared(graph, initialEstimate); + SPCGOptimizer optimizer( + boost::make_shared(graph), + boost::make_shared(initialEstimate), + solver->ordering(), + solver); + + // Levenberg-Marquardt + SPCGOptimizer result = optimizer.levenbergMarquardt(parameters); + return *result.values(); + } /** * optimization that returns the values diff --git a/nonlinear/NonlinearOptimizer-inl.h b/nonlinear/NonlinearOptimizer-inl.h index f53b74468..123727c49 100644 --- a/nonlinear/NonlinearOptimizer-inl.h +++ b/nonlinear/NonlinearOptimizer-inl.h @@ -85,6 +85,24 @@ namespace gtsam { "NonlinearOptimizer constructor: ordering = NULL"); } + template + NonlinearOptimizer::NonlinearOptimizer( + shared_graph graph, shared_values values, shared_ordering ordering, shared_solver solver, const double lambda): + graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering), solver_(solver), + lambda_(lambda), dimensions_(new vector(values->dims(*ordering))) { + if (!graph) throw std::invalid_argument( + "NonlinearOptimizer constructor: graph = NULL"); + if (!values) throw std::invalid_argument( + "NonlinearOptimizer constructor: values = NULL"); + if (!ordering) throw std::invalid_argument( + "NonlinearOptimizer constructor: ordering = NULL"); + if (!solver) throw std::invalid_argument( + "NonlinearOptimizer constructor: solver = NULL"); + } + + + + /* ************************************************************************* */ // linearize and optimize /* ************************************************************************* */ diff --git a/nonlinear/NonlinearOptimizer.h b/nonlinear/NonlinearOptimizer.h index 58bd62c57..729e775bd 100644 --- a/nonlinear/NonlinearOptimizer.h +++ b/nonlinear/NonlinearOptimizer.h @@ -130,6 +130,13 @@ namespace gtsam { NonlinearOptimizer(shared_graph graph, shared_values values, shared_ordering ordering, const double lambda = 1e-5); + NonlinearOptimizer(shared_graph graph, + shared_values values, + shared_ordering ordering, + shared_solver solver, + const double lambda = 1e-5); + + /** * Copy constructor */