Continued work on unordered classes and elimination algorithm

gtsam/CMakeLists.txt

@@ -5,6 +5,7 @@ set (gtsam_subdirs
     base 
     geometry 
     inference 
+	symbolic 
     discrete 
     linear 
     nonlinear 
@@ -76,6 +77,7 @@ set(gtsam_srcs
     ${base_srcs}
     ${geometry_srcs}
     ${inference_srcs}
+	${symbolic_srcs}
     ${discrete_srcs}
     ${linear_srcs}
     ${nonlinear_srcs}

gtsam/inference/EliminationTreeUnordered-inl.h

@@ -162,18 +162,21 @@ namespace gtsam {
 
     // Until the stack is empty
     while(!eliminationStack.empty()) {
-      // Process the next node.  If it has children, add its children to the stack and skip it -
+      // Process the next node.  If it has children, add its children to the stack and mark it
-      // we'll come back and eliminate it later after the children have been processed.  If it has
+      // expanded - we'll come back and eliminate it later after the children have been processed.
-      // no children, we can eliminate it immediately and remove it from the stack.
       EliminationNode& node = nodeStack.top();
       if(node.expanded) {
         // Remove from stack
         nodeStack.pop();
+
         // Do a dense elimination step
+        std::vector<Key> keyAsVector(1); keyAsVector[0] = node.key;
         std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
-          function(node.factors, node.key);
+          function(node.factors, keyAsVector);
+
         // Add conditional to BayesNet and remaining factor to parent
         bayesNet->push_back(eliminationResult.first);
+
         // TODO: Don't add null factor?
         if(node.parent)
           node.parent->factors.push_back(eliminationResult.second);

gtsam/inference/EliminationTreeUnordered.h

@@ -24,7 +24,7 @@
 #include <gtsam/base/Testable.h>
 #include <gtsam/inference/Key.h>
 
-class EliminationTreeTester; // for unit tests, see testEliminationTree
+class EliminationTreeUnorderedTester; // for unit tests, see testEliminationTree
 
 namespace gtsam {
 
@@ -61,6 +61,8 @@ namespace gtsam {
     typedef typename BayesNetType::ConditionalType ConditionalType; ///< The type of conditionals
     typedef typename GRAPH::Eliminate Eliminate; ///< Typedef for an eliminate subroutine
 
+  private:
+
     class Node {
     public:
       typedef boost::shared_ptr<Node> shared_ptr;
@@ -74,8 +76,6 @@ namespace gtsam {
 
     typedef Node::shared_ptr sharedNode; ///< Shared pointer to Node
 
-  private:
-
     /** concept check */
     GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
 
@@ -105,13 +105,13 @@ namespace gtsam {
     */
     EliminationTreeUnordered(const FactorGraphType& factorGraph, const std::vector<Key>& order);
 
-    /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
+    /** TODO: Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
      *  copied, factors are not cloned. */
-    EliminationTreeUnordered(const This& other);
+    EliminationTreeUnordered(const This& other) { throw std::runtime_error("Not implemented"); }
 
-    /** Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are
+    /** TODO: Assignment operator - makes a deep copy of the tree structure, but only pointers to factors are
      *  copied, factors are not cloned. */
-    This& operator=(const This& other);
+    This& operator=(const This& other) { throw std::runtime_error("Not implemented"); }
 
     /// @}
     /// @name Standard Interface
@@ -141,7 +141,7 @@ namespace gtsam {
   private:
 
     /// Allow access to constructor and add methods for testing purposes
-    friend class ::EliminationTreeTester;
+    friend class ::EliminationTreeUnorderedTester;
 
   };
 

gtsam/inference/FactorGraphUnordered.h

@@ -185,37 +185,6 @@ namespace gtsam {
     /** Get the last factor */
     sharedFactor back() const { return factors_.back(); }
 
-    ///** Eliminate the first \c n frontal variables, returning the resulting
-    // * conditional and remaining factor graph - this is very inefficient for
-    // * eliminating all variables, to do that use EliminationTree or
-    // * JunctionTree.
-    // */
-    //std::pair<sharedConditional, FactorGraph<FactorType> > eliminateFrontals(size_t nFrontals, const Eliminate& eliminate) const;
-    //
-    ///** Factor the factor graph into a conditional and a remaining factor graph.  Given the factor
-    // * graph \f$ f(X) \f$, and \c variables to factorize out \f$ V \f$, this function factorizes
-    // * into \f$ f(X) = f(V;Y)f(Y) \f$, where \f$ Y := X \backslash V \f$ are the remaining
-    // * variables.  If \f$ f(X) = p(X) \f$ is a probability density or likelihood, the factorization
-    // * produces a conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$.
-    // *
-    // * For efficiency, this function treats the variables to eliminate
-    // * \c variables as fully-connected, so produces a dense (fully-connected)
-    // * conditional on all of the variables in \c variables, instead of a sparse BayesNet.  If the
-    // * variables are not fully-connected, it is more efficient to sequentially factorize multiple
-    // * times.
-    // */
-    //std::pair<sharedConditional, FactorGraph<FactorType> > eliminate(
-    //  const std::vector<KeyType>& variables, const Eliminate& eliminateFcn,
-    //  boost::optional<const VariableIndex&> variableIndex = boost::none) const;
-
-    ///** Eliminate a single variable, by calling FactorGraph::eliminate. */
-    //std::pair<sharedConditional, FactorGraph<FactorType> > eliminateOne(
-    //    KeyType variable, const Eliminate& eliminateFcn,
-    //    boost::optional<const VariableIndex&> variableIndex = boost::none) const {
-    //  std::vector<size_t> variables(1, variable);
-    //  return eliminate(variables, eliminateFcn, variableIndex);
-    //}
-
     /// @}
     /// @name Modifying Factor Graphs (imperative, discouraged)
     /// @{

gtsam/inference/FactorUnordered.h

@@ -77,13 +77,16 @@ public:
   FactorUnordered() {}
 
   /** Construct unary factor */
-  FactorUnordered(Key key) : keys_(1) { keys_[0] = key; }
+  FactorUnordered(Key key) : keys_(1) {
+    keys_[0] = key; }
 
   /** Construct binary factor */
-  FactorUnordered(Key key1, Key key2) : keys_(2) { keys_[0] = key1; keys_[1] = key2; }
+  FactorUnordered(Key key1, Key key2) : keys_(2) {
+    keys_[0] = key1; keys_[1] = key2; }
 
   /** Construct ternary factor */
-  FactorUnordered(Key key1, Key key2, Key key3) : keys_(3) { keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; }
+  FactorUnordered(Key key1, Key key2, Key key3) : keys_(3) {
+    keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; }
 
   /** Construct 4-way factor */
   FactorUnordered(Key key1, Key key2, Key key3, Key key4) : keys_(4) {
@@ -103,14 +106,14 @@ public:
   /// @name Advanced Constructors
   /// @{
 
+  /** Construct n-way factor from iterator over keys. */
+  template<typename ITERATOR> static FactorUnordered FromIterator(ITERATOR first, ITERATOR last) {
+    FactorUnordered result; result.keys_.assign(first, last); }
+  
   /** Construct n-way factor from container of keys. */
   template<class CONTAINER>
   static FactorUnordered FromKeys(const CONTAINER& keys) { return FromIterator(keys.begin(), keys.end()); }
 
-  /** Construct n-way factor from iterator over keys. */
-  template<class ITERATOR> static FactorUnordered FromIterator(ITERATOR first, ITERATOR last) {
-    FactorUnordered result; result.keys_.assign(first, last); }
-
   /// @}
 
 

gtsam/inference/SymbolicFactorGraphUnordered.h

@@ -1,104 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file SymbolicFactorGraph.h
- * @date Oct 29, 2009
- * @author Frank Dellaert
- */
-
-#pragma once
-
-#include <gtsam/base/types.h>
-#include <gtsam/inference/FactorGraphUnordered.h>
-#include <gtsam/inference/SymbolicFactorUnordered.h>
-
-namespace gtsam {
-
-  /** Symbolic Factor Graph
-   *  \nosubgrouping
-   */
-  class SymbolicFactorGraphUnordered: public FactorGraphUnordered<SymbolicFactorUnordered> {
-
-  public:
-
-    /// @name Standard Constructors
-    /// @{
-
-    /** Construct empty factor graph */
-    SymbolicFactorGraphUnordered() {}
-    
-    ///** Eliminate the first \c n frontal variables, returning the resulting
-    // * conditional and remaining factor graph - this is very inefficient for
-    // * eliminating all variables, to do that use EliminationTree or
-    // * JunctionTree.  Note that this version simply calls
-    // * FactorGraph<IndexFactor>::eliminateFrontals with EliminateSymbolic
-    // * as the eliminate function argument.
-    // */
-    //GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminateFrontals(size_t nFrontals) const;
-    //        
-    ///** Factor the factor graph into a conditional and a remaining factor graph.
-    // * Given the factor graph \f$ f(X) \f$, and \c variables to factorize out
-    // * \f$ V \f$, this function factorizes into \f$ f(X) = f(V;Y)f(Y) \f$, where
-    // * \f$ Y := X\V \f$ are the remaining variables.  If \f$ f(X) = p(X) \f$ is
-    // * a probability density or likelihood, the factorization produces a
-    // * conditional probability density and a marginal \f$ p(X) = p(V|Y)p(Y) \f$.
-    // *
-    // * For efficiency, this function treats the variables to eliminate
-    // * \c variables as fully-connected, so produces a dense (fully-connected)
-    // * conditional on all of the variables in \c variables, instead of a sparse
-    // * BayesNet.  If the variables are not fully-connected, it is more efficient
-    // * to sequentially factorize multiple times.
-    // * Note that this version simply calls
-    // * FactorGraph<GaussianFactor>::eliminate with EliminateSymbolic as the eliminate
-    // * function argument.
-    // */
-    //GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminate(const std::vector<Index>& variables) const;
-
-    ///** Eliminate a single variable, by calling SymbolicFactorGraph::eliminate. */
-    //GTSAM_EXPORT std::pair<sharedConditional, SymbolicFactorGraph> eliminateOne(Index variable) const;
-
-    /// @}
-    /// @name Standard Interface
-    /// @{
-
-    /// @}
-    /// @name Advanced Interface
-    /// @{
-
-    /** Push back unary factor */
-    GTSAM_EXPORT void push_factor(Key key);
-
-    /** Push back binary factor */
-    GTSAM_EXPORT void push_factor(Key key1, Key key2);
-
-    /** Push back ternary factor */
-    GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3);
-
-    /** Push back 4-way factor */
-    GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3, Key key4);
-  };
-
-  /** Create a combined joint factor (new style for EliminationTree). */
-  GTSAM_EXPORT IndexFactor::shared_ptr CombineSymbolic(const FactorGraph<IndexFactor>& factors,
-    const FastMap<Index, std::vector<Index> >& variableSlots);
-
-  /**
-   * CombineAndEliminate provides symbolic elimination.
-   * Combine and eliminate can also be called separately, but for this and
-   * derived classes calling them separately generally does extra work.
-   */
-  GTSAM_EXPORT std::pair<boost::shared_ptr<IndexConditional>, boost::shared_ptr<IndexFactor> >
-  EliminateSymbolic(const FactorGraph<IndexFactor>&, size_t nrFrontals = 1);
-
-  /// @}
-
-} // namespace gtsam

gtsam/inference/SymbolicFactorUnordered.cpp

@@ -1,23 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * GTSAM Copyright 2010, Georgia Tech Research Corporation,
- * Atlanta, Georgia 30332-0415
- * All Rights Reserved
- * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
-
- * See LICENSE for the license information
-
- * -------------------------------------------------------------------------- */
-
-/**
- * @file    SymbolicFactor.cpp
- * @author  Richard Roberts
- * @date    Oct 17, 2010
- */
-
-#include <gtsam/inference/SymbolicFactorUnordered.h>
-
-using namespace std;
-
-namespace gtsam {
-} // gtsam

gtsam/symbolic/CMakeLists.txt

@@ -0,0 +1,29 @@
+# Install headers
+file(GLOB symbolic_headers "*.h")
+install(FILES ${symbolic_headers} DESTINATION include/gtsam/symbolic)
+
+# Components to link tests in this subfolder against
+set(symbolic_local_libs
+   symbolic
+   inference
+   geometry
+   base
+   ccolamd
+)
+
+# Files to exclude from compilation of tests and timing scripts
+set(symbolic_excluded_files
+# "${CMAKE_CURRENT_SOURCE_DIR}/tests/testTypedDiscreteFactor.cpp" # Example of excluding a test     
+    ""  # Add to this list, with full path, to exclude
+)
+
+# Build tests
+if (GTSAM_BUILD_TESTS)
+    gtsam_add_subdir_tests(symbolic "${symbolic_local_libs}" "${gtsam-default}" "${symbolic_excluded_files}") 
+endif(GTSAM_BUILD_TESTS)
+
+# Build timing scripts
+if (GTSAM_BUILD_TIMING)
+    gtsam_add_subdir_timing(symbolic "${symbolic_local_libs}" "${gtsam-default}" "${symbolic_excluded_files}") 
+endif(GTSAM_BUILD_TIMING)
+

gtsam/symbolic/SymbolicBayesNetUnordered.h

@@ -0,0 +1,44 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file SymbolicBayesNet.h
+ * @date Oct 29, 2009
+ * @author Frank Dellaert
+ * @author Richard Roberts
+ */
+
+#pragma once
+
+#include <gtsam/base/types.h>
+#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
+
+namespace gtsam {
+
+  /** Symbolic Bayes Net
+   *  \nosubgrouping
+   */
+  class SymbolicBayesNetUnordered: public SymbolicFactorGraphUnordered {
+
+  public:
+
+    /// @name Standard Constructors
+    /// @{
+
+    /** Construct empty factor graph */
+    SymbolicBayesNetUnordered() {}
+    
+    /// @}
+    /// @name Standard Interface
+    /// @{
+  };
+
+} // namespace gtsam

gtsam/symbolic/SymbolicConditionalUnordered.cpp

@@ -15,7 +15,7 @@
  * @date    Oct 17, 2010
  */
 
-#include <gtsam/inference/SymbolicConditionalUnordered.h>
+#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
 
 namespace gtsam {
 

gtsam/symbolic/SymbolicConditionalUnordered.h

@@ -18,8 +18,8 @@
 #pragma once
 
 #include <gtsam/base/types.h>
-#include <gtsam/inference/SymbolicFactorUnordered.h>
 #include <gtsam/inference/ConditionalUnordered.h>
+#include <gtsam/symbolic/SymbolicFactorUnordered.h>
 
 namespace gtsam {
 
@@ -62,14 +62,22 @@ namespace gtsam {
     SymbolicConditionalUnordered(Index j, Index parent1, Index parent2, Index parent3) : BaseFactor(j, parent1, parent2, parent3), BaseConditional(3) {}
 
     /** Named constructor from an arbitrary number of keys and frontals */
-    template<class ITERATOR>
+    template<typename ITERATOR>
-    static SymbolicConditionalUnordered FromIterator(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals) :
+    static SymbolicConditionalUnordered FromIterator(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals)
     {
       SymbolicConditionalUnordered result;
-      result.keys_.assign(firstKey, lastKey);
+      result = BaseFactor::FromIterator(firstKey, lastKey);
       result.nrFrontals_ = nrFrontals;
-      return result;
+      return result; }
-    }
+
+    /** Named constructor from an arbitrary number of keys and frontals */
+    template<class CONTAINER>
+    static SymbolicConditionalUnordered FromKeys(const CONTAINER& keys, size_t nrFrontals)
+    {
+      SymbolicConditionalUnordered result;
+      result = BaseFactor::FromKeys(keys);
+      result.nrFrontals_ = nrFrontals;
+      return result; }
 
     /// @}
 

gtsam/symbolic/SymbolicEliminationTreeUnordered.h

@@ -16,15 +16,16 @@
  * @author Richard Roberts
  */
 
-#include <gtsam/inference/SymbolicBayesNetUnordered.h>
-#include <gtsam/inference/SymbolicFactorGraphUnordered.h>
 #include <gtsam/inference/EliminationTreeUnordered.h>
+#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
+#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
 
 namespace gtsam {
 
-  class SymbolicEliminationTreeUnordered : public EliminationTreeUnordered<SymbolicBayesNetUnordered,SymbolicFactorGraphUnordered> {
+  class SymbolicEliminationTreeUnordered :
+    public EliminationTreeUnordered<SymbolicBayesNetUnordered, SymbolicFactorGraphUnordered> {
   public:
-    typedef EliminationTreeUnordered<SymbolicBayesNetUnordered,SymbolicFactorGraphUnordered> Base; ///< Base class
+    typedef EliminationTreeUnordered<SymbolicBayesNetUnordered, SymbolicFactorGraphUnordered> Base; ///< Base class
     typedef SymbolicEliminationTreeUnordered This; ///< This class
     typedef boost::shared_ptr<This> shared_ptr; ///< Shared pointer to this class
     
@@ -45,7 +46,7 @@ namespace gtsam {
     * constructor instead.
     * @param factorGraph The factor graph for which to build the elimination tree
     */
-    SymbolicEliminationTreeUnordered(const FactorGraphType& factorGraph, const std::vector<Key>& order) :
+    SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, const std::vector<Key>& order) :
       Base(factorGraph, order) {}
 
     /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are

gtsam/symbolic/SymbolicFactorGraphUnordered.cpp

@@ -17,7 +17,7 @@
 
 #include <boost/make_shared.hpp>
 
-#include <gtsam/inference/SymbolicFactorGraphUnordered.h>
+#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
 
 namespace gtsam {
 

gtsam/symbolic/SymbolicFactorGraphUnordered.h

@@ -0,0 +1,59 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file SymbolicFactorGraph.h
+ * @date Oct 29, 2009
+ * @author Frank Dellaert
+ * @author Richard Roberts
+ */
+
+#pragma once
+
+#include <gtsam/base/types.h>
+#include <gtsam/inference/FactorGraphUnordered.h>
+#include <gtsam/symbolic/SymbolicFactorUnordered.h>
+
+namespace gtsam {
+
+  /** Symbolic Factor Graph
+   *  \nosubgrouping
+   */
+  class SymbolicFactorGraphUnordered: public FactorGraphUnordered<SymbolicFactorUnordered> {
+
+  public:
+
+    /// @name Standard Constructors
+    /// @{
+
+    /** Construct empty factor graph */
+    SymbolicFactorGraphUnordered() {}
+
+    /// @}
+    /// @name Standard Interface
+    /// @{
+
+    /** Push back unary factor */
+    GTSAM_EXPORT void push_factor(Key key);
+
+    /** Push back binary factor */
+    GTSAM_EXPORT void push_factor(Key key1, Key key2);
+
+    /** Push back ternary factor */
+    GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3);
+
+    /** Push back 4-way factor */
+    GTSAM_EXPORT void push_factor(Key key1, Key key2, Key key3, Key key4);
+
+    /// @}
+  };
+
+} // namespace gtsam

gtsam/symbolic/SymbolicFactorUnordered.cpp

@@ -0,0 +1,54 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    SymbolicFactor.cpp
+ * @author  Richard Roberts
+ * @date    Oct 17, 2010
+ */
+
+#include <boost/foreach.hpp>
+#include <boost/make_shared.hpp>
+
+#include <gtsam/symbolic/SymbolicFactorUnordered.h>
+#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
+
+using namespace std;
+
+namespace gtsam {
+
+  /* ************************************************************************* */
+  std::pair<boost::shared_ptr<SymbolicConditionalUnordered>, boost::shared_ptr<SymbolicFactorUnordered> >
+    EliminateSymbolicUnordered(const vector<SymbolicFactorUnordered::shared_ptr>& factors, const vector<Key>& keys)
+  {
+    // Gather all keys
+    FastSet<Key> allKeys;
+    BOOST_FOREACH(const SymbolicFactorUnordered::shared_ptr& factor, factors) {
+      allKeys.insert(factor->begin(), factor->end()); }
+
+    // Sort frontal keys
+    FastSet<Key> frontals(keys);
+    const size_t nFrontals = keys.size();
+
+    // Build a key vector with the frontals followed by the separator
+    vector<Key> orderedKeys(allKeys.size());
+    std::copy(keys.begin(), keys.end(), orderedKeys.begin());
+    std::set_difference(allKeys.begin(), allKeys.end(), frontals.begin(), frontals.end(), orderedKeys.begin() + nFrontals);
+
+    // Return resulting conditional and factor
+    return make_pair(
+      boost::make_shared<SymbolicConditionalUnordered>(
+      SymbolicConditionalUnordered::FromKeys(orderedKeys, nFrontals)),
+      boost::make_shared<SymbolicFactorUnordered>(
+      SymbolicFactorUnordered::FromIterator(orderedKeys.begin() + nFrontals, orderedKeys.end())));
+  }
+
+} // gtsam

gtsam/symbolic/SymbolicFactorUnordered.h

@@ -48,7 +48,7 @@ namespace gtsam {
     /// @{
 
     /** Virtual destructor */
-    virtual ~SymbolicFactorUnordered() {}
+    ~SymbolicFactorUnordered() {}
 
     /** Copy constructor */
     SymbolicFactorUnordered(const This& f) : Base(f) {}
@@ -80,7 +80,18 @@ namespace gtsam {
     /// @{
 
     /** Constructor from a collection of keys */
-    template<class KeyIterator> SymbolicFactorUnordered(KeyIterator beginKey, KeyIterator endKey) : Base(beginKey, endKey) {}
+    template<typename KEYITERATOR>
+    static SymbolicFactorUnordered FromIterator(KEYITERATOR beginKey, KEYITERATOR endKey) {
+      SymbolicFactorUnordered result;
+      result = Base::FromIterator(beginKey, endKey);
+      return result; }
+
+    /** Constructor from a collection of keys */
+    template<class CONTAINER>
+    static SymbolicFactorUnordered FromKeys(const CONTAINER& keys) {
+      SymbolicFactorUnordered result;
+      result = Base::FromKeys(keys);
+      return result; }
 
     /// @}
 
@@ -91,7 +102,10 @@ namespace gtsam {
     void serialize(ARCHIVE & ar, const unsigned int version) {
       ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
     }
-
   }; // IndexFactor
 
+
+  std::pair<boost::shared_ptr<SymbolicConditionalUnordered>, boost::shared_ptr<SymbolicFactorUnordered> >
+    EliminateSymbolicUnordered(const vector<SymbolicFactorUnordered::shared_ptr>& factors, const vector<Key>& keys);
+
 }

gtsam/symbolic/tests/testSymbolicEliminationTree.cpp

@@ -0,0 +1,117 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    testSymbolicEliminationTree.cpp
+ * @brief   
+ * @author  Richard Roberts
+ * @date Oct 14, 2010
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/symbolic/SymbolicEliminationTreeUnordered.h>
+
+using namespace gtsam;
+using namespace std;
+
+class EliminationTreeUnorderedTester {
+public:
+  // build hardcoded tree
+  static SymbolicEliminationTreeUnordered buildHardcodedTree(const SymbolicFactorGraphUnordered& fg) {
+
+    SymbolicEliminationTreeUnordered::sharedNode leaf0(new SymbolicEliminationTree);
+    leaf0->add(fg[0]);
+    leaf0->add(fg[1]);
+
+    SymbolicEliminationTreeUnordered::sharedNode node1(new SymbolicEliminationTree(1));
+    node1->add(fg[2]);
+    node1->add(leaf0);
+
+    SymbolicEliminationTreeUnordered::sharedNode node2(new SymbolicEliminationTree(2));
+    node2->add(fg[3]);
+    node2->add(node1);
+
+    SymbolicEliminationTreeUnordered::sharedNode leaf3(new SymbolicEliminationTree(3));
+    leaf3->add(fg[4]);
+
+    SymbolicEliminationTreeUnordered::sharedNode etree(new SymbolicEliminationTree(4));
+    etree->add(leaf3);
+    etree->add(node2);
+
+    return etree;
+  }
+};
+
+TEST(EliminationTree, Create)
+{
+  // create example factor graph
+  SymbolicFactorGraph fg;
+  fg.push_factor(0, 1);
+  fg.push_factor(0, 2);
+  fg.push_factor(1, 4);
+  fg.push_factor(2, 4);
+  fg.push_factor(3, 4);
+
+  SymbolicEliminationTree::shared_ptr expected = EliminationTreeTester::buildHardcodedTree(fg);
+
+  // Build from factor graph
+  SymbolicEliminationTree::shared_ptr actual = SymbolicEliminationTree::Create(fg);
+
+  CHECK(assert_equal(*expected,*actual));
+}
+
+/* ************************************************************************* */
+// Test to drive elimination tree development
+// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4)
+/* ************************************************************************* */
+
+TEST(EliminationTree, eliminate )
+{
+  // create expected Chordal bayes Net
+  SymbolicBayesNet expected;
+  expected.push_front(boost::make_shared<IndexConditional>(4));
+  expected.push_front(boost::make_shared<IndexConditional>(3,4));
+  expected.push_front(boost::make_shared<IndexConditional>(2,4));
+  expected.push_front(boost::make_shared<IndexConditional>(1,2,4));
+  expected.push_front(boost::make_shared<IndexConditional>(0,1,2));
+
+  // Create factor graph
+  SymbolicFactorGraph fg;
+  fg.push_factor(0, 1);
+  fg.push_factor(0, 2);
+  fg.push_factor(1, 4);
+  fg.push_factor(2, 4);
+  fg.push_factor(3, 4);
+
+  // eliminate
+  SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();
+
+  CHECK(assert_equal(expected,actual));
+}
+
+/* ************************************************************************* */
+TEST(EliminationTree, disconnected_graph) {
+  SymbolicFactorGraph fg;
+  fg.push_factor(0, 1);
+  fg.push_factor(0, 2);
+  fg.push_factor(1, 2);
+  fg.push_factor(3, 4);
+
+  CHECK_EXCEPTION(SymbolicEliminationTree::Create(fg), DisconnectedGraphException);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */