Optimized version of symbolic elimination

gtsam/inference/EliminationTree-inl.h

@@ -11,9 +11,12 @@
 #include <gtsam/inference/EliminationTree.h>
 #include <gtsam/inference/VariableSlots.h>
 #include <gtsam/inference/FactorGraph-inl.h>
+#include <gtsam/inference/IndexFactor.h>
+#include <gtsam/inference/IndexConditional.h>
 
 #include <boost/foreach.hpp>
 #include <boost/lambda/lambda.hpp>
+#include <boost/static_assert.hpp>
 #include <iostream>
 #include <set>
 #include <vector>
@@ -31,8 +34,6 @@ EliminationTree<FACTOR>::eliminate_(Conditionals& conditionals) const {
 
   if(debug) cout << "ETree: eliminating " << this->key_ << endl;
 
-  FastSet<Index> separator;
-
   // Create the list of factors to be eliminated, initially empty, and reserve space
   FactorGraph<FACTOR> factors;
   factors.reserve(this->factors_.size() + this->subTrees_.size());
@@ -56,6 +57,39 @@ EliminationTree<FACTOR>::eliminate_(Conditionals& conditionals) const {
   return eliminated.second;
 }
 
+/* ************************************************************************* */
+// This is the explicit specialization for symbolic factors, i.e. IndexFactor
+template<> inline FastSet<Index> EliminationTree<IndexFactor>::eliminateSymbolic_(Conditionals& conditionals) const {
+
+  static const bool debug = false;
+
+  if(debug) cout << "ETree: eliminating " << this->key_ << endl;
+
+  FastSet<Index> variables;
+  BOOST_FOREACH(const sharedFactor& factor, factors_) {
+    variables.insert(factor->begin(), factor->end());
+  }
+  BOOST_FOREACH(const shared_ptr& child, subTrees_) {
+    sharedFactor factor(child->eliminate_(conditionals));
+    variables.insert(factor->begin(), factor->end());
+  }
+  conditionals[this->key_] = IndexConditional::FromRange(variables.begin(), variables.end(), 1);
+  variables.erase(variables.begin());
+
+  if(debug) cout << "Eliminated " << this->key_ << " to get:\n";
+  if(debug) conditionals[this->key_]->print("Conditional: ");
+
+  return variables;
+}
+
+/* ************************************************************************* */
+// This non-specialized version cannot be called.
+template<class FACTOR> FastSet<Index>
+EliminationTree<FACTOR>::eliminateSymbolic_(Conditionals& conditionals) const {
+  throw invalid_argument("symbolic eliminate should never be called from a non-IndexFactor EliminationTree");
+  return FastSet<Index>();
+}
+
 /* ************************************************************************* */
 template<class FACTOR>
 vector<Index> EliminationTree<FACTOR>::ComputeParents(const VariableIndex& structure) {
@@ -204,4 +238,28 @@ EliminationTree<FACTOR>::eliminate() const {
   return bayesNet;
 }
 
+/* ************************************************************************* */
+// Specialization for symbolic elimination that calls the optimized eliminateSymbolic_
+template<>
+inline typename EliminationTree<IndexFactor>::BayesNet::shared_ptr
+EliminationTree<IndexFactor>::eliminate() const {
+
+  // call recursive routine
+  tic(1, "ET recursive eliminate");
+  Conditionals conditionals(this->key_ + 1);
+  (void)eliminateSymbolic_(conditionals);
+  toc(1, "ET recursive eliminate");
+
+  // Add conditionals to BayesNet
+  tic(2, "assemble BayesNet");
+  typename BayesNet::shared_ptr bayesNet(new BayesNet);
+  BOOST_FOREACH(const typename BayesNet::sharedConditional& conditional, conditionals) {
+    if(conditional)
+      bayesNet->push_back(conditional);
+  }
+  toc(2, "assemble BayesNet");
+
+  return bayesNet;
+}
+
 }

gtsam/inference/EliminationTree.h

@@ -11,6 +11,7 @@
 #include <utility>
 
 #include <gtsam/base/FastList.h>
+#include <gtsam/base/FastSet.h>
 #include <gtsam/inference/VariableIndex.h>
 #include <gtsam/inference/BayesNet.h>
 #include <gtsam/inference/FactorGraph.h>
@@ -62,6 +63,12 @@ private:
    */
   sharedFactor eliminate_(Conditionals& conditionals) const;
 
+  /**
+   * Special optimized eliminate for symbolic factors.  Will not compile if
+   * called in a non-IndexFactor EliminationTree.
+   */
+  FastSet<Index> eliminateSymbolic_(Conditionals& conditionals) const;
+
   // Allow access to constructor and add methods for testing purposes
   friend class ::EliminationTreeTester;