conditionalBayesNet and an internal eliminate - developed for making shortcuts

gtsam/inference/GenericSequentialSolver-inl.h

@@ -32,9 +32,9 @@ namespace gtsam {
   template<class FACTOR>
   GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
       const FactorGraph<FACTOR>& factorGraph) :
-			factors_(new FactorGraph<FACTOR>(factorGraph)),
+      factors_(new FactorGraph<FACTOR>(factorGraph)), structure_(
-			structure_(new VariableIndex(factorGraph)),
+          new VariableIndex(factorGraph)), eliminationTree_(
-			eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) {
+          EliminationTree<FACTOR>::Create(*factors_, *structure_)) {
   }
 
   /* ************************************************************************* */
@@ -42,8 +42,8 @@ namespace gtsam {
   GenericSequentialSolver<FACTOR>::GenericSequentialSolver(
       const sharedFactorGraph& factorGraph,
       const boost::shared_ptr<VariableIndex>& variableIndex) :
-			factors_(factorGraph), structure_(variableIndex),
+      factors_(factorGraph), structure_(variableIndex), eliminationTree_(
-			eliminationTree_(EliminationTree<FACTOR>::Create(*factors_, *structure_)) {
+          EliminationTree<FACTOR>::Create(*factors_, *structure_)) {
   }
 
   /* ************************************************************************* */
@@ -58,9 +58,12 @@ namespace gtsam {
   template<class FACTOR>
   bool GenericSequentialSolver<FACTOR>::equals(
       const GenericSequentialSolver& expected, double tol) const {
-		if (!this->factors_->equals(*expected.factors_, tol)) return false;
+    if (!this->factors_->equals(*expected.factors_, tol))
-		if (!this->structure_->equals(*expected.structure_, tol)) return false;
+      return false;
-		if (!this->eliminationTree_->equals(*expected.eliminationTree_, tol)) return false;
+    if (!this->structure_->equals(*expected.structure_, tol))
+      return false;
+    if (!this->eliminationTree_->equals(*expected.eliminationTree_, tol))
+      return false;
     return true;
   }
 
@@ -77,43 +80,90 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class FACTOR>
-	typename boost::shared_ptr<BayesNet<typename FACTOR::ConditionalType> > //
+  typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
   GenericSequentialSolver<FACTOR>::eliminate(Eliminate function) const {
     return eliminationTree_->eliminate(function);
   }
 
   /* ************************************************************************* */
   template<class FACTOR>
-  typename BayesNet<typename FACTOR::ConditionalType>::shared_ptr //
+  typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
-  GenericSequentialSolver<FACTOR>::jointBayesNet(
+  GenericSequentialSolver<FACTOR>::eliminate(const Permutation& permutation,
-      const std::vector<Index>& js, Eliminate function) const {
+      Eliminate function, boost::optional<size_t> nrToEliminate) const {
 
-    // Compute a COLAMD permutation with the marginal variables constrained to the end.
+    // Create inverse permutation
-    Permutation::shared_ptr permutation(inference::PermutationCOLAMD(*structure_, js));
+    Permutation::shared_ptr permutationInverse(permutation.inverse());
-    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 boost::shared_ptr<FACTOR>& factor, *factors_)
-      if (factor) factor->permuteWithInverse(*permutationInverse);
+      if (factor)
+        factor->permuteWithInverse(*permutationInverse);
 
-    // Eliminate all variables
+    // Eliminate using elimination tree provided
-    typename BayesNet<Conditional>::shared_ptr
+    typename EliminationTree<FACTOR>::shared_ptr etree;
-      bayesNet(EliminationTree<FACTOR>::Create(*factors_)->eliminate(function));
+    if (nrToEliminate) {
+      VariableIndex structure(*factors_, *nrToEliminate);
+      etree = EliminationTree<FACTOR>::Create(*factors_, structure);
+    } else
+      etree = EliminationTree<FACTOR>::Create(*factors_);
+    sharedBayesNet bayesNet = etree->eliminate(function);
 
     // Undo the permutation on the original factors and on the structure.
     BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, *factors_)
-      if (factor) factor->permuteWithInverse(*permutation);
+      if (factor)
-
+        factor->permuteWithInverse(permutation);
-    // Get rid of conditionals on variables that we want to marginalize out
-    size_t nrMarginalizedOut = bayesNet->size()-js.size();
-    for(int i=0;i<nrMarginalizedOut;i++)
-      bayesNet->pop_front();
 
     // Undo the permutation on the conditionals
     BOOST_FOREACH(const boost::shared_ptr<Conditional>& c, *bayesNet)
-      c->permuteWithInverse(*permutation);
+      c->permuteWithInverse(permutation);
+
+    return bayesNet;
+  }
+
+  /* ************************************************************************* */
+  template<class FACTOR>
+  typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
+  GenericSequentialSolver<FACTOR>::conditionalBayesNet(
+      const std::vector<Index>& js, size_t nrFrontals,
+      Eliminate function) const {
+
+    // Compute a COLAMD permutation with the marginal variables constrained to the end.
+    // TODO in case of nrFrontals, the order of js has to be respected here !
+    Permutation::shared_ptr permutation(
+        inference::PermutationCOLAMD(*structure_, js));
+
+    // Eliminate only variables J \cup F from P(J,F,S) to get P(F|S)
+    size_t nrVariables = factors_->keys().size(); // TODO expensive!
+    size_t nrMarginalized = nrVariables - js.size();
+    size_t nrToEliminate = nrMarginalized + nrFrontals;
+    sharedBayesNet bayesNet = eliminate(*permutation, function, nrToEliminate);
+
+    // Get rid of conditionals on variables that we want to marginalize out
+    for (int i = 0; i < nrMarginalized; i++)
+      bayesNet->pop_front();
+
+    return bayesNet;
+  }
+
+  /* ************************************************************************* */
+  template<class FACTOR>
+  typename GenericSequentialSolver<FACTOR>::sharedBayesNet //
+  GenericSequentialSolver<FACTOR>::jointBayesNet(const std::vector<Index>& js,
+      Eliminate function) const {
+
+    // Compute a COLAMD permutation with the marginal variables constrained to the end.
+    Permutation::shared_ptr permutation(
+        inference::PermutationCOLAMD(*structure_, js));
+
+    // Eliminate all variables
+    sharedBayesNet bayesNet = eliminate(*permutation, function);
+
+    // Get rid of conditionals on variables that we want to marginalize out
+    size_t nrMarginalizedOut = bayesNet->size() - js.size();
+    for (int i = 0; i < nrMarginalizedOut; i++)
+      bayesNet->pop_front();
 
     return bayesNet;
   }
@@ -125,8 +175,8 @@ namespace gtsam {
       const std::vector<Index>& js, Eliminate function) const {
 
     // Eliminate all variables
-    typename BayesNet<Conditional>::shared_ptr
+    typename BayesNet<Conditional>::shared_ptr bayesNet = jointBayesNet(js,
-      bayesNet = jointBayesNet(js,function);
+        function);
 
     return boost::make_shared<FactorGraph<FACTOR> >(*bayesNet);
   }
@@ -134,7 +184,8 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class FACTOR>
   typename boost::shared_ptr<FACTOR> //
-	GenericSequentialSolver<FACTOR>::marginalFactor(Index j, Eliminate function) const {
+  GenericSequentialSolver<FACTOR>::marginalFactor(Index j,
+      Eliminate function) const {
     // Create a container for the one variable index
     std::vector<Index> js(1);
     js[0] = j;

gtsam/inference/GenericSequentialSolver.h

@@ -18,16 +18,28 @@
 
 #pragma once
 
-#include <utility>
-#include <boost/function.hpp>
-#include <vector>
 #include <gtsam/base/types.h>
 #include <gtsam/base/Testable.h>
 
-namespace gtsam { class VariableIndex; }
+#include <boost/function.hpp>
-namespace gtsam { template<class FACTOR> class EliminationTree; }
+#include <boost/optional.hpp>
-namespace gtsam { template<class FACTOR> class FactorGraph; }
+
-namespace gtsam { template<class CONDITIONAL> class BayesNet; }
+#include <utility>
+#include <vector>
+
+namespace gtsam {
+  class VariableIndex;
+  class Permutation;
+}
+namespace gtsam {
+  template<class FACTOR> class EliminationTree;
+}
+namespace gtsam {
+  template<class FACTOR> class FactorGraph;
+}
+namespace gtsam {
+  template<class CONDITIONAL> class BayesNet;
+}
 
 namespace gtsam {
 
@@ -52,8 +64,10 @@ namespace gtsam {
 
     typedef boost::shared_ptr<FactorGraph<FACTOR> > sharedFactorGraph;
     typedef typename FACTOR::ConditionalType Conditional;
+    typedef typename boost::shared_ptr<BayesNet<Conditional> > sharedBayesNet;
     typedef std::pair<boost::shared_ptr<Conditional>, boost::shared_ptr<FACTOR> > EliminationResult;
-		typedef boost::function<EliminationResult(const FactorGraph<FACTOR>&, size_t)> Eliminate;
+    typedef boost::function<
+        EliminationResult(const FactorGraph<FACTOR>&, size_t)> Eliminate;
 
     /** Store the original factors for computing marginals
      * TODO Frank says: really? Marginals should be computed from result.
@@ -68,7 +82,15 @@ namespace gtsam {
 
     /** concept checks */
     GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
-//		GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree)
+    // GTSAM_CONCEPT_TESTABLE_TYPE(EliminationTree)
+
+    /**
+     * Eliminate in a different order, given a permutation
+     * If given a number of variables to eliminate, will only eliminate that many
+     */
+    sharedBayesNet
+    eliminate(const Permutation& permutation, Eliminate function,
+        boost::optional<size_t> nrToEliminate = boost::none) const;
 
   public:
 
@@ -86,8 +108,7 @@ namespace gtsam {
      * VariableIndex.  The solver will store these pointers, so this constructor
      * is the fastest.
      */
-		GenericSequentialSolver(
+    GenericSequentialSolver(const sharedFactorGraph& factorGraph,
-				const sharedFactorGraph& factorGraph,
         const boost::shared_ptr<VariableIndex>& variableIndex);
 
     /// @}
@@ -115,14 +136,22 @@ namespace gtsam {
      * Eliminate the factor graph sequentially.  Uses a column elimination tree
      * to recursively eliminate.
      */
-		typename boost::shared_ptr<BayesNet<Conditional> >
+    sharedBayesNet eliminate(Eliminate function) const;
-		eliminate(Eliminate function) const;
+
+    /**
+     * Compute a conditional density P(F|S) while marginalizing out variables J
+     * P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S)
+     * Returns the result as a Bayes net.
+     */
+    sharedBayesNet
+    conditionalBayesNet(const std::vector<Index>& js, size_t nrFrontals,
+        Eliminate function) const;
 
     /**
      * Compute the marginal joint over a set of variables, by integrating out
      * all of the other variables.  Returns the result as a Bayes net
      */
-		typename BayesNet<Conditional>::shared_ptr
+    sharedBayesNet
     jointBayesNet(const std::vector<Index>& js, Eliminate function) const;
 
     /**
@@ -141,8 +170,10 @@ namespace gtsam {
 
     /// @}
 
-	}; // GenericSequentialSolver
+  }
+  ;
+// GenericSequentialSolver
 
-} // namespace gtsam
+}// namespace gtsam
 
 #include <gtsam/inference/GenericSequentialSolver-inl.h>

gtsam/inference/SymbolicSequentialSolver.h

@@ -55,6 +55,14 @@ namespace gtsam {
     SymbolicBayesNet::shared_ptr eliminate() const
     { return Base::eliminate(&EliminateSymbolic); };
 
+    /**
+     * Compute a conditional density P(F|S) while marginalizing out variables J
+     * P(F|S) is obtained by P(J,F,S)=P(J|F,S)P(F|S)P(S) and dropping P(S)
+     * Returns the result as a Bayes net.
+     */
+    SymbolicBayesNet::shared_ptr conditionalBayesNet(const std::vector<Index>& js, size_t nrFrontals) const
+    { return Base::conditionalBayesNet(js, nrFrontals, &EliminateSymbolic); };
+
     /**
      * Compute the marginal joint over a set of variables, by integrating out
      * all of the other variables.  Returns the result as a Bayes net.