Multifrontal QR using new solver interface

inference/BayesTree-inl.h

@@ -35,7 +35,7 @@ namespace lam = boost::lambda;
 #include <gtsam/base/FastSet.h>
 #include <gtsam/inference/BayesTree.h>
 #include <gtsam/inference/inference-inl.h>
-#include <gtsam/inference/GenericSequentialSolver.h>
+#include <gtsam/inference/GenericSequentialSolver-inl.h>
 
 namespace gtsam {
 

inference/BayesTree.h

@@ -43,6 +43,7 @@ namespace gtsam {
 
 	public:
 
+	  typedef boost::shared_ptr<BayesTree<CONDITIONAL> > shared_ptr;
 		typedef boost::shared_ptr<CONDITIONAL> sharedConditional;
 		typedef boost::shared_ptr<BayesNet<CONDITIONAL> > sharedBayesNet;
 

inference/GenericMultifrontalSolver-inl.h

@@ -19,66 +19,19 @@ namespace gtsam {
 
 /* ************************************************************************* */
 template<class FACTOR>
-GenericSequentialSolver<FACTOR>::GenericSequentialSolver(const FactorGraph<FACTOR>& factorGraph) :
+GenericMultifrontalSolver<FACTOR>::GenericMultifrontalSolver(const FactorGraph<FACTOR>& factorGraph) :
-    structure_(factorGraph),
+    junctionTree_(new JunctionTree<FactorGraph<FACTOR> >(factorGraph)) {}
-    eliminationTree_(EliminationTree<FACTOR>::Create(factorGraph, structure_)) {
+
-  factors_.push_back(factorGraph);
+/* ************************************************************************* */
+template<class FACTOR>
+typename BayesTree<typename FACTOR::Conditional>::shared_ptr GenericMultifrontalSolver<FACTOR>::eliminate() const {
+  return junctionTree_->eliminate();
 }
 
 /* ************************************************************************* */
 template<class FACTOR>
-typename BayesNet<typename FACTOR::Conditional>::shared_ptr GenericSequentialSolver<FACTOR>::eliminate() const {
+typename FACTOR::shared_ptr GenericMultifrontalSolver<FACTOR>::marginal(Index j) const {
-  return eliminationTree_->eliminate();
+  return eliminate()->marginal(j);
-}
-
-/* ************************************************************************* */
-template<class FACTOR>
-typename FactorGraph<FACTOR>::shared_ptr GenericSequentialSolver<FACTOR>::joint(const std::vector<Index>& 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<typename FACTOR::Conditional>::shared_ptr bayesNet(
-      EliminationTree<FACTOR>::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<FACTOR>::shared_ptr joint(new FactorGraph<FACTOR>);
-  joint->reserve(js.size());
-  typename BayesNet<typename FACTOR::Conditional>::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<class FACTOR>
-typename FACTOR::shared_ptr GenericSequentialSolver<FACTOR>::marginal(Index j) const {
-  // Create a container for the one variable index
-  vector<Index> js(1); js[0] = j;
-
-  // Call joint and return the only factor in the factor graph it returns
-  return (*this->joint(js))[0];
 }
 
 }

inference/GenericMultifrontalSolver.h

@@ -20,14 +20,8 @@ class GenericMultifrontalSolver {
 
 protected:
 
-  // Store the original factors for computing marginals
-  FactorGraph<FACTOR> factors_;
-
-  // Column structure of the factor graph
-  VariableIndex<> structure_;
-
   // Elimination tree that performs elimination.
-  typename JunctionTree<FactorGraph<FACTOR> >::shared_ptr eliminationTree_;
+  typename JunctionTree<FactorGraph<FACTOR> >::shared_ptr junctionTree_;
 
 public:
 
@@ -41,7 +35,7 @@ public:
    * Eliminate the factor graph sequentially.  Uses a column elimination tree
    * to recursively eliminate.
    */
-  typename BayesNet<typename FACTOR::Conditional>::shared_ptr eliminate() const;
+  typename BayesTree<typename FACTOR::Conditional>::shared_ptr eliminate() const;
 
   /**
    * Compute the marginal Gaussian density over a variable, by integrating out

linear/GaussianJunctionTree.h

@@ -23,6 +23,8 @@
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 
+#include <boost/shared_ptr.hpp>
+
 namespace gtsam {
 
 	/* ************************************************************************* */
@@ -31,6 +33,7 @@ namespace gtsam {
 	 */
 	class GaussianJunctionTree: public JunctionTree<GaussianFactorGraph> {
 	public:
+	  typedef boost::shared_ptr<GaussianJunctionTree> shared_ptr;
 		typedef JunctionTree<GaussianFactorGraph> Base;
 		typedef Base::sharedClique sharedClique;
 

linear/GaussianMultifrontalSolver.cpp

@@ -0,0 +1,35 @@
+/**
+ * @file    GaussianMultifrontalSolver.cpp
+ * @brief   
+ * @author  Richard Roberts
+ * @created Oct 21, 2010
+ */
+
+#include <gtsam/linear/GaussianMultifrontalSolver.h>
+
+#include <gtsam/inference/GenericMultifrontalSolver-inl.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+GaussianMultifrontalSolver::GaussianMultifrontalSolver(const FactorGraph<GaussianFactor>& factorGraph) :
+    junctionTree_(new GaussianJunctionTree(factorGraph)) {}
+
+/* ************************************************************************* */
+typename BayesTree<GaussianConditional>::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<GaussianConditional> bayesTree;
+  bayesTree.insert(junctionTree_->eliminate());
+  return bayesTree.marginal(j);
+}
+
+}

linear/GaussianMultifrontalSolver.h

@@ -0,0 +1,89 @@
+/**
+ * @file    GaussianMultifrontalSolver.h
+ * @brief   
+ * @author  Richard Roberts
+ * @created Oct 21, 2010
+ */
+
+#pragma once
+
+#include <gtsam/linear/GaussianJunctionTree.h>
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/VectorValues.h>
+
+#include <utility>
+#include <vector>
+
+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<GaussianFactor>,
+ * 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<GaussianFactor>& factorGraph);
+
+  /**
+   * Eliminate the factor graph sequentially.  Uses a column elimination tree
+   * to recursively eliminate.
+   */
+  typename BayesTree<GaussianConditional>::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<Vector, Matrix> marginalStandard(Index j) const;
+
+};
+
+}
+
+

linear/GaussianSequentialSolver.h

@@ -8,8 +8,6 @@
 #pragma once
 
 #include <gtsam/inference/GenericSequentialSolver.h>
-#include <gtsam/inference/VariableIndex.h>
-#include <gtsam/inference/EliminationTree.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
@@ -19,11 +17,6 @@
 
 namespace gtsam {
 
-
-/** A GaussianEliminationTree is just a typedef of the template EliminationTree */
-typedef EliminationTree<GaussianFactor> 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.

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