From 2650939bb743b6751a5ca3ccbd15e1d0617e6c9a Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 21 Oct 2010 23:04:42 +0000 Subject: [PATCH] New solver interface with all the files this time :-) --- inference/GenericMultifrontalSolver-inl.h | 86 ++++++++++++++++ inference/GenericMultifrontalSolver.h | 56 +++++++++++ inference/GenericSequentialSolver-inl.h | 84 ++++++++++++++++ inference/GenericSequentialSolver.h | 62 ++++++++++++ inference/SymbolicSequentialSolver.cpp | 28 ++++++ inference/SymbolicSequentialSolver.h | 48 +++++++++ linear/GaussianSequentialSolver.cpp | 57 +++++++++++ linear/GaussianSequentialSolver.h | 113 ++++++++++++++++++++++ 8 files changed, 534 insertions(+) create mode 100644 inference/GenericMultifrontalSolver-inl.h create mode 100644 inference/GenericMultifrontalSolver.h create mode 100644 inference/GenericSequentialSolver-inl.h create mode 100644 inference/GenericSequentialSolver.h create mode 100644 inference/SymbolicSequentialSolver.cpp create mode 100644 inference/SymbolicSequentialSolver.h create mode 100644 linear/GaussianSequentialSolver.cpp create mode 100644 linear/GaussianSequentialSolver.h diff --git a/inference/GenericMultifrontalSolver-inl.h b/inference/GenericMultifrontalSolver-inl.h new file mode 100644 index 000000000..597ea7598 --- /dev/null +++ b/inference/GenericMultifrontalSolver-inl.h @@ -0,0 +1,86 @@ +/** + * @file GenericMultifrontalSolver-inl.h + * @brief + * @author Richard Roberts + * @created Oct 21, 2010 + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +template +GenericSequentialSolver::GenericSequentialSolver(const FactorGraph& factorGraph) : + structure_(factorGraph), + eliminationTree_(EliminationTree::Create(factorGraph, structure_)) { + factors_.push_back(factorGraph); +} + +/* ************************************************************************* */ +template +typename BayesNet::shared_ptr GenericSequentialSolver::eliminate() const { + return eliminationTree_->eliminate(); +} + +/* ************************************************************************* */ +template +typename FactorGraph::shared_ptr GenericSequentialSolver::joint(const std::vector& js) const { + + // Compute a COLAMD permutation with the marginal variable constrained to the end. + Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(structure_, js)); + Permutation::shared_ptr permutationInverse(permutation->inverse()); + + // Permute the factors - NOTE that this permutes the original factors, not + // copies. Other parts of the code may hold shared_ptr's to these factors so + // we must undo the permutation before returning. + BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, factors_) { + if(factor) + factor->permuteWithInverse(*permutationInverse); + } + + // Eliminate all variables + typename BayesNet::shared_ptr bayesNet( + EliminationTree::Create(factors_)->eliminate()); + + // Undo the permuation on the original factors and on the structure. + BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, factors_) { + if(factor) + factor->permuteWithInverse(*permutation); + } + + // Take the joint marginal from the Bayes net. + typename FactorGraph::shared_ptr joint(new FactorGraph); + joint->reserve(js.size()); + typename BayesNet::const_reverse_iterator conditional = bayesNet->rbegin(); + for(size_t i = 0; i < js.size(); ++i) { + joint->push_back(typename FACTOR::shared_ptr(new FACTOR(**(conditional++)))); } + + // Undo the permutation on the eliminated joint marginal factors + BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *joint) { + factor->permuteWithInverse(*permutation); } + + return joint; +} + +/* ************************************************************************* */ +template +typename FACTOR::shared_ptr GenericSequentialSolver::marginal(Index j) const { + // Create a container for the one variable index + vector js(1); js[0] = j; + + // Call joint and return the only factor in the factor graph it returns + return (*this->joint(js))[0]; +} + +} + + diff --git a/inference/GenericMultifrontalSolver.h b/inference/GenericMultifrontalSolver.h new file mode 100644 index 000000000..f08bb1b66 --- /dev/null +++ b/inference/GenericMultifrontalSolver.h @@ -0,0 +1,56 @@ +/** + * @file GenericMultifrontalSolver.h + * @brief + * @author Richard Roberts + * @created Oct 21, 2010 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +template +class GenericMultifrontalSolver { + +protected: + + // Store the original factors for computing marginals + FactorGraph factors_; + + // Column structure of the factor graph + VariableIndex<> structure_; + + // Elimination tree that performs elimination. + typename JunctionTree >::shared_ptr eliminationTree_; + +public: + + /** + * Construct the solver for a factor graph. This builds the elimination + * tree, which already does some of the symbolic work of elimination. + */ + GenericMultifrontalSolver(const FactorGraph& factorGraph); + + /** + * Eliminate the factor graph sequentially. Uses a column elimination tree + * to recursively eliminate. + */ + typename BayesNet::shared_ptr eliminate() const; + + /** + * Compute the marginal Gaussian density over a variable, by integrating out + * all of the other variables. This function returns the result as a factor. + */ + typename FACTOR::shared_ptr marginal(Index j) const; + +}; + +} + + diff --git a/inference/GenericSequentialSolver-inl.h b/inference/GenericSequentialSolver-inl.h new file mode 100644 index 000000000..0c7c2437a --- /dev/null +++ b/inference/GenericSequentialSolver-inl.h @@ -0,0 +1,84 @@ +/** + * @file GenericSequentialSolver.cpp + * @brief + * @author Richard Roberts + * @created Oct 21, 2010 + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +template +GenericSequentialSolver::GenericSequentialSolver(const FactorGraph& factorGraph) : + structure_(factorGraph), + eliminationTree_(EliminationTree::Create(factorGraph, structure_)) { + factors_.push_back(factorGraph); +} + +/* ************************************************************************* */ +template +typename BayesNet::shared_ptr GenericSequentialSolver::eliminate() const { + return eliminationTree_->eliminate(); +} + +/* ************************************************************************* */ +template +typename FactorGraph::shared_ptr GenericSequentialSolver::joint(const std::vector& js) const { + + // Compute a COLAMD permutation with the marginal variable constrained to the end. + Permutation::shared_ptr permutation(Inference::PermutationCOLAMD(structure_, js)); + Permutation::shared_ptr permutationInverse(permutation->inverse()); + + // Permute the factors - NOTE that this permutes the original factors, not + // copies. Other parts of the code may hold shared_ptr's to these factors so + // we must undo the permutation before returning. + BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, factors_) { + if(factor) + factor->permuteWithInverse(*permutationInverse); + } + + // Eliminate all variables + typename BayesNet::shared_ptr bayesNet( + EliminationTree::Create(factors_)->eliminate()); + + // Undo the permuation on the original factors and on the structure. + BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, factors_) { + if(factor) + factor->permuteWithInverse(*permutation); + } + + // Take the joint marginal from the Bayes net. + typename FactorGraph::shared_ptr joint(new FactorGraph); + joint->reserve(js.size()); + typename BayesNet::const_reverse_iterator conditional = bayesNet->rbegin(); + for(size_t i = 0; i < js.size(); ++i) { + joint->push_back(typename FACTOR::shared_ptr(new FACTOR(**(conditional++)))); } + + // Undo the permutation on the eliminated joint marginal factors + BOOST_FOREACH(const typename FACTOR::shared_ptr& factor, *joint) { + factor->permuteWithInverse(*permutation); } + + return joint; +} + +/* ************************************************************************* */ +template +typename FACTOR::shared_ptr GenericSequentialSolver::marginal(Index j) const { + // Create a container for the one variable index + vector js(1); js[0] = j; + + // Call joint and return the only factor in the factor graph it returns + return (*this->joint(js))[0]; +} + +} diff --git a/inference/GenericSequentialSolver.h b/inference/GenericSequentialSolver.h new file mode 100644 index 000000000..05227cc60 --- /dev/null +++ b/inference/GenericSequentialSolver.h @@ -0,0 +1,62 @@ +/** + * @file GenericSequentialSolver.h + * @brief + * @author Richard Roberts + * @created Oct 21, 2010 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +template +class GenericSequentialSolver { + +protected: + + // Store the original factors for computing marginals + FactorGraph factors_; + + // Column structure of the factor graph + VariableIndex<> structure_; + + // Elimination tree that performs elimination. + typename EliminationTree::shared_ptr eliminationTree_; + +public: + + /** + * Construct the solver for a factor graph. This builds the elimination + * tree, which already does some of the symbolic work of elimination. + */ + GenericSequentialSolver(const FactorGraph& factorGraph); + + /** + * Eliminate the factor graph sequentially. Uses a column elimination tree + * to recursively eliminate. + */ + typename BayesNet::shared_ptr eliminate() const; + + /** + * Compute the marginal Gaussian density over a variable, by integrating out + * all of the other variables. This function returns the result as a factor. + */ + typename FACTOR::shared_ptr marginal(Index j) const; + + /** + * Compute the marginal joint over a set of variables, by integrating out + * all of the other variables. This function returns the result as a factor + * graph. + */ + typename FactorGraph::shared_ptr joint(const std::vector& js) const; + +}; + +} + diff --git a/inference/SymbolicSequentialSolver.cpp b/inference/SymbolicSequentialSolver.cpp new file mode 100644 index 000000000..08dd0c2dd --- /dev/null +++ b/inference/SymbolicSequentialSolver.cpp @@ -0,0 +1,28 @@ +/** + * @file SymbolicSequentialSolver.cpp + * @brief + * @author Richard Roberts + * @created Oct 21, 2010 + */ + +#include +#include + +namespace gtsam { + + +/* ************************************************************************* */ +SymbolicSequentialSolver::SymbolicSequentialSolver(const FactorGraph& factorGraph) : + Base(factorGraph) {} + +/* ************************************************************************* */ +typename BayesNet::shared_ptr SymbolicSequentialSolver::eliminate() const { + return Base::eliminate(); +} + +/* ************************************************************************* */ +SymbolicFactorGraph::shared_ptr SymbolicSequentialSolver::joint(const std::vector& js) const { + return SymbolicFactorGraph::shared_ptr(new SymbolicFactorGraph(*Base::joint(js))); +} + +} diff --git a/inference/SymbolicSequentialSolver.h b/inference/SymbolicSequentialSolver.h new file mode 100644 index 000000000..74a89a6e7 --- /dev/null +++ b/inference/SymbolicSequentialSolver.h @@ -0,0 +1,48 @@ +/** + * @file SymbolicSequentialSolver.h + * @brief + * @author Richard Roberts + * @created Oct 21, 2010 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +class SymbolicSequentialSolver : GenericSequentialSolver { + +protected: + + typedef GenericSequentialSolver Base; + +public: + + SymbolicSequentialSolver(const FactorGraph& factorGraph); + + /** + * Eliminate the factor graph sequentially. Uses a column elimination tree + * to recursively eliminate. + */ + typename BayesNet::shared_ptr eliminate() const; + + /** + * Compute the marginal Gaussian density over a variable, by integrating out + * all of the other variables. This function returns the result as a factor. + */ + IndexFactor::shared_ptr marginal(Index j) const; + + /** + * Compute the marginal joint over a set of variables, by integrating out + * all of the other variables. This function returns the result as an upper- + * triangular R factor and right-hand-side, i.e. a GaussianBayesNet with + * R*x = d. To get a mean and covariance matrix, use jointStandard(...) + */ + SymbolicFactorGraph::shared_ptr joint(const std::vector& js) const; + +}; + +} + diff --git a/linear/GaussianSequentialSolver.cpp b/linear/GaussianSequentialSolver.cpp new file mode 100644 index 000000000..5f6b308f4 --- /dev/null +++ b/linear/GaussianSequentialSolver.cpp @@ -0,0 +1,57 @@ +/** + * @file SequentialSolver.cpp + * @brief + * @author Richard Roberts + * @created Oct 19, 2010 + */ + +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +GaussianSequentialSolver::GaussianSequentialSolver(const FactorGraph& factorGraph) : + Base(factorGraph) {} + +/* ************************************************************************* */ +GaussianBayesNet::shared_ptr GaussianSequentialSolver::eliminate() const { + return Base::eliminate(); +} + +/* ************************************************************************* */ +VectorValues::shared_ptr GaussianSequentialSolver::optimize() const { + + static const bool debug = false; + + if(debug) this->factors_.print("GaussianSequentialSolver, eliminating "); + if(debug) this->eliminationTree_->print("GaussianSequentialSolver, elimination tree "); + + // Eliminate using the elimination tree + GaussianBayesNet::shared_ptr bayesNet(this->eliminate()); + + if(debug) bayesNet->print("GaussianSequentialSolver, Bayes net "); + + // Allocate the solution vector if it is not already allocated +// VectorValues::shared_ptr solution = allocateVectorValues(*bayesNet); + + // Back-substitute + VectorValues::shared_ptr solution(gtsam::optimize_(*bayesNet)); + + if(debug) solution->print("GaussianSequentialSolver, solution "); + + return solution; +} + +/* ************************************************************************* */ +GaussianFactor::shared_ptr GaussianSequentialSolver::marginal(Index j) const { + return Base::marginal(j); +} + +/* ************************************************************************* */ +GaussianFactorGraph::shared_ptr GaussianSequentialSolver::joint(const std::vector& js) const { + return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::joint(js))); +} + +} diff --git a/linear/GaussianSequentialSolver.h b/linear/GaussianSequentialSolver.h new file mode 100644 index 000000000..4952e4def --- /dev/null +++ b/linear/GaussianSequentialSolver.h @@ -0,0 +1,113 @@ +/** + * @file SequentialSolver.h + * @brief Solves a GaussianFactorGraph (i.e. a sparse linear system) using sequential variable elimination. + * @author Richard Roberts + * @created Oct 19, 2010 + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace gtsam { + + +/** A GaussianEliminationTree is just a typedef of the template EliminationTree */ +typedef EliminationTree GaussianEliminationTree; + + +/** This solver uses sequential variable elimination to solve a + * GaussianFactorGraph, i.e. a sparse linear system. Underlying this is a + * column elimination tree (inference/EliminationTree), see Gilbert 2001 BIT. + * + * The elimination ordering is "baked in" to the variable indices at this + * stage, i.e. elimination proceeds in order from '0'. A fill-reducing + * ordering is computed symbolically from the NonlinearFactorGraph, on the + * nonlinear side of gtsam. (To be precise, it is possible to permute an + * existing GaussianFactorGraph into a COLAMD ordering instead, this is done + * when computing marginals). + * + * This is not the most efficient algorithm we provide, most efficient is the + * MultifrontalSolver, which performs Multi-frontal QR factorization. However, + * sequential variable elimination is easier to understand so this is a good + * starting point to learn about these algorithms and our implementation. + * Additionally, the first step of MFQR is symbolic sequential elimination. + * + * The EliminationTree recursively produces a BayesNet, + * typedef'ed in linear/GaussianBayesNet, on which this class calls + * optimize(...) to perform back-substitution. + */ +class GaussianSequentialSolver : GenericSequentialSolver { + +protected: + + typedef GenericSequentialSolver Base; + +public: + + /** + * Construct the solver for a factor graph. This builds the elimination + * tree, which already does some of the symbolic work of elimination. + */ + GaussianSequentialSolver(const FactorGraph& factorGraph); + + /** + * Eliminate the factor graph sequentially. Uses a column elimination tree + * to recursively eliminate. + */ + GaussianBayesNet::shared_ptr eliminate() const; + + /** + * Compute the least-squares solution of the GaussianFactorGraph. This + * eliminates to create a BayesNet and then back-substitutes this BayesNet to + * obtain the solution. + */ + VectorValues::shared_ptr optimize() const; + + /** + * Compute the marginal Gaussian density over a variable, by integrating out + * all of the other variables. This function returns the result as an upper- + * triangular R factor and right-hand-side, i.e. a GaussianConditional with + * R*x = d. To get a mean and covariance matrix, use marginalStandard(...) + */ + GaussianFactor::shared_ptr marginal(Index j) const; + + /** + * Compute the marginal Gaussian density over a variable, by integrating out + * all of the other variables. This function returns the result as a mean + * vector and covariance matrix. Compared to marginalCanonical, which + * returns a GaussianConditional, this function back-substitutes the R factor + * to obtain the mean, then computes \Sigma = (R^T * R)^-1. + */ +// std::pair marginalStandard(Index j) const; + + /** + * Compute the marginal joint over a set of variables, by integrating out + * all of the other variables. This function returns the result as an upper- + * triangular R factor and right-hand-side, i.e. a GaussianBayesNet with + * R*x = d. To get a mean and covariance matrix, use jointStandard(...) + */ + GaussianFactorGraph::shared_ptr joint(const std::vector& js) const; + + /** + * Compute the marginal joint over a set of variables, by integrating out + * all of the other variables. This function returns the result as a mean + * vector and covariance matrix. The variables will be ordered in the + * return values as they are ordered in the 'js' argument, not as they are + * ordered in the original factor graph. Compared to jointCanonical, which + * returns a GaussianBayesNet, this function back-substitutes the BayesNet to + * obtain the mean, then computes \Sigma = (R^T * R)^-1. + */ +// std::pair jointStandard(const std::vector& js) const; +}; + +} +