diff --git a/inference/BayesTree-inl.h b/inference/BayesTree-inl.h index d8a4fea86..bd265f15b 100644 --- a/inference/BayesTree-inl.h +++ b/inference/BayesTree-inl.h @@ -35,7 +35,7 @@ namespace lam = boost::lambda; #include #include #include -#include +#include namespace gtsam { diff --git a/inference/BayesTree.h b/inference/BayesTree.h index 1c147c7aa..ff2a4eac3 100644 --- a/inference/BayesTree.h +++ b/inference/BayesTree.h @@ -43,6 +43,7 @@ namespace gtsam { public: + typedef boost::shared_ptr > shared_ptr; typedef boost::shared_ptr sharedConditional; typedef boost::shared_ptr > sharedBayesNet; diff --git a/inference/GenericMultifrontalSolver-inl.h b/inference/GenericMultifrontalSolver-inl.h index 597ea7598..18899a76c 100644 --- a/inference/GenericMultifrontalSolver-inl.h +++ b/inference/GenericMultifrontalSolver-inl.h @@ -19,66 +19,19 @@ namespace gtsam { /* ************************************************************************* */ template -GenericSequentialSolver::GenericSequentialSolver(const FactorGraph& factorGraph) : - structure_(factorGraph), - eliminationTree_(EliminationTree::Create(factorGraph, structure_)) { - factors_.push_back(factorGraph); +GenericMultifrontalSolver::GenericMultifrontalSolver(const FactorGraph& factorGraph) : + junctionTree_(new JunctionTree >(factorGraph)) {} + +/* ************************************************************************* */ +template +typename BayesTree::shared_ptr GenericMultifrontalSolver::eliminate() const { + return junctionTree_->eliminate(); } /* ************************************************************************* */ 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]; +typename FACTOR::shared_ptr GenericMultifrontalSolver::marginal(Index j) const { + return eliminate()->marginal(j); } } diff --git a/inference/GenericMultifrontalSolver.h b/inference/GenericMultifrontalSolver.h index f08bb1b66..a36693dfb 100644 --- a/inference/GenericMultifrontalSolver.h +++ b/inference/GenericMultifrontalSolver.h @@ -20,14 +20,8 @@ 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_; + typename JunctionTree >::shared_ptr junctionTree_; public: @@ -41,7 +35,7 @@ public: * Eliminate the factor graph sequentially. Uses a column elimination tree * to recursively eliminate. */ - typename BayesNet::shared_ptr eliminate() const; + typename BayesTree::shared_ptr eliminate() const; /** * Compute the marginal Gaussian density over a variable, by integrating out diff --git a/linear/GaussianJunctionTree.h b/linear/GaussianJunctionTree.h index f581d4070..d76ed6974 100644 --- a/linear/GaussianJunctionTree.h +++ b/linear/GaussianJunctionTree.h @@ -23,6 +23,8 @@ #include #include +#include + namespace gtsam { /* ************************************************************************* */ @@ -31,6 +33,7 @@ namespace gtsam { */ class GaussianJunctionTree: public JunctionTree { public: + typedef boost::shared_ptr shared_ptr; typedef JunctionTree Base; typedef Base::sharedClique sharedClique; diff --git a/linear/GaussianMultifrontalSolver.cpp b/linear/GaussianMultifrontalSolver.cpp new file mode 100644 index 000000000..f09d754a5 --- /dev/null +++ b/linear/GaussianMultifrontalSolver.cpp @@ -0,0 +1,35 @@ +/** + * @file GaussianMultifrontalSolver.cpp + * @brief + * @author Richard Roberts + * @created Oct 21, 2010 + */ + +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph& factorGraph) : + junctionTree_(new GaussianJunctionTree(factorGraph)) {} + +/* ************************************************************************* */ +typename BayesTree::sharedClique GaussianMultifrontalSolver::eliminate() const { + return junctionTree_->eliminate(); +} + +/* ************************************************************************* */ +VectorValues::shared_ptr GaussianMultifrontalSolver::optimize() const { + return VectorValues::shared_ptr(new VectorValues(junctionTree_->optimize())); +} + +/* ************************************************************************* */ +GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginal(Index j) const { + BayesTree bayesTree; + bayesTree.insert(junctionTree_->eliminate()); + return bayesTree.marginal(j); +} + +} diff --git a/linear/GaussianMultifrontalSolver.h b/linear/GaussianMultifrontalSolver.h new file mode 100644 index 000000000..28912a4b6 --- /dev/null +++ b/linear/GaussianMultifrontalSolver.h @@ -0,0 +1,89 @@ +/** + * @file GaussianMultifrontalSolver.h + * @brief + * @author Richard Roberts + * @created Oct 21, 2010 + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include + +namespace gtsam { + +/** 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 GaussianMultifrontalSolver { + +protected: + + GaussianJunctionTree::shared_ptr junctionTree_; + +public: + + /** + * Construct the solver for a factor graph. This builds the elimination + * tree, which already does some of the symbolic work of elimination. + */ + GaussianMultifrontalSolver(const FactorGraph& factorGraph); + + /** + * Eliminate the factor graph sequentially. Uses a column elimination tree + * to recursively eliminate. + */ + typename BayesTree::sharedClique 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; + +}; + +} + + diff --git a/linear/GaussianSequentialSolver.h b/linear/GaussianSequentialSolver.h index 4952e4def..de9ab6775 100644 --- a/linear/GaussianSequentialSolver.h +++ b/linear/GaussianSequentialSolver.h @@ -8,8 +8,6 @@ #pragma once #include -#include -#include #include #include #include @@ -19,11 +17,6 @@ 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. diff --git a/linear/Makefile.am b/linear/Makefile.am index 7cd462e62..d5009e3a9 100644 --- a/linear/Makefile.am +++ b/linear/Makefile.am @@ -22,7 +22,7 @@ check_PROGRAMS += tests/testVectorValues #check_PROGRAMS += tests/testVectorMap tests/testVectorBTree # Solvers -sources += GaussianSequentialSolver.cpp +sources += GaussianSequentialSolver.cpp GaussianMultifrontalSolver.cpp # Gaussian Factor Graphs headers += GaussianFactorSet.h Factorization.h