More work on elimination and BayesTree

gtsam/base/tests/testTreeTraversal.cpp

@@ -0,0 +1,109 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 testTreeTraversal
+ * @brief Unit tests for tree traversal, cloning, and printing
+ * @author Richard Roberts
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <vector>
+#include <list>
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
+#include <boost/assign/std/list.hpp>
+
+using boost::assign::operator+=;
+using namespace std;
+
+struct TestNode {
+  typedef boost::shared_ptr<TestNode> shared_ptr;
+  int data;
+  vector<shared_ptr> children;
+  TestNode(int data) : data(data) {}
+};
+
+struct TestForest {
+  typedef TestNode::shared_ptr sharedNode;
+  vector<sharedNode> roots_;
+  const vector<sharedNode>& roots() const { return roots_; }
+};
+
+TestForest makeTestForest() {
+  //    0     1
+  //   / \
+  //  2   3
+  //      |
+  //      4
+  TestForest forest;
+  forest.roots_.push_back(boost::make_shared<TestNode>(0));
+  forest.roots_.push_back(boost::make_shared<TestNode>(1));
+  forest.roots_[0]->children.push_back(boost::make_shared<TestNode>(2));
+  forest.roots_[0]->children.push_back(boost::make_shared<TestNode>(3));
+  forest.roots_[0]->children[1]->children.push_back(boost::make_shared<TestNode>(4));
+  return forest;
+}
+
+/* ************************************************************************* */
+struct PreOrderVisitor {
+  // This visitor stores the nodes visited so the visit order can be checked later.  It also returns
+  // the current node index as the data and checks that the parent index properly matches the index
+  // referenced in the node.
+  std::list<int> visited;
+  bool parentsMatched;
+  PreOrderVisitor() : parentsMatched(true) {}
+  int operator()(const TestNode::shared_ptr& node, int parentData) {
+    visited.push_back(node->data);
+    // Check parent index
+    const int expectedParentIndex =
+      node->data == 0 ? -1 :
+      node->data == 1 ? -1 :
+      node->data == 2 ? 0 :
+      node->data == 3 ? 0 :
+      node->data == 4 ? 0 :
+      (throw std::runtime_error("Unexpected node index"), -1);
+    if(expectedParentIndex != parentData)
+      parentsMatched = false;
+    return node->data;
+  }
+};
+
+/* ************************************************************************* */
+struct PostOrderVisitor {
+  // This visitor stores the nodes visited so the visit order can be checked later.
+  std::list<int> visited;
+  void operator()(const TestNode::shared_ptr& node, int myData) {
+    visited.push_back(node->data);
+  }
+};
+
+/* ************************************************************************* */
+TEST(treeTraversal, DepthFirst)
+{
+  // Get test forest
+  TestForest testForest = makeTestForest();
+
+  // Expected pre-order
+  std::list<int> preOrderExpected;
+  preOrderExpected += 0, 2, 3, 4, 1;
+  std::list<int> postOrderExpected;
+  postOrderExpected += 2, 4, 3, 0, 1;
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

gtsam/base/treeTraversal-inst.h

@@ -18,6 +18,10 @@
 
 #include <gtsam/base/FastList.h>
 
+#include <stack>
+#include <boost/foreach.hpp>
+#include <boost/bind.hpp>
+
 namespace gtsam {
 
   /** Internal functions used for traversing trees */
@@ -29,30 +33,15 @@ namespace gtsam {
       template<typename NODE, typename DATA>
       struct TraversalNode {
         bool expanded;
-        NODE& const treeNode;
+        const boost::shared_ptr<NODE>& treeNode;
         DATA data;
-        TraversalNode(NODE* _treeNode, const DATA& _data) :
+        TraversalNode(const boost::shared_ptr<NODE>& _treeNode, const DATA& _data) :
           expanded(false), treeNode(_treeNode), data(_data) {}
       };
 
-      // Functional object to visit a node and then add it to the traversal stack
-      template<typename NODE, typename VISITOR, typename DATA, typename STACK>
-      struct PreOrderExpand {
-        VISITOR& visitor_;
-        DATA& parentData_;
-        STACK& stack_;
-        PreOrderExpand(VISITOR& visitor, DATA& parentData, STACK& stack) :
-          visitor_(visitor), parentData_(parentData), stack_(stack) {}
-        template<typename P>
-        void operator()(const P& nodePointer) {
-          // Add node
-          stack_.push(TraversalNode<NODE,DATA>(*nodePointer, visitor_(nodePointer, parentData_)));
-        }
-      };
-
       /// Do nothing - default argument for post-visitor for tree traversal
       template<typename NODE, typename DATA>
-      void no_op(const NODE& node, const DATA& data) {}
+      void no_op(const boost::shared_ptr<NODE>& node, const DATA& data) {}
     }
 
     /** Traverse a forest depth-first with pre-order and post-order visits.
@@ -72,16 +61,19 @@ namespace gtsam {
     template<class FOREST, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
     void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost)
     {
+      // Typedefs
+      typedef typename FOREST::Node Node;
+      typedef boost::shared_ptr<Node> sharedNode;
+
       // Depth first traversal stack
       typedef TraversalNode<typename FOREST::Node, DATA> TraversalNode;
       typedef std::stack<TraversalNode, FastList<TraversalNode> > Stack;
-      typedef PreOrderExpand<typename FOREST::Node, VISITOR_PRE, DATA, Stack> Expander;
       Stack stack;
 
       // Add roots to stack (use reverse iterators so children are processed in the order they
       // appear)
-      (void) std::for_each(forest.roots().rbegin(), forest.roots().rend(),
+      BOOST_REVERSE_FOREACH(const sharedNode& root, forest.roots())
-        Expander(visitorPre, rootData, stack));
+        stack.push(TraversalNode(root, visitorPre(root, rootData)));
 
       // Traverse
       while(!stack.empty())
@@ -97,8 +89,8 @@ namespace gtsam {
         } else {
           // If not already visited, visit the node and add its children (use reverse iterators so
           // children are processed in the order they appear)
-          (void) std::for_each(node.treeNode.children.rbegin(), node.treeNode.children.rend(),
+          BOOST_REVERSE_FOREACH(const sharedNode& child, node.treeNode->children)
-            Expander(visitorPre, node.data, stack));
+            stack.push(TraversalNode(child, visitorPre(child, node.data)));
           node.expanded = true;
         }
       }
@@ -118,8 +110,7 @@ namespace gtsam {
     template<class FOREST, typename DATA, typename VISITOR_PRE>
     void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre)
     {
-      DepthFirstForest<FOREST, DATA, VISITOR_PRE, void(&)(const typename FOREST::Node&, const DATA&)>(
+      DepthFirstForest(forest, rootData, visitorPre, no_op<typename FOREST::Node, DATA>);
-        forest, rootData, visitorPre, no_op<typename FOREST::Node, DATA>);
     }
     
 
@@ -127,10 +118,11 @@ namespace gtsam {
     /** Traversal function for CloneForest */
     namespace {
       template<typename NODE>
-      boost::shared_ptr<NODE> CloneForestVisitorPre(const NODE& node, const boost::shared_ptr<NODE>& parentPointer)
+      boost::shared_ptr<NODE>
+        CloneForestVisitorPre(const boost::shared_ptr<NODE>& node, const boost::shared_ptr<NODE>& parentPointer)
       {
         // Clone the current node and add it to its cloned parent
-        boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(node);
+        boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node);
         parentPointer->children.push_back(clone);
         return clone;
       }
@@ -155,10 +147,11 @@ namespace gtsam {
     /** Traversal function for PrintForest */
     namespace {
       template<typename NODE>
-      std::string PrintForestVisitorPre(const NODE& node, const std::string& parentString, const KeyFormatter& formatter)
+      std::string
+        PrintForestVisitorPre(const boost::shared_ptr<NODE>& node, const std::string& parentString, const KeyFormatter& formatter)
       {
         // Print the current node
-        node.print(parentString + "-", formatter);
+        node->print(parentString + "-", formatter);
         // Increment the indentation
         return parentString + "| ";
       }
@@ -169,7 +162,7 @@ namespace gtsam {
     template<class FOREST>
     void PrintForest(const FOREST& forest, const std::string& str, const KeyFormatter& keyFormatter) {
       typedef typename FOREST::Node Node;
-      DepthFirstForest(forest, str, boost::bind(PrintForestVisitorPre<Node>, _1, _2, formatter));
+      DepthFirstForest(forest, str, boost::bind(PrintForestVisitorPre<Node>, _1, _2, keyFormatter));
     }
   }
 

gtsam/inference/BayesNetUnordered-inst.h

@@ -19,7 +19,9 @@
 #pragma once
 
 #include <gtsam/inference/BayesNetUnordered.h>
-#include <gtsam/inference/FactorGraphUnordered-inst.h>
+
+#include <boost/foreach.hpp>
+#include <fstream>
 
 namespace gtsam {
 
@@ -32,17 +34,16 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class CONDITIONAL>
-  void BayesNetUnordered<CONDITIONAL>::saveGraph(
+  void BayesNetUnordered<CONDITIONAL>::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const
-    const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter)
   {
     std::ofstream of(s.c_str());
     of << "digraph G{\n";
 
     BOOST_REVERSE_FOREACH(const sharedConditional& conditional, *this) {
       typename CONDITIONAL::Frontals frontals = conditional->frontals();
-      Index me = frontals.front();
+      Key me = frontals.front();
       typename CONDITIONAL::Parents parents = conditional->parents();
-      BOOST_FOREACH(Index p, parents)
+      BOOST_FOREACH(Key p, parents)
         of << p << "->" << me << std::endl;
     }
 

gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h

@@ -22,22 +22,22 @@
 namespace gtsam {
 
   /* ************************************************************************* */
-  template<class DERIVED, class CONDITIONAL>
+  template<class DERIVED, class FACTORGRAPH, class BAYESNET>
   std::vector<Key>
-    BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::separator_setminus_B(derived_ptr B) const
+    BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::separator_setminus_B(derived_ptr B) const
   {
     FastSet<Key> p_F_S_parents(this->conditional()->beginParents(), this->conditional()->endParents());
     FastSet<Key> indicesB(B->conditional()->begin(), B->conditional()->end());
     std::vector<Key> S_setminus_B;
-    std::set_difference(p_F_S->beginParents(), p_F_S->endParents(),
+    std::set_difference(p_F_S_parents.begin(), p_F_S_parents.end(),
       indicesB.begin(), indicesB.end(), back_inserter(S_setminus_B));
     return S_setminus_B;
   }
 
   /* ************************************************************************* */
-  template<class DERIVED, class CONDITIONAL>
+  template<class DERIVED, class FACTORGRAPH, class BAYESNET>
-  std::vector<Key> BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::shortcut_indices(
+  std::vector<Key> BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::shortcut_indices(
-    derived_ptr B, const FactorGraphUnordered<FactorType>& p_Cp_B) const
+    derived_ptr B, const FactorGraphType& p_Cp_B) const
   {
     gttic(shortcut_indices);
     FastSet<Key> allKeys = p_Cp_B.keys();
@@ -54,31 +54,31 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  template<class DERIVED, class CONDITIONAL>
+  template<class DERIVED, class FACTORGRAPH, class BAYESNET>
-  void BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::print(
+  void BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::print(
     const std::string& s, const KeyFormatter& keyFormatter) const
   {
     conditional_->print(s, keyFormatter);
   }
 
   /* ************************************************************************* */
-  template<class DERIVED, class CONDITIONAL>
+  template<class DERIVED, class FACTORGRAPH, class BAYESNET>
-  size_t BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::treeSize() const {
+  size_t BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::treeSize() const {
     size_t size = 1;
-    BOOST_FOREACH(const derived_ptr& child, children_)
+    BOOST_FOREACH(const derived_ptr& child, children)
       size += child->treeSize();
     return size;
   }
 
   /* ************************************************************************* */
-  template<class DERIVED, class CONDITIONAL>
+  template<class DERIVED, class FACTORGRAPH, class BAYESNET>
-  size_t BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::numCachedSeparatorMarginals() const
+  size_t BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::numCachedSeparatorMarginals() const
   {
     if (!cachedSeparatorMarginal_)
       return 0;
 
     size_t subtree_count = 1;
-    BOOST_FOREACH(const derived_ptr& child, children_)
+    BOOST_FOREACH(const derived_ptr& child, children)
       subtree_count += child->numCachedSeparatorMarginals();
 
     return subtree_count;
@@ -89,146 +89,146 @@ namespace gtsam {
   // clique on the root. We can compute it recursively from the parent shortcut
   // P(Sp|R) as \int P(Fp|Sp) P(Sp|R), where Fp are the frontal nodes in p
   /* ************************************************************************* */
-  template<class DERIVED, class CONDITIONAL>
+  //template<class DERIVED, class FACTORGRAPH, class BAYESNET>
-  BayesNetUnordered<CONDITIONAL> BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::shortcut(
+  //BayesNetUnordered<CONDITIONAL> BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::shortcut(
-    derived_ptr B, Eliminate function) const
+  //  derived_ptr B, Eliminate function) const
-  {
+  //{
-    gttic(BayesTreeCliqueBaseUnordered_shortcut);
+  //  gttic(BayesTreeCliqueBaseUnordered_shortcut);
 
-    // We only calculate the shortcut when this clique is not B
+  //  // We only calculate the shortcut when this clique is not B
-    // and when the S\B is not empty
+  //  // and when the S\B is not empty
-    std::vector<Key> S_setminus_B = separator_setminus_B(B);
+  //  std::vector<Key> S_setminus_B = separator_setminus_B(B);
-    if (B.get() != this && !S_setminus_B.empty()) {
+  //  if (B.get() != this && !S_setminus_B.empty()) {
 
-      // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
+  //    // Obtain P(Cp||B) = P(Fp|Sp) * P(Sp||B) as a factor graph
-      derived_ptr parent(parent_.lock());
+  //    derived_ptr parent(parent_.lock());
-      gttoc(BayesTreeCliqueBaseUnordered_shortcut);
+  //    gttoc(BayesTreeCliqueBaseUnordered_shortcut);
-      FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
+  //    FactorGraphType p_Cp_B(parent->shortcut(B, function)); // P(Sp||B)
-      gttic(BayesTreeCliqueBaseUnordered_shortcut);
+  //    gttic(BayesTreeCliqueBaseUnordered_shortcut);
-      p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp)
+  //    p_Cp_B.push_back(parent->conditional()->toFactor()); // P(Fp|Sp)
 
-      // Determine the variables we want to keepSet, S union B
+  //    // Determine the variables we want to keepSet, S union B
-      std::vector<Key> keep = shortcut_indices(B, p_Cp_B);
+  //    std::vector<Key> keep = shortcut_indices(B, p_Cp_B);
 
-      // Reduce the variable indices to start at zero
+  //    // Reduce the variable indices to start at zero
-      gttic(Reduce);
+  //    gttic(Reduce);
-      const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys());
+  //    const Permutation reduction = internal::createReducingPermutation(p_Cp_B.keys());
-      internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
+  //    internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
-      BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp_B) {
+  //    BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp_B) {
-        if(factor) factor->reduceWithInverse(inverseReduction); }
+  //      if(factor) factor->reduceWithInverse(inverseReduction); }
-      inverseReduction.applyInverse(keep);
+  //    inverseReduction.applyInverse(keep);
-      gttoc(Reduce);
+  //    gttoc(Reduce);
 
-      // Create solver that will marginalize for us
+  //    // Create solver that will marginalize for us
-      GenericSequentialSolver<FactorType> solver(p_Cp_B);
+  //    GenericSequentialSolver<FactorType> solver(p_Cp_B);
 
-      // Finally, we only want to have S\B variables in the Bayes net, so
+  //    // Finally, we only want to have S\B variables in the Bayes net, so
-      size_t nrFrontals = S_setminus_B.size();
+  //    size_t nrFrontals = S_setminus_B.size();
-      BayesNet<CONDITIONAL> result = *solver.conditionalBayesNet(keep, nrFrontals, function);
+  //    BayesNet<CONDITIONAL> result = *solver.conditionalBayesNet(keep, nrFrontals, function);
 
-      // Undo the reduction
+  //    // Undo the reduction
-      gttic(Undo_Reduce);
+  //    gttic(Undo_Reduce);
-      BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp_B) {
+  //    BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp_B) {
-        if (factor) factor->permuteWithInverse(reduction); }
+  //      if (factor) factor->permuteWithInverse(reduction); }
-      result.permuteWithInverse(reduction);
+  //    result.permuteWithInverse(reduction);
-      gttoc(Undo_Reduce);
+  //    gttoc(Undo_Reduce);
 
-      assertInvariants();
+  //    assertInvariants();
 
-      return result;
+  //    return result;
-    } else {
+  //  } else {
-      return BayesNet<CONDITIONAL>();
+  //    return BayesNet<CONDITIONAL>();
-    }
+  //  }
-  }
+  //}
 
   /* ************************************************************************* */
   // separator marginal, uses separator marginal of parent recursively
   // P(C) = P(F|S) P(S)
   /* ************************************************************************* */
-  template<class DERIVED, class CONDITIONAL>
+  //template<class DERIVED, class FACTORGRAPH, class BAYESNET>
-  FactorGraph<typename BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::FactorType> BayesTreeCliqueBaseUnordered<
+  //FactorGraph<typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::FactorType> BayesTreeCliqueBaseUnordered<
-      DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const
+  //    DERIVED, CONDITIONAL>::separatorMarginal(derived_ptr R, Eliminate function) const
-  {
+  //{
-    gttic(BayesTreeCliqueBaseUnordered_separatorMarginal);
+  //  gttic(BayesTreeCliqueBaseUnordered_separatorMarginal);
-    // Check if the Separator marginal was already calculated
+  //  // Check if the Separator marginal was already calculated
-    if (!cachedSeparatorMarginal_) {
+  //  if (!cachedSeparatorMarginal_) {
-      gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss);
+  //    gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss);
-      // If this is the root, there is no separator
+  //    // If this is the root, there is no separator
-      if (R.get() == this) {
+  //    if (R.get() == this) {
-        // we are root, return empty
+  //      // we are root, return empty
-        FactorGraph<FactorType> empty;
+  //      FactorGraph<FactorType> empty;
-        cachedSeparatorMarginal_ = empty;
+  //      cachedSeparatorMarginal_ = empty;
-      } else {
+  //    } else {
-        // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
+  //      // Obtain P(S) = \int P(Cp) = \int P(Fp|Sp) P(Sp)
-        // initialize P(Cp) with the parent separator marginal
+  //      // initialize P(Cp) with the parent separator marginal
-        derived_ptr parent(parent_.lock());
+  //      derived_ptr parent(parent_.lock());
-        gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); // Flatten recursion in timing outline
+  //      gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss); // Flatten recursion in timing outline
-        gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal);
+  //      gttoc(BayesTreeCliqueBaseUnordered_separatorMarginal);
-        FactorGraph<FactorType> p_Cp(parent->separatorMarginal(R, function)); // P(Sp)
+  //      FactorGraph<FactorType> p_Cp(parent->separatorMarginal(R, function)); // P(Sp)
-        gttic(BayesTreeCliqueBaseUnordered_separatorMarginal);
+  //      gttic(BayesTreeCliqueBaseUnordered_separatorMarginal);
-        gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss);
+  //      gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachemiss);
-        // now add the parent conditional
+  //      // now add the parent conditional
-        p_Cp.push_back(parent->conditional()->toFactor()); // P(Fp|Sp)
+  //      p_Cp.push_back(parent->conditional()->toFactor()); // P(Fp|Sp)
 
-        // Reduce the variable indices to start at zero
+  //      // Reduce the variable indices to start at zero
-        gttic(Reduce);
+  //      gttic(Reduce);
-        const Permutation reduction = internal::createReducingPermutation(p_Cp.keys());
+  //      const Permutation reduction = internal::createReducingPermutation(p_Cp.keys());
-        internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
+  //      internal::Reduction inverseReduction = internal::Reduction::CreateAsInverse(reduction);
-        BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp) {
+  //      BOOST_FOREACH(const boost::shared_ptr<FactorType>& factor, p_Cp) {
-          if(factor) factor->reduceWithInverse(inverseReduction); }
+  //        if(factor) factor->reduceWithInverse(inverseReduction); }
 
-        // The variables we want to keepSet are exactly the ones in S
+  //      // The variables we want to keepSet are exactly the ones in S
-        sharedConditional p_F_S = this->conditional();
+  //      sharedConditional p_F_S = this->conditional();
-        std::vector<Key> indicesS(p_F_S->beginParents(), p_F_S->endParents());
+  //      std::vector<Key> indicesS(p_F_S->beginParents(), p_F_S->endParents());
-        inverseReduction.applyInverse(indicesS);
+  //      inverseReduction.applyInverse(indicesS);
-        gttoc(Reduce);
+  //      gttoc(Reduce);
 
-        // Create solver that will marginalize for us
+  //      // Create solver that will marginalize for us
-        GenericSequentialSolver<FactorType> solver(p_Cp);
+  //      GenericSequentialSolver<FactorType> solver(p_Cp);
 
-        cachedSeparatorMarginal_ = *(solver.jointBayesNet(indicesS, function));
+  //      cachedSeparatorMarginal_ = *(solver.jointBayesNet(indicesS, function));
 
-        // Undo the reduction
+  //      // Undo the reduction
-        gttic(Undo_Reduce);
+  //      gttic(Undo_Reduce);
-        BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp) {
+  //      BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, p_Cp) {
-          if (factor) factor->permuteWithInverse(reduction); }
+  //        if (factor) factor->permuteWithInverse(reduction); }
-        BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, *cachedSeparatorMarginal_) {
+  //      BOOST_FOREACH(const typename boost::shared_ptr<FactorType>& factor, *cachedSeparatorMarginal_) {
-          if (factor) factor->permuteWithInverse(reduction); }
+  //        if (factor) factor->permuteWithInverse(reduction); }
-        gttoc(Undo_Reduce);
+  //      gttoc(Undo_Reduce);
-      }
+  //    }
-    } else {
+  //  } else {
-      gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachehit);
+  //    gttic(BayesTreeCliqueBaseUnordered_separatorMarginal_cachehit);
-    }
+  //  }
 
-    // return the shortcut P(S||B)
+  //  // return the shortcut P(S||B)
-    return *cachedSeparatorMarginal_; // return the cached version
+  //  return *cachedSeparatorMarginal_; // return the cached version
-  }
+  //}
 
   /* ************************************************************************* */
   // marginal2, uses separator marginal of parent recursively
   // P(C) = P(F|S) P(S)
   /* ************************************************************************* */
-  template<class DERIVED, class CONDITIONAL>
+  //template<class DERIVED, class FACTORGRAPH, class BAYESNET>
-  FactorGraph<typename BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::FactorType> BayesTreeCliqueBaseUnordered<
+  //FactorGraph<typename BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::FactorType> BayesTreeCliqueBaseUnordered<
-      DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const
+  //    DERIVED, CONDITIONAL>::marginal2(derived_ptr R, Eliminate function) const
-  {
+  //{
-    gttic(BayesTreeCliqueBaseUnordered_marginal2);
+  //  gttic(BayesTreeCliqueBaseUnordered_marginal2);
-    // initialize with separator marginal P(S)
+  //  // initialize with separator marginal P(S)
-    FactorGraph<FactorType> p_C(this->separatorMarginal(R, function));
+  //  FactorGraph<FactorType> p_C(this->separatorMarginal(R, function));
-    // add the conditional P(F|S)
+  //  // add the conditional P(F|S)
-    p_C.push_back(this->conditional()->toFactor());
+  //  p_C.push_back(this->conditional()->toFactor());
-    return p_C;
+  //  return p_C;
-  }
+  //}
 
   /* ************************************************************************* */
-  template<class DERIVED, class CONDITIONAL>
+  template<class DERIVED, class FACTORGRAPH, class BAYESNET>
-  void BayesTreeCliqueBaseUnordered<DERIVED, CONDITIONAL>::deleteCachedShortcuts() {
+  void BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET>::deleteCachedShortcuts() {
 
     // When a shortcut is requested, all of the shortcuts between it and the
     // root are also generated. So, if this clique's cached shortcut is set,
     // recursively call over all child cliques. Otherwise, it is unnecessary.
     if (cachedSeparatorMarginal_) {
-      BOOST_FOREACH(derived_ptr& child, children_) {
+      BOOST_FOREACH(derived_ptr& child, children) {
         child->deleteCachedShortcuts();
       }
 

gtsam/inference/BayesTreeCliqueBaseUnordered.h

@@ -22,7 +22,7 @@
 #include <gtsam/inference/BayesNetUnordered.h>
 
 namespace gtsam {
-  template<class CONDITIONAL, class CLIQUE> class BayesTreeUnordered;
+  template<class CLIQUE> class BayesTreeUnordered;
 }
 
 namespace gtsam {
@@ -43,18 +43,21 @@ namespace gtsam {
   template<class DERIVED, class FACTORGRAPH, class BAYESNET>
   struct BayesTreeCliqueBaseUnordered {
 
-  public:
+  private:
     typedef BayesTreeCliqueBaseUnordered<DERIVED, FACTORGRAPH, BAYESNET> This;
     typedef DERIVED DerivedType;
-    typedef FACTORGRAPH FactorGraphType;
-    typedef BAYESNET BayesNetType;
-    typedef BayesNetType::ConditionalType ConditionalType;
-    typedef boost::shared_ptr<ConditionalType> sharedConditional;
     typedef boost::shared_ptr<This> shared_ptr;
     typedef boost::weak_ptr<This> weak_ptr;
     typedef boost::shared_ptr<DerivedType> derived_ptr;
     typedef boost::weak_ptr<DerivedType> derived_weak_ptr;
-    typedef FactorGraphType::FactorType FactorType;
+
+  public:
+    typedef FACTORGRAPH FactorGraphType;
+    typedef BAYESNET BayesNetType;
+    typedef typename BayesNetType::ConditionalType ConditionalType;
+    typedef boost::shared_ptr<ConditionalType> sharedConditional;
+    typedef typename FactorGraphType::FactorType FactorType;
+    typedef typename FactorGraphType::Eliminate Eliminate;
 
   protected:
 
@@ -87,8 +90,7 @@ namespace gtsam {
     }
 
     /** print this node */
-    void print(const std::string& s = "", const KeyFormatter& keyFormatter =
+    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
-        DefaultKeyFormatter) const;
 
     /// @}
     /// @name Standard Interface
@@ -113,14 +115,14 @@ namespace gtsam {
     /// @name Advanced Interface
     /// @{
 
-    /** return the conditional P(S|Root) on the separator given the root */
+    ///** return the conditional P(S|Root) on the separator given the root */
-    BayesNetType shortcut(derived_ptr root, Eliminate function) const;
+    //BayesNetType shortcut(derived_ptr root, Eliminate function) const;
 
-    /** return the marginal P(S) on the separator */
+    ///** return the marginal P(S) on the separator */
-    FactorGraphType separatorMarginal(derived_ptr root, Eliminate function) const;
+    //FactorGraphType separatorMarginal(derived_ptr root, Eliminate function) const;
 
-    /** return the marginal P(C) of the clique, using marginal caching */
+    ///** return the marginal P(C) of the clique, using marginal caching */
-    FactorGraphType marginal2(derived_ptr root, Eliminate function) const;
+    //FactorGraphType marginal2(derived_ptr root, Eliminate function) const;
 
     /**
      * This deletes the cached shortcuts of all cliques (subtree) below this clique.
@@ -146,30 +148,13 @@ namespace gtsam {
      * The factor graph p_Cp_B has keys from the parent clique Cp and from B.
      * But we only keep the variables not in S union B.
      */
-    std::vector<Key> shortcut_indices(derived_ptr B,
+    std::vector<Key> shortcut_indices(derived_ptr B, const FactorGraphType& p_Cp_B) const;
-        const FactorGraph<FactorType>& p_Cp_B) const;
 
     /** Non-recursive delete cached shortcuts and marginals - internal only. */
     void deleteCachedShortcutsNonRecursive() { cachedSeparatorMarginal_ = boost::none; }
 
   private:
 
-    /**
-     * Cliques cannot be copied except by the clone() method, which does not
-     * copy the parent and child pointers.
-     */
-    BayesTreeCliqueBaseUnordered(const This& other) {
-      assert(false);
-    }
-
-    /** Cliques cannot be copied except by the clone() method, which does not
-     * copy the parent and child pointers.
-     */
-    This& operator=(const This& other) {
-      assert(false);
-      return *this;
-    }
-
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>

gtsam/inference/BayesTreeCliqueDefault.h

@@ -40,13 +40,13 @@ namespace gtsam {
     public BayesTreeCliqueBaseUnordered<BayesTreeCliqueDefaultUnordered<FACTORGRAPH,BAYESNET>, FACTORGRAPH, BAYESNET> {
   public:
     typedef typename BAYESNET::ConditionalType ConditionalType;
+    typedef typename FACTORGRAPH::FactorType FactorType;
     typedef BayesTreeCliqueDefaultUnordered<FACTORGRAPH,BAYESNET> This;
     typedef BayesTreeCliqueBaseUnordered<This, FACTORGRAPH, BAYESNET> Base;
     typedef boost::shared_ptr<This> shared_ptr;
     typedef boost::weak_ptr<This> weak_ptr;
     BayesTreeCliqueDefaultUnordered() {}
-    BayesTreeCliqueDefaultUnordered(const typename ConditionalType::shared_ptr& conditional) : Base(conditional) {}
+    BayesTreeCliqueDefaultUnordered(const boost::shared_ptr<ConditionalType>& conditional) : Base(conditional) {}
-    BayesTreeCliqueDefaultUnordered(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
 
   private:
     /** Serialization function */

gtsam/inference/BayesTreeUnordered-inst.h

@@ -21,8 +21,10 @@
 #pragma once
 
 #include <gtsam/inference/BayesTreeUnordered.h>
+#include <gtsam/base/treeTraversal-inst.h>
 
 #include <boost/foreach.hpp>
+#include <fstream>
 
 namespace gtsam {
 
@@ -31,16 +33,17 @@ namespace gtsam {
   typename BayesTreeUnordered<CLIQUE>::CliqueData
   BayesTreeUnordered<CLIQUE>::getCliqueData() const {
     CliqueData data;
-    getCliqueData(data, root_);
+    BOOST_FOREACH(const sharedClique& root, roots_)
+      getCliqueData(data, root);
     return data;
   }
 
   /* ************************************************************************* */
   template<class CLIQUE>
   void BayesTreeUnordered<CLIQUE>::getCliqueData(CliqueData& data, sharedClique clique) const {
-    data.conditionalSizes.push_back((*clique)->nrFrontals());
+    data.conditionalSizes.push_back(clique->conditional()->nrFrontals());
-    data.separatorSizes.push_back((*clique)->nrParents());
+    data.separatorSizes.push_back(clique->conditional()->nrParents());
-    BOOST_FOREACH(sharedClique c, clique->children_) {
+    BOOST_FOREACH(sharedClique c, clique->children) {
       getCliqueData(data, c);
     }
   }
@@ -48,16 +51,20 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class CLIQUE>
   size_t BayesTreeUnordered<CLIQUE>::numCachedSeparatorMarginals() const {
-    return (root_) ? root_->numCachedSeparatorMarginals() : 0;
+    size_t count = 0;
+    BOOST_FOREACH(const sharedClique& root, roots_)
+      count += root->numCachedSeparatorMarginals();
+    return count;
   }
 
   /* ************************************************************************* */
   template<class CLIQUE>
   void BayesTreeUnordered<CLIQUE>::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const {
-    if (!root_.get()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
+    if (roots_.empty()) throw std::invalid_argument("the root of Bayes tree has not been initialized!");
     std::ofstream of(s.c_str());
     of<< "digraph G{\n";
-    saveGraph(of, root_, keyFormatter);
+    BOOST_FOREACH(const sharedClique& root, roots_)
+      saveGraph(of, root, keyFormatter);
     of<<"}";
     of.close();
   }
@@ -77,7 +84,7 @@ namespace gtsam {
       parent += indexFormatter(index);
     }
 
-    if( clique != root_){
+    if(clique->parent()){
       parent += " : ";
       s << parentnum << "->" << num << "\n";
     }
@@ -91,7 +98,7 @@ namespace gtsam {
     s << parent;
     parentnum = num;
 
-    BOOST_FOREACH(sharedClique c, clique->children_) {
+    BOOST_FOREACH(sharedClique c, clique->children) {
       num++;
       saveGraph(s, c, indexFormatter, parentnum);
     }
@@ -152,27 +159,24 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class CLIQUE>
-  typename BayesTreeUnordered<CLIQUE>::sharedClique
+  size_t BayesTreeUnordered<CLIQUE>::size() const {
-  BayesTreeUnordered<CLIQUE>::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) {
+    size_t size = 0;
-    sharedClique new_clique(new Clique(conditional));
+    BOOST_FOREACH(const sharedClique& clique, roots_)
-    addClique(new_clique, parent_clique);
+      size += clique->treeSize();
-    return new_clique;
+    return size;
   }
 
   /* ************************************************************************* */
   template<class CLIQUE>
   void BayesTreeUnordered<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
-    nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size()));
+    BOOST_FOREACH(Index j, clique->conditional()->frontals())
-    BOOST_FOREACH(Index j, (*clique)->frontals())
       nodes_[j] = clique;
     if (parent_clique != NULL) {
       clique->parent_ = parent_clique;
-      parent_clique->children_.push_back(clique);
+      parent_clique->children.push_back(clique);
     } else {
-      assert(!root_);
+      roots_.push_back(clique);
-      root_ = clique;
     }
-    clique->assertInvariants();
   }
 
   /* ************************************************************************* */
@@ -181,13 +185,11 @@ namespace gtsam {
       const sharedConditional& conditional, std::list<sharedClique>& child_cliques)
   {
     sharedClique new_clique(new Clique(conditional));
-    nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size()));
     BOOST_FOREACH(Index j, conditional->frontals())
       nodes_[j] = new_clique;
-    new_clique->children_ = child_cliques;
+    new_clique->children.assign(child_cliques.begin(), child_cliques.end());
     BOOST_FOREACH(sharedClique& child, child_cliques)
       child->parent_ = new_clique;
-    new_clique->assertInvariants();
     return new_clique;
   }
 
@@ -196,16 +198,16 @@ namespace gtsam {
   void BayesTreeUnordered<CLIQUE>::removeClique(sharedClique clique)
   {
     if (clique->isRoot())
-      root_.reset();
+      roots_.erase(std::find(roots_.begin(), roots_.end(), clique));
     else { // detach clique from parent
       sharedClique parent = clique->parent_.lock();
-      typename FastList<sharedClique>::iterator child = std::find(parent->children().begin(), parent->children().end(), clique);
+      typename std::vector<sharedClique>::iterator child = std::find(parent->children.begin(), parent->children.end(), clique);
-      assert(child != parent->children().end());
+      assert(child != parent->children.end());
-      parent->children().erase(child);
+      parent->children.erase(child);
     }
 
     // orphan my children
-    BOOST_FOREACH(sharedClique child, clique->children_)
+    BOOST_FOREACH(sharedClique child, clique->children)
       child->parent_ = typename Clique::weak_ptr();
 
     BOOST_FOREACH(Key j, clique->conditional()->frontals()) {
@@ -304,7 +306,9 @@ namespace gtsam {
   template<class CLIQUE>
   BayesTreeUnordered<CLIQUE>& BayesTreeUnordered<CLIQUE>::operator=(const This& other) {
     this->clear();
-    other.cloneTo(*this);
+    std::vector<sharedClique> clonedRoots = treeTraversal::CloneForest(other);
+    BOOST_FOREACH(const sharedClique& root, clonedRoots)
+      insertRoot(root);
     return *this;
   }
 
@@ -312,24 +316,25 @@ namespace gtsam {
   template<class CLIQUE>
   void BayesTreeUnordered<CLIQUE>::print(const std::string& s, const KeyFormatter& keyFormatter) const {
     std::cout << s << ": cliques: " << size() << ", variables: " << nodes_.size() << std::endl;
-    treeTraversal::PrintForest(*this, s, keyFormatter)
+    treeTraversal::PrintForest(*this, s, keyFormatter);
   }
 
   /* ************************************************************************* */
   // binary predicate to test equality of a pair for use in equals
   template<class CLIQUE>
   bool check_sharedCliques(
-      const typename BayesTreeUnordered<CLIQUE>::sharedClique& v1,
+      const std::pair<Key, typename BayesTreeUnordered<CLIQUE>::sharedClique>& v1,
-      const typename BayesTreeUnordered<CLIQUE>::sharedClique& v2
+      const std::pair<Key, typename BayesTreeUnordered<CLIQUE>::sharedClique>& v2
   ) {
-    return (!v1 && !v2) || (v1 && v2 && v1->equals(*v2));
+    return v1.first == v2.first &&
+      ((!v1.second && !v2.second) || (v1.second && v2.second && v1.second->equals(*v2.second)));
   }
 
   /* ************************************************************************* */
   template<class CLIQUE>
   bool BayesTreeUnordered<CLIQUE>::equals(const BayesTreeUnordered<CLIQUE>& other, double tol) const {
     return size()==other.size() &&
-      std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CONDITIONAL,CLIQUE>);
+      std::equal(nodes_.begin(), nodes_.end(), other.nodes_.begin(), &check_sharedCliques<CLIQUE>);
   }
 
   /* ************************************************************************* */
@@ -348,31 +353,15 @@ namespace gtsam {
     BOOST_FOREACH(const Key& j, subtree->conditional()->frontals()) { nodes_[j] = subtree; }
     // Fill index for each child
     typedef typename BayesTreeUnordered<CLIQUE>::sharedClique sharedClique;
-    BOOST_FOREACH(const sharedClique& child, subtree->children_) {
+    BOOST_FOREACH(const sharedClique& child, subtree->children) {
       fillNodesIndex(child); }
   }
 
   /* ************************************************************************* */
   template<class CLIQUE>
-  void BayesTreeUnordered<CLIQUE>::insert(const sharedClique& subtree) {
+  void BayesTreeUnordered<CLIQUE>::insertRoot(const sharedClique& subtree) {
-    if(subtree) {
+    roots_.push_back(subtree); // Add to roots
-      // Find the parent clique of the new subtree.  By the running intersection
+    fillNodesIndex(subtree); // Populate nodes index
-      // property, those separator variables in the subtree that are ordered
-      // lower than the highest frontal variable of the subtree root will all
-      // appear in the separator of the subtree root.
-      if(subtree->conditional()->parents().empty()) {
-        assert(!root_);
-        root_ = subtree;
-      } else {
-        Key parentRepresentative = findParentClique(subtree->conditional()->parents());
-        sharedClique parent = (*this)[parentRepresentative];
-        parent->children_ += subtree;
-        subtree->parent_ = parent; // set new parent!
-      }
-
-      // Now fill in the nodes index
-      fillNodesIndex(subtree);
-    }
   }
 
   /* ************************************************************************* */
@@ -576,7 +565,15 @@ namespace gtsam {
   void BayesTreeUnordered<CLIQUE>::clear() {
     // Remove all nodes and clear the root pointer
     nodes_.clear();
-    root_.reset();
+    roots_.clear();
+  }
+
+  /* ************************************************************************* */
+  template<class CLIQUE>
+  void BayesTreeUnordered<CLIQUE>::deleteCachedShortcuts() {
+    BOOST_FOREACH(const sharedClique& root, roots_) {
+      root->deleteCachedShortcuts();
+    }
   }
 
   /* ************************************************************************* */
@@ -596,7 +593,8 @@ namespace gtsam {
       this->removePath(typename Clique::shared_ptr(clique->parent_.lock()), bn, orphans);
 
       // add children to list of orphans (splice also removed them from clique->children_)
-      orphans.splice(orphans.begin(), clique->children_);
+      orphans.insert(orphans.begin(), clique->children.begin(), clique->children.end());
+      clique->children.clear();
 
       bn.push_back(clique->conditional());
 
@@ -638,15 +636,16 @@ namespace gtsam {
 
     // Remove the first clique from its parents
     if(!subtree->isRoot())
-      subtree->parent()->children().remove(subtree);
+      subtree->parent()->children.erase(std::find(
+      subtree->parent()->children.begin(), subtree->parent()->children.end(), subtree));
     else
-      root_.reset();
+      roots_.erase(std::find(roots_.begin(), roots_.end(), subtree));
 
     // Add all subtree cliques and erase the children and parent of each
     for(typename Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique)
     {
       // Add children
-      BOOST_FOREACH(const sharedClique& child, (*clique)->children()) {
+      BOOST_FOREACH(const sharedClique& child, (*clique)->children) {
         cliques.push_back(child); }
 
       // Delete cached shortcuts
@@ -658,7 +657,7 @@ namespace gtsam {
 
       // Erase the parent and children pointers
       (*clique)->parent_.reset();
-      (*clique)->children_.clear();
+      (*clique)->children.clear();
     }
 
     return cliques;

gtsam/inference/BayesTreeUnordered.h

@@ -47,6 +47,8 @@ namespace gtsam {
     typedef boost::shared_ptr<This> shared_ptr;
     typedef CLIQUE Clique; ///< The clique type, normally BayesTreeClique
     typedef boost::shared_ptr<Clique> sharedClique; ///< Shared pointer to a clique
+    typedef Clique Node; ///< Synonym for Clique (TODO: remove)
+    typedef sharedClique sharedNode; ///< Synonym for sharedClique (TODO: remove)
     typedef typename CLIQUE::ConditionalType ConditionalType;
     typedef boost::shared_ptr<ConditionalType> sharedConditional;
     typedef typename CLIQUE::BayesNetType BayesNetType;
@@ -128,12 +130,7 @@ namespace gtsam {
     /// @{
 
     /** number of cliques */
-    inline size_t size() const {
+    size_t size() const;
-      if(root_)
-        return root_->treeSize();
-      else
-        return 0;
-    }
 
     /** Check if there are any cliques in the tree */
     inline bool empty() const {
@@ -152,7 +149,7 @@ namespace gtsam {
       if(c == nodes_.end())
         throw std::out_of_range("Requested the BayesTree clique for a key that is not in the BayesTree");
       else
-        return *c;
+        return c->second;
     }
 
     /** Gather data on all cliques */
@@ -205,9 +202,7 @@ namespace gtsam {
     void clear();
 
     /** Clear all shortcut caches - use before timing on marginal calculation to avoid residual cache data */
-    void deleteCachedShortcuts() {
+    void deleteCachedShortcuts();
-      root_->deleteCachedShortcuts();
-    }
 
     /**
      * Remove path from clique to root and return that path as factors
@@ -226,14 +221,10 @@ namespace gtsam {
      * Remove the requested subtree. */
     Cliques removeSubtree(const sharedClique& subtree);
 
-    /**
+    /** Insert a new subtree with known parent clique.  This function does not check that the
-     * Hang a new subtree off of the existing tree.  This finds the appropriate
+     * specified parent is the correct parent.  This function updates all of the internal data
-     * parent clique for the subtree (which may be the root), and updates the
+     * structures associated with adding a subtree, such as populating the nodes index. */
-     * nodes index with the new cliques in the subtree.  None of the frontal
+    void insertRoot(const sharedClique& subtree);
-     * variables in the subtree may appear in the separators of the existing
-     * BayesTree.
-     */
-    void insert(const sharedClique& subtree);
 
     /**
      * Create a clone of this object as a shared pointer
@@ -255,9 +246,6 @@ namespace gtsam {
     /** remove a clique: warning, can result in a forest */
     void removeClique(sharedClique clique);
 
-    /** add a clique (top down) */
-    sharedClique addClique(const sharedConditional& conditional, const sharedClique& parent_clique = sharedClique());
-
     /** add a clique (top down) */
     void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique());
 

gtsam/inference/ConditionalUnordered.h

@@ -75,7 +75,8 @@ namespace gtsam {
     void print(const std::string& s = "Conditional", const KeyFormatter& formatter = DefaultKeyFormatter) const;
 
     /** check equality */
-    bool equals(const This& c, double tol = 1e-9) const { return nrFrontals_ == c.nrFrontals_; }
+    bool equals(const This& c, double tol = 1e-9) const {
+      return asFactor().equals(c.asFactor()) && nrFrontals_ == c.nrFrontals_; }
 
     /// @}
     /// @name Standard Interface

gtsam/inference/EliminateableFactorGraph-inst.h

@@ -35,11 +35,11 @@ namespace gtsam {
     std::pair<boost::shared_ptr<BAYESNET>, boost::shared_ptr<FACTORGRAPH> > result;
     if(ordering) {
       // Do elimination with given ordering
-      result = EliminationTreeType(*this, variableIndex, *ordering).eliminate(function);
+      result = ELIMINATIONTREE(asDerived(), variableIndex, *ordering).eliminate(function);
     } else {
       // Compute ordering
-      OrderingUnordered colamdOrdering = variableIndex.orderingCOLAMD();
+      OrderingUnordered colamdOrdering = OrderingUnordered::COLAMD(variableIndex);
-      result = EliminationTreeType(*this, variableIndex, colamdOrdering).eliminate(function);
+      result = ELIMINATIONTREE(asDerived(), variableIndex, colamdOrdering).eliminate(function);
     }
 
     // If any factors are remaining, the ordering was incomplete
@@ -62,11 +62,11 @@ namespace gtsam {
     std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<FACTORGRAPH> > result;
     if(ordering) {
       // Do elimination with given ordering
-      result = JunctionTreeType(*this, variableIndex, *ordering).eliminate(function);
+      result = JUNCTIONTREE(ELIMINATIONTREE(asDerived(), variableIndex, *ordering)).eliminate(function);
     } else {
       // Compute ordering
-      OrderingUnordered colamdOrdering = variableIndex.orderingCOLAMD();
+      OrderingUnordered colamdOrdering = OrderingUnordered::COLAMD(variableIndex);
-      result = JunctionTreeType(*this, variableIndex, colamdOrdering).eliminate(function);
+      result = JUNCTIONTREE(ELIMINATIONTREE(asDerived(), variableIndex, colamdOrdering)).eliminate(function);
     }
 
     // If any factors are remaining, the ordering was incomplete
@@ -76,4 +76,5 @@ namespace gtsam {
     // Return the Bayes tree
     return result.first;
   }
+
 }

gtsam/inference/EliminateableFactorGraph.h

@@ -121,6 +121,13 @@ namespace gtsam {
       eliminatePartialMultifrontal(const Eliminate& function, const std::vector<Key>& variables,
       const VariableIndexUnordered& variableIndex = VariableIndexUnordered(*this));
 
+  private:
+
+    // Access the derived factor graph class
+    const FACTORGRAPH& asDerived() const { return static_cast<const FACTORGRAPH&>(*this); }
+
+    // Access the derived factor graph class
+    FACTORGRAPH& asDerived() { return static_cast<FACTORGRAPH&>(*this); }
   };
 
 }

gtsam/inference/EliminationTreeUnordered-inst.h

@@ -26,6 +26,7 @@
 #include <gtsam/base/treeTraversal-inst.h>
 #include <gtsam/inference/EliminationTreeUnordered.h>
 #include <gtsam/inference/VariableIndexUnordered.h>
+#include <gtsam/inference/OrderingUnordered.h>
 #include <gtsam/inference/inference-inst.h>
 
 namespace gtsam {
@@ -64,8 +65,8 @@ namespace gtsam {
   void EliminationTreeUnordered<BAYESNET,GRAPH>::Node::print(
     const std::string& str, const KeyFormatter& keyFormatter) const
   {
-    std::cout << str << "(" << formatter(node->key) << ")\n";
+    std::cout << str << "(" << keyFormatter(key) << ")\n";
-    BOOST_FOREACH(const typename ETREE::sharedFactor& factor, node->factors) {
+    BOOST_FOREACH(const sharedFactor& factor, factors) {
       if(factor)
         factor->print(str + "| ");
       else
@@ -77,7 +78,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class BAYESNET, class GRAPH>
   EliminationTreeUnordered<BAYESNET,GRAPH>::EliminationTreeUnordered(const FactorGraphType& graph,
-    const VariableIndexUnordered& structure, const std::vector<Key>& order)
+    const VariableIndexUnordered& structure, const OrderingUnordered& order)
   {
     gttic(ET_Create1);
 
@@ -156,7 +157,7 @@ namespace gtsam {
   /* ************************************************************************* */
   template<class BAYESNET, class GRAPH>
   EliminationTreeUnordered<BAYESNET,GRAPH>::EliminationTreeUnordered(
-    const FactorGraphType& factorGraph, const std::vector<Key>& order)
+    const FactorGraphType& factorGraph, const OrderingUnordered& order)
   {
     gttic(ET_Create2);
     // Build variable index first

gtsam/inference/EliminationTreeUnordered.h

@@ -27,6 +27,7 @@ class EliminationTreeUnorderedTester; // for unit tests, see testEliminationTree
 namespace gtsam {
 
   class VariableIndexUnordered;
+  class OrderingUnordered;
 
   /**
   * An elimination tree is a data structure used intermediately during
@@ -98,14 +99,14 @@ namespace gtsam {
     * @return The elimination tree
     */
     EliminationTreeUnordered(const FactorGraphType& factorGraph,
-      const VariableIndexUnordered& structure, const std::vector<Key>& order);
+      const VariableIndexUnordered& structure, const OrderingUnordered& order);
 
     /** Build the elimination tree of a factor graph.  Note that this has to compute the column
     * structure as a VariableIndex, so if you already have this precomputed, use the other
     * constructor instead.
     * @param factorGraph The factor graph for which to build the elimination tree
     */
-    EliminationTreeUnordered(const FactorGraphType& factorGraph, const std::vector<Key>& order);
+    EliminationTreeUnordered(const FactorGraphType& factorGraph, const OrderingUnordered& order);
 
     /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
      *  copied, factors are not cloned. */

gtsam/inference/FactorGraphUnordered.h

@@ -60,7 +60,7 @@ namespace gtsam {
 
   public:
 
-    /// @name Standard Constructor
+    /// @name Standard Constructors
     /// @{
 
     /** Default constructor */

gtsam/inference/JunctionTreeUnordered-inst.h

@@ -26,47 +26,19 @@
 #include <gtsam/symbolic/SymbolicConditionalUnordered.h>
 
 #include <boost/foreach.hpp>
+#include <boost/bind.hpp>
 
 namespace gtsam {
   
-  /* ************************************************************************* */
-  template<class BAYESTREE, class GRAPH>
-  typename JunctionTreeUnordered<BAYESTREE,GRAPH>::sharedFactor
-    JunctionTreeUnordered<BAYESTREE,GRAPH>::Node::eliminate(
-    const boost::shared_ptr<BayesTreeType>& output,
-    const Eliminate& function, const std::vector<sharedFactor>& childrenResults) const
-  {
-    // This function eliminates one node (Node::eliminate) - see below eliminate for the whole tree.
-
-    assert(childrenResults.size() == children.size());
-
-    // Gather factors
-    std::vector<sharedFactor> gatheredFactors;
-    gatheredFactors.reserve(factors.size() + children.size());
-    gatheredFactors.insert(gatheredFactors.end(), factors.begin(), factors.end());
-    gatheredFactors.insert(gatheredFactors.end(), childrenResults.begin(), childrenResults.end());
-
-    // Do dense elimination step
-    std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
-      function(gatheredFactors, keys);
-
-    // Add conditional to BayesNet
-    output->push_back(eliminationResult.first);
-
-    // Return result
-    return eliminationResult.second;
-  }
-
-
   namespace {
     /* ************************************************************************* */
     template<class BAYESTREE, class GRAPH>
     struct ConstructorTraversalData {
-      const ConstructorTraversalData* parentData;
+      ConstructorTraversalData* const parentData;
       typename JunctionTreeUnordered<BAYESTREE,GRAPH>::sharedNode myJTNode;
       std::vector<SymbolicConditionalUnordered::shared_ptr> childSymbolicConditionals;
       std::vector<SymbolicFactorUnordered::shared_ptr> childSymbolicFactors;
-      ConstructorTraversalData(const ConstructorTraversalData* _parentData) : parentData(_parentData) {}
+      ConstructorTraversalData(ConstructorTraversalData* _parentData) : parentData(_parentData) {}
     };
 
     /* ************************************************************************* */
@@ -74,12 +46,12 @@ namespace gtsam {
     template<class BAYESTREE, class GRAPH, class ETREE_NODE>
     ConstructorTraversalData<BAYESTREE,GRAPH> ConstructorTraversalVisitorPre(
       const boost::shared_ptr<ETREE_NODE>& node,
-      const ConstructorTraversalData<BAYESTREE,GRAPH>& parentData)
+      ConstructorTraversalData<BAYESTREE,GRAPH>& parentData)
     {
       // On the pre-order pass, before children have been visited, we just set up a traversal data
       // structure with its own JT node, and create a child pointer in its parent.
       ConstructorTraversalData<BAYESTREE,GRAPH> myData = ConstructorTraversalData<BAYESTREE,GRAPH>(&parentData);
-      myData.myJTNode = boost::make_shared<typename JunctionTreeUnordered<BAYESREE,GRAPH>::Node>();
+      myData.myJTNode = boost::make_shared<typename JunctionTreeUnordered<BAYESTREE,GRAPH>::Node>();
       myData.myJTNode->keys.push_back(node->key);
       myData.myJTNode->factors.insert(myData.myJTNode->factors.begin(), node->factors.begin(), node->factors.end());
       parentData.myJTNode->children.push_back(myData.myJTNode);
@@ -93,17 +65,27 @@ namespace gtsam {
       const boost::shared_ptr<ETREE_NODE>& node,
       const ConstructorTraversalData<BAYESTREE,GRAPH>& myData)
     {
+      // In this post-order visitor, we combine the symbolic elimination results from the
+      // elimination tree children and symbolically eliminate the current elimination tree node.  We
+      // then check whether each of our elimination tree child nodes should be merged with us.  The
+      // check for this is that our number of symbolic elimination parents is exactly 1 less than
+      // our child's symbolic elimination parents - this condition indicates that eliminating the
+      // current node did not introduce any parents beyond those already in the child.
+
       // Do symbolic elimination for this node
       std::vector<SymbolicFactorUnordered::shared_ptr> symbolicFactors;
       symbolicFactors.reserve(node->factors.size() + myData.childSymbolicFactors.size());
+      // Add symbolic versions of the ETree node factors
       BOOST_FOREACH(const typename GRAPH::sharedFactor& factor, node->factors) {
-        symbolicFactors.push_back(boost::make_shared<SymbolicFactorUnordered>(factor)); }
+        symbolicFactors.push_back(boost::make_shared<SymbolicFactorUnordered>(
+          SymbolicFactorUnordered::FromKeys(*factor))); }
+      // Add symbolic factors passed up from children
       symbolicFactors.insert(symbolicFactors.end(), myData.childSymbolicFactors.begin(), myData.childSymbolicFactors.end());
       std::vector<Key> keyAsVector(1); keyAsVector[0] = node->key;
       std::pair<SymbolicConditionalUnordered::shared_ptr, SymbolicFactorUnordered::shared_ptr> symbolicElimResult =
         EliminateSymbolicUnordered(symbolicFactors, keyAsVector);
 
-      // Store symbolic elimination results
+      // Store symbolic elimination results in the parent
       myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first);
       myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second);
 
@@ -118,7 +100,7 @@ namespace gtsam {
         if(myNrParents + 1 == myData.childSymbolicConditionals[child]->nrParents()) {
           // Get a reference to the child, adjusting the index to account for children previously
           // merged and removed from the child list.
-          const typename JunctionTreeUnordered<BAYESREE,GRAPH>::Node& childToMerge =
+          const typename JunctionTreeUnordered<BAYESTREE,GRAPH>::Node& childToMerge =
             *myData.myJTNode->children[child - nrMergedChildren];
           // Merge keys, factors, and children.
           myData.myJTNode->keys.insert(myData.myJTNode->keys.end(), childToMerge.keys.begin(), childToMerge.keys.end());
@@ -129,12 +111,77 @@ namespace gtsam {
         }
       }
     }
+
+    /* ************************************************************************* */
+    // Elimination traversal data - stores a pointer to the parent data and collects the factors
+    // resulting from elimination of the children.  Also sets up BayesTree cliques with parent and
+    // child pointers.
+    template<class JUNCTIONTREE>
+    struct EliminationData {
+      EliminationData* const parentData;
+      std::vector<typename JUNCTIONTREE::sharedFactor> childFactors;
+      boost::shared_ptr<typename JUNCTIONTREE::BayesTreeType::Node> bayesTreeNode;
+      EliminationData(EliminationData* _parentData, size_t nChildren) :
+        parentData(_parentData),
+        bayesTreeNode(boost::make_shared<typename JUNCTIONTREE::BayesTreeType::Node>()) {
+          childFactors.reserve(nChildren);
+          // Set up BayesTree parent and child pointers
+          if(parentData) {
+            bayesTreeNode->parent_ = parentData->bayesTreeNode;
+            parentData->bayesTreeNode->children.push_back(bayesTreeNode);
+          }
+      }
+    };
+
+    /* ************************************************************************* */
+    // Elimination pre-order visitor - just creates the EliminationData structure for the visited
+    // node.
+    template<class JUNCTIONTREE>
+    EliminationData<JUNCTIONTREE> eliminationPreOrderVisitor(
+      const typename JUNCTIONTREE::sharedNode& node, EliminationData<JUNCTIONTREE>& parentData)
+    {
+      return EliminationData<JUNCTIONTREE>(&parentData, node->children.size());
+    }
+
+    /* ************************************************************************* */
+    // Elimination post-order visitor - combine the child factors with our own factors, add the
+    // resulting conditional to the BayesTree, and add the remaining factor to the parent.  The
+    // extra argument 'eliminationFunction' is passed from JunctionTree::eliminate using
+    // boost::bind.
+    template<class JUNCTIONTREE>
+    void eliminationPostOrderVisitor(const typename JUNCTIONTREE::sharedNode& node, EliminationData<JUNCTIONTREE>& myData,
+      const typename JUNCTIONTREE::Eliminate& eliminationFunction)
+    {
+      // Typedefs
+      typedef typename JUNCTIONTREE::sharedFactor sharedFactor;
+      typedef typename JUNCTIONTREE::FactorType FactorType;
+      typedef typename JUNCTIONTREE::ConditionalType ConditionalType;
+      typedef typename JUNCTIONTREE::BayesTreeType::Node BTNode;
+
+      // Gather factors
+      assert(node->children.size() == myData.childFactors.size());
+      std::vector<sharedFactor> gatheredFactors;
+      gatheredFactors.reserve(node->factors.size() + node->children.size());
+      gatheredFactors.insert(gatheredFactors.end(), node->factors.begin(), node->factors.end());
+      gatheredFactors.insert(gatheredFactors.end(), myData.childFactors.begin(), myData.childFactors.end());
+
+      // Do dense elimination step
+      std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
+        eliminationFunction(gatheredFactors, node->keys);
+
+      // Store conditional in BayesTree clique
+      myData.bayesTreeNode->conditional_ = eliminationResult.first;
+
+      // Store remaining factor in parent's gathered factors
+      myData.parentData->childFactors.push_back(eliminationResult.second);
+    }
   }
 
   /* ************************************************************************* */
   template<class BAYESTREE, class GRAPH>
   template<class ETREE>
-  JunctionTreeUnordered<BAYESTREE,GRAPH>::JunctionTreeUnordered(const ETREE& eliminationTree)
+  JunctionTreeUnordered<BAYESTREE,GRAPH>
+    JunctionTreeUnordered<BAYESTREE,GRAPH>::FromEliminationTree(const ETREE& eliminationTree)
   {
     // Here we rely on the BayesNet having been produced by this elimination tree, such that the
     // conditionals are arranged in DFS post-order.  We traverse the elimination tree, and inspect
@@ -144,16 +191,20 @@ namespace gtsam {
 
     // Traverse the elimination tree, doing symbolic elimination and merging nodes as we go.  Gather
     // the created junction tree roots in a dummy Node.
+    typedef typename ETREE::Node ETreeNode;
     ConstructorTraversalData<BAYESTREE, GRAPH> rootData(0);
     rootData.myJTNode = boost::make_shared<Node>(); // Make a dummy node to gather the junction tree roots
-    treeTraversal.DepthFirstForest(eliminationTree, rootData,
+    treeTraversal::DepthFirstForest(eliminationTree, rootData,
-      ConstructorTraversalVisitorPre<BAYESREE,GRAPH>, ConstructorTraversalVisitorPost<BAYESTREE,GRAPH>);
+      ConstructorTraversalVisitorPre<BAYESTREE,GRAPH,ETreeNode>, ConstructorTraversalVisitorPost<BAYESTREE,GRAPH,ETreeNode>);
 
     // Assign roots from the dummy node
-    roots_ = rootData.myJTNode->children;
+    This result;
+    result.roots_ = rootData.myJTNode->children;
 
     // Transfer remaining factors from elimination tree
-    remainingFactors_ = eliminationTree.remainingFactors();
+    result.remainingFactors_ = eliminationTree.remainingFactors();
+
+    return result;
   }
 
   /* ************************************************************************* */
@@ -175,16 +226,22 @@ namespace gtsam {
   std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> >
     JunctionTreeUnordered<BAYESTREE,GRAPH>::eliminate(const Eliminate& function) const
   {
-    // Allocate result
+    // Do elimination (depth-first traversal).  The rootsContainer stores a 'dummy' BayesTree node
-    boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
+    // that contains all of the roots as its children.  rootsContainer also stores the remaining
+    // uneliminated factors passed up from the roots.
+    EliminationData<This> rootsContainer(0, roots_.size());
+    treeTraversal::DepthFirstForest(*this, rootsContainer, eliminationPreOrderVisitor<This>,
+      boost::bind(eliminationPostOrderVisitor<This>, _1, _2, function));
 
-    // Run tree elimination algorithm
+    // Create BayesTree from roots stored in the dummy BayesTree node.
-    std::vector<sharedFactor> remainingFactors = inference::EliminateTree(result, *this, function);
+    boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
+    BOOST_FOREACH(const typename BayesTreeType::sharedNode& root, rootsContainer.bayesTreeNode->children)
+      result->insertRoot(root);
 
     // Add remaining factors that were not involved with eliminated variables
     boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>();
     allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end());
-    allRemainingFactors->push_back(remainingFactors.begin(), remainingFactors.end());
+    allRemainingFactors->push_back(rootsContainer.childFactors.begin(), rootsContainer.childFactors.end());
 
     // Return result
     return std::make_pair(result, allRemainingFactors);

gtsam/inference/JunctionTreeUnordered.h

@@ -71,9 +71,6 @@ namespace gtsam {
       Keys keys; ///< Frontal keys of this node
       Factors factors; ///< Factors associated with this node
       Children children; ///< sub-trees
-
-      sharedFactor eliminate(const boost::shared_ptr<BayesTreeType>& output,
-        const Eliminate& function, const std::vector<sharedFactor>& childrenFactors) const;
     };
 
     typedef boost::shared_ptr<Node> sharedNode; ///< Shared pointer to Node
@@ -83,17 +80,17 @@ namespace gtsam {
     /** concept check */
     GTSAM_CONCEPT_TESTABLE_TYPE(FactorType);
 
-    std::vector<sharedNode>& roots_;
+    std::vector<sharedNode> roots_;
     std::vector<sharedFactor> remainingFactors_;
 
-  public:
+  protected:
 
     /// @name Standard Constructors
     /// @{
 
     /** Build the junction tree from an elimination tree. */
     template<class ETREE>
-    JunctionTreeUnordered(const ETREE& eliminationTree);
+    static This FromEliminationTree(const ETREE& eliminationTree);
     
     /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
      *  copied, factors are not cloned. */
@@ -105,6 +102,8 @@ namespace gtsam {
 
     /// @}
 
+  public:
+
     /// @name Standard Interface
     /// @{
 
@@ -129,6 +128,11 @@ namespace gtsam {
 
     /// @}
 
+  private:
+
+    // Private default constructor (used in static construction methods)
+    JunctionTreeUnordered() {}
+
   };
 
 }

gtsam/inference/OrderingUnordered.cpp

@@ -0,0 +1,95 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    Ordering.cpp
+ * @author  Richard Roberts
+ * @date    Sep 2, 2010
+ */
+
+#pragma once
+
+#include <vector>
+#include <limits>
+
+#include <gtsam/inference/OrderingUnordered.h>
+#include <gtsam/3rdparty/CCOLAMD/Include/ccolamd.h>
+
+using namespace std;
+
+namespace gtsam {
+
+  /* ************************************************************************* */
+  OrderingUnordered OrderingUnordered::COLAMD(const VariableIndexUnordered& variableIndex)
+  {
+    gttic(OrderingUnordered_COLAMD);
+
+    gttic(Prepare);
+    size_t nEntries = variableIndex.nEntries(), nFactors = variableIndex.nFactors(), nVars = variableIndex.size();
+    // Convert to compressed column major format colamd wants it in (== MATLAB format!)
+    size_t Alen = ccolamd_recommended((int)nEntries, (int)nFactors, (int)nVars); /* colamd arg 3: size of the array A */
+    vector<int> A = vector<int>(Alen); /* colamd arg 4: row indices of A, of size Alen */
+    vector<int> p = vector<int>(nVars + 1); /* colamd arg 5: column pointers of A, of size n_col+1 */
+    vector<int> cmember(nVars, 0); // For now, no constraints
+
+    // Fill in input data for COLAMD
+    p[0] = 0;
+    int count = 0;
+    vector<Key> keys(nVars); // Array to store the keys in the order we add them so we can retrieve them in permuted order
+    size_t index = 0;
+    BOOST_FOREACH(const VariableIndexUnordered::const_iterator::value_type key_factors, variableIndex) {
+      // Arrange factor indices into COLAMD format
+      const VariableIndexUnordered::Factors& column = key_factors.second;
+      size_t lastFactorId = numeric_limits<size_t>::max();
+      BOOST_FOREACH(size_t factorIndex, column) {
+        if(lastFactorId != numeric_limits<size_t>::max())
+          assert(factorIndex > lastFactorId);
+        A[count++] = (int)factorIndex; // copy sparse column
+      }
+      p[index+1] = count; // column j (base 1) goes from A[j-1] to A[j]-1
+      // Store key in array and increment index
+      keys[index] = key_factors.first;
+      ++ index;
+    }
+
+    assert((size_t)count == variableIndex.nEntries());
+
+    //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
+    double knobs[CCOLAMD_KNOBS];
+    ccolamd_set_defaults(knobs);
+    knobs[CCOLAMD_DENSE_ROW]=-1;
+    knobs[CCOLAMD_DENSE_COL]=-1;
+
+    int stats[CCOLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
+
+    gttoc(Prepare);
+
+    // call colamd, result will be in p
+    /* returns (1) if successful, (0) otherwise*/
+    if(nVars > 0) {
+      gttic(ccolamd);
+      int rv = ccolamd((int)nFactors, (int)nVars, (int)Alen, &A[0], &p[0], knobs, stats, &cmember[0]);
+      if(rv != 1)
+        throw runtime_error((boost::format("ccolamd failed with return value %1%")%rv).str());
+    }
+
+    //  ccolamd_report(stats);
+
+    gttic(Fill_Ordering);
+    // Convert elimination ordering in p to an ordering
+    OrderingUnordered result;
+    result.assign(p.begin(), p.end() - 1);
+    gttoc(Fill_Ordering);
+
+    return result;
+  }
+
+}

gtsam/inference/OrderingUnordered.h

@@ -20,10 +20,23 @@
 #include <vector>
 
 #include <gtsam/inference/Key.h>
+#include <gtsam/inference/VariableIndexUnordered.h>
+#include <gtsam/inference/FactorGraphUnordered.h>
 
 namespace gtsam {
-  class OrderingUnordered : std::vector<Key> {
+  class OrderingUnordered : public std::vector<Key> {
   public:
-    OrderingUnordered() {}
+    /// Create an empty ordering
+    GTSAM_EXPORT OrderingUnordered() {}
+
+    /// Compute an ordering using COLAMD directly from a factor graph - this internally builds a
+    /// VariableIndex so if you already have a VariableIndex, it is faster to use COLAMD(const
+    /// VariableIndexUnordered&)
+    template<class FACTOR>
+    static OrderingUnordered COLAMD(const FactorGraphUnordered<FACTOR>& graph) {
+      return COLAMD(VariableIndexUnordered(graph));
+    }
+
+    static GTSAM_EXPORT OrderingUnordered COLAMD(const VariableIndexUnordered& variableIndex);
   };
 }

gtsam/inference/inference-inst.h

@@ -20,6 +20,7 @@
 #pragma once
 
 #include <boost/shared_ptr.hpp>
+#include <boost/bind.hpp>
 #include <utility>
 
 #include <gtsam/base/treeTraversal-inst.h>
@@ -49,11 +50,11 @@ namespace gtsam {
 
       /* ************************************************************************* */
       template<class TREE, class RESULT>
-      void eliminationPostOrderVisitor(const typename TREE::Node& const node, EliminationData<TREE>& myData,
+      void eliminationPostOrderVisitor(const typename TREE::sharedNode& node, EliminationData<TREE>& myData,
         RESULT& result, const typename TREE::Eliminate& eliminationFunction)
       {
         // Call eliminate on the node and add the result to the parent's gathered factors
-        myData.parentData->childFactors.push_back(node.eliminate(result, eliminationFunction, myData.childFactors));
+        myData.parentData->childFactors.push_back(node->eliminate(result, eliminationFunction, myData.childFactors));
       }
     }
 

gtsam/symbolic/SymbolicBayesNetUnordered.cpp

@@ -16,6 +16,7 @@
  * @author Richard Roberts
  */
 
+#include <gtsam/inference/BayesNetUnordered-inst.h>
 #include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
 
 namespace gtsam {

gtsam/symbolic/SymbolicBayesNetUnordered.h

@@ -27,7 +27,7 @@ namespace gtsam {
   /** Symbolic Bayes Net
    *  \nosubgrouping
    */
-  class SymbolicBayesNetUnordered: public BayesNetUnordered<SymbolicConditionalUnordered> {
+  class GTSAM_EXPORT SymbolicBayesNetUnordered: public BayesNetUnordered<SymbolicConditionalUnordered> {
 
   public:
 

gtsam/symbolic/SymbolicBayesTreeUnordered.cpp

@@ -19,80 +19,86 @@
 #include <boost/foreach.hpp>
 
 #include <gtsam/symbolic/SymbolicBayesTreeUnordered.h>
+#include <gtsam/inference/BayesTreeUnordered-inst.h>
+#include <gtsam/inference/BayesTreeCliqueBaseUnordered-inst.h>
 
 namespace gtsam {
 
-  /* ************************************************************************* */
+  void SymbolicBayesTreeUnordered::noop() const {
-  void SymbolicBayesTreeUnordered::insert(const sharedConditional& conditional)
-  {
-    static const bool debug = false;
 
-    // get indices and parents
-    const typename CONDITIONAL::Parents& parents = conditional->parents();
-
-    if(debug) conditional->print("Adding conditional ");
-
-    // if no parents, start a new root clique
-    if (parents.empty()) {
-      if(debug) std::cout << "No parents so making root" << std::endl;
-      bayesTree.root_ = bayesTree.addClique(conditional);
-      return;
-    }
-
-    // otherwise, find the parent clique by using the index data structure
-    // to find the lowest-ordered parent
-    Index parentRepresentative = bayesTree.findParentClique(parents);
-    if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl;
-    sharedClique parent_clique = bayesTree[parentRepresentative];
-    if(debug) parent_clique->print("Parent clique is ");
-
-    // if the parents and parent clique have the same size, add to parent clique
-    if ((*parent_clique)->size() == size_t(parents.size())) {
-      if(debug) std::cout << "Adding to parent clique" << std::endl;
-#ifndef NDEBUG
-      // Debug check that the parent indices of the new conditional match the indices
-      // currently in the clique.
-      //      list<Index>::const_iterator parent = parents.begin();
-      //      typename Clique::const_iterator cond = parent_clique->begin();
-      //      while(parent != parents.end()) {
-      //        assert(cond != parent_clique->end());
-      //        assert(*parent == (*cond)->key());
-      //        ++ parent;
-      //        ++ cond;
-      //      }
-#endif
-      addToCliqueFront(bayesTree, conditional, parent_clique);
-    } else {
-      if(debug) std::cout << "Starting new clique" << std::endl;
-      // otherwise, start a new clique and add it to the tree
-      bayesTree.addClique(conditional,parent_clique);
-    }
   }
 
-  /* ************************************************************************* */
+//  /* ************************************************************************* */
-  void SymbolicBayesTreeUnordered::addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique) {
+//  void SymbolicBayesTreeUnordered::insert(const sharedConditional& conditional)
-    static const bool debug = false;
+//  {
-#ifndef NDEBUG
+//    static const bool debug = false;
-    // Debug check to make sure the conditional variable is ordered lower than
+//
-    // its parents and that all of its parents are present either in this
+//    // get indices and parents
-    // clique or its separator.
+//    const SymbolicConditionalUnordered::Parents& parents = conditional->parents();
-    BOOST_FOREACH(Key parent, conditional->parents()) {
+//
-      assert(parent > conditional->lastFrontalKey());
+//    if(debug) conditional->print("Adding conditional ");
-      const Clique& cliquer(*clique);
+//
-      assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end());
+//    // if no parents, start a new root clique
-    }
+//    if (parents.empty()) {
-#endif
+//      if(debug) std::cout << "No parents so making root" << std::endl;
-    if(debug) conditional->print("Adding conditional ");
+//      addClique(boost::make_shared<SymbolicBayesTreeCliqueUnordered>(conditional));
-    if(debug) clique->print("To clique ");
+//      return;
-    Index j = conditional->lastFrontalKey();
+//    }
-    this->nodes_.resize(std::max(j+1, this->nodes_.size()));
+//
-    this->nodes_[j] = clique;
+//    // otherwise, find the parent clique by using the index data structure
-    FastVector<Index> newIndices(clique->conditional()->size() + 1);
+//    // to find the lowest-ordered parent
-    newIndices[0] = j;
+//    Index parentRepresentative = findParentClique(parents);
-    std::copy(clique->conditional()->begin(), clique->conditional()->end(), newIndices.begin()+1);
+//    if(debug) std::cout << "First-eliminated parent is " << parentRepresentative << ", have " << bayesTree.nodes_.size() << " nodes." << std::endl;
-    clique->conditional_ = ConditionalType::FromKeys(newIndices, (*clique)->nrFrontals() + 1);
+//    sharedClique parent_clique = bayesTree[parentRepresentative];
-    if(debug) clique->print("Expanded clique is ");
+//    if(debug) parent_clique->print("Parent clique is ");
-    clique->assertInvariants();
+//
-  }
+//    // if the parents and parent clique have the same size, add to parent clique
+//    if ((*parent_clique)->size() == size_t(parents.size())) {
+//      if(debug) std::cout << "Adding to parent clique" << std::endl;
+//#ifndef NDEBUG
+//      // Debug check that the parent indices of the new conditional match the indices
+//      // currently in the clique.
+//      //      list<Index>::const_iterator parent = parents.begin();
+//      //      typename Clique::const_iterator cond = parent_clique->begin();
+//      //      while(parent != parents.end()) {
+//      //        assert(cond != parent_clique->end());
+//      //        assert(*parent == (*cond)->key());
+//      //        ++ parent;
+//      //        ++ cond;
+//      //      }
+//#endif
+//      addToCliqueFront(bayesTree, conditional, parent_clique);
+//    } else {
+//      if(debug) std::cout << "Starting new clique" << std::endl;
+//      // otherwise, start a new clique and add it to the tree
+//      bayesTree.addClique(conditional,parent_clique);
+//    }
+//  }
+//
+//  /* ************************************************************************* */
+//  void SymbolicBayesTreeUnordered::addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique) {
+//    static const bool debug = false;
+//#ifndef NDEBUG
+//    // Debug check to make sure the conditional variable is ordered lower than
+//    // its parents and that all of its parents are present either in this
+//    // clique or its separator.
+//    BOOST_FOREACH(Key parent, conditional->parents()) {
+//      assert(parent > conditional->lastFrontalKey());
+//      const Clique& cliquer(*clique);
+//      assert(find(cliquer->begin(), cliquer->end(), parent) != cliquer->end());
+//    }
+//#endif
+//    if(debug) conditional->print("Adding conditional ");
+//    if(debug) clique->print("To clique ");
+//    Index j = conditional->lastFrontalKey();
+//    this->nodes_.resize(std::max(j+1, this->nodes_.size()));
+//    this->nodes_[j] = clique;
+//    FastVector<Index> newIndices(clique->conditional()->size() + 1);
+//    newIndices[0] = j;
+//    std::copy(clique->conditional()->begin(), clique->conditional()->end(), newIndices.begin()+1);
+//    clique->conditional_ = ConditionalType::FromKeys(newIndices, (*clique)->nrFrontals() + 1);
+//    if(debug) clique->print("Expanded clique is ");
+//    clique->assertInvariants();
+//  }
 
 }

gtsam/symbolic/SymbolicBayesTreeUnordered.h

@@ -19,18 +19,33 @@
 #pragma once
 
 #include <gtsam/inference/BayesTreeUnordered.h>
-#include <gtsam/inference/BayesTreeCliqueBaseUnordered.h>
+#include <gtsam/inference/BayesTreeCliqueDefault.h>
-#include <gtsam/symbolic/SymbolicConditionalUnordered.h>
+#include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
+#include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
 
 namespace gtsam {
 
-  class SymbolicBayesTreeUnordered :
+  /* ************************************************************************* */
-    public BayesTreeUnordered<BayesTreeCliqueBaseUnordered<>
+  class GTSAM_EXPORT SymbolicBayesTreeCliqueUnordered :
+    public BayesTreeCliqueBaseUnordered<SymbolicBayesTreeCliqueUnordered, SymbolicFactorGraphUnordered, SymbolicBayesNetUnordered>
+  {
+  public:
+    typedef SymbolicBayesTreeCliqueUnordered This;
+    typedef BayesTreeCliqueBaseUnordered<SymbolicBayesTreeCliqueUnordered, SymbolicFactorGraphUnordered, SymbolicBayesNetUnordered> Base;
+    typedef boost::shared_ptr<This> shared_ptr;
+    typedef boost::weak_ptr<This> weak_ptr;
+    SymbolicBayesTreeCliqueUnordered() {}
+    SymbolicBayesTreeCliqueUnordered(const SymbolicConditionalUnordered::shared_ptr& conditional) : Base(conditional) {}
+  };
+
+  /* ************************************************************************* */
+  class GTSAM_EXPORT SymbolicBayesTreeUnordered :
+    public BayesTreeUnordered<SymbolicBayesTreeCliqueUnordered>
   {
 
   public:
     /** Insert a new conditional */
-    void insert(const sharedConditional& conditional);
+    //void insert(const sharedConditional& conditional);
 
   protected:
     
@@ -39,7 +54,12 @@ namespace gtsam {
      * parents are already in the clique or its separators.  This function does
      * not check for this condition, it just updates the data structures.
      */
-    void addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique);
+    //void addToCliqueFront(const sharedConditional& conditional, const sharedClique& clique);
+
+  private:
+
+    // Dummy method to export class
+    void noop() const;
 
   };
 

gtsam/symbolic/SymbolicConditionalUnordered.h

@@ -50,16 +50,16 @@ namespace gtsam {
     SymbolicConditionalUnordered() {}
 
     /** No parents */
-    SymbolicConditionalUnordered(Index j) : BaseFactor(j), BaseConditional(0) {}
+    SymbolicConditionalUnordered(Index j) : BaseFactor(j), BaseConditional(1) {}
 
     /** Single parent */
     SymbolicConditionalUnordered(Index j, Index parent) : BaseFactor(j, parent), BaseConditional(1) {}
 
     /** Two parents */
-    SymbolicConditionalUnordered(Index j, Index parent1, Index parent2) : BaseFactor(j, parent1, parent2), BaseConditional(2) {}
+    SymbolicConditionalUnordered(Index j, Index parent1, Index parent2) : BaseFactor(j, parent1, parent2), BaseConditional(1) {}
 
     /** Three parents */
-    SymbolicConditionalUnordered(Index j, Index parent1, Index parent2, Index parent3) : BaseFactor(j, parent1, parent2, parent3), BaseConditional(3) {}
+    SymbolicConditionalUnordered(Index j, Index parent1, Index parent2, Index parent3) : BaseFactor(j, parent1, parent2, parent3), BaseConditional(1) {}
 
     /** Named constructor from an arbitrary number of keys and frontals */
     template<typename ITERATOR>
@@ -77,6 +77,17 @@ namespace gtsam {
 
     /// @}
 
+    /// @name Testable
+
+    /** Print with optional formatter */
+    void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      BaseConditional::print(str, keyFormatter); }
+
+    /** Check equality */
+    bool equals(const This& c, double tol = 1e-9) const { return BaseConditional::equals(c); }
+
+    /// @}
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;

gtsam/symbolic/SymbolicEliminationTreeUnordered.h

@@ -16,6 +16,8 @@
  * @author Richard Roberts
  */
 
+#pragma once
+
 #include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
 #include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
 #include <gtsam/inference/EliminationTreeUnordered.h>
@@ -38,7 +40,7 @@ namespace gtsam {
     * @return The elimination tree
     */
     SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph,
-      const VariableIndexUnordered& structure, const std::vector<Key>& order) :
+      const VariableIndexUnordered& structure, const OrderingUnordered& order) :
     Base(factorGraph, structure, order) {}
 
     /** Build the elimination tree of a factor graph.  Note that this has to compute the column
@@ -46,7 +48,7 @@ namespace gtsam {
     * constructor instead.
     * @param factorGraph The factor graph for which to build the elimination tree
     */
-    SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, const std::vector<Key>& order) :
+    SymbolicEliminationTreeUnordered(const SymbolicFactorGraphUnordered& factorGraph, const OrderingUnordered& order) :
       Base(factorGraph, order) {}
 
     /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are

gtsam/symbolic/SymbolicFactorGraphUnordered.h

@@ -24,9 +24,9 @@
 #include <gtsam/symbolic/SymbolicFactorUnordered.h>
 
 namespace gtsam { class SymbolicConditionalUnordered; }
-namespace gtsam { class SymbolicBayesNet; }
+namespace gtsam { class SymbolicBayesNetUnordered; }
 namespace gtsam { class SymbolicEliminationTreeUnordered; }
-namespace gtsam { class SymbolicBayesTree; }
+namespace gtsam { class SymbolicBayesTreeUnordered; }
 namespace gtsam { class SymbolicJunctionTreeUnordered; }
 
 namespace gtsam {
@@ -37,18 +37,16 @@ namespace gtsam {
   class GTSAM_EXPORT SymbolicFactorGraphUnordered:
     public FactorGraphUnordered<SymbolicFactorUnordered>,
     public EliminateableFactorGraph<
-    SymbolicFactorGraphUnordered, SymbolicFactorUnordered, SymbolicConditionalUnordered,
+    SymbolicFactorUnordered, SymbolicFactorGraphUnordered, SymbolicConditionalUnordered, SymbolicBayesNetUnordered,
-    SymbolicBayesNet, SymbolicEliminationTreeUnordered, SymbolicBayesTree, SymbolicJunctionTreeUnordered>
+    SymbolicEliminationTreeUnordered, SymbolicBayesTreeUnordered, SymbolicJunctionTreeUnordered>
   {
-
   public:
 
     typedef SymbolicFactorGraphUnordered This;
     typedef FactorGraphUnordered<SymbolicFactorUnordered> Base;
     typedef EliminateableFactorGraph<
-      SymbolicFactorGraphUnordered, SymbolicFactorUnordered, SymbolicConditionalUnordered,
+      SymbolicFactorUnordered, SymbolicFactorGraphUnordered, SymbolicConditionalUnordered, SymbolicBayesNetUnordered,
-      SymbolicBayesNet, SymbolicEliminationTreeUnordered, SymbolicBayesTree, SymbolicJunctionTreeUnordered>
+      SymbolicEliminationTreeUnordered, SymbolicBayesTreeUnordered, SymbolicJunctionTreeUnordered> BaseEliminateable;
-      BaseEliminateable;
     typedef BaseEliminateable::Eliminate Eliminate;
 
     /// @name Standard Constructors

gtsam/symbolic/SymbolicJunctionTreeUnordered.cpp

@@ -21,7 +21,7 @@
 
 namespace gtsam {
 
-  SymbolicJunctionTreeUnordered::SymbolicJunctionTreeUnordered() {
+  void SymbolicJunctionTreeUnordered::noop() const {
 
   }
 

gtsam/symbolic/SymbolicJunctionTreeUnordered.h

@@ -39,7 +39,7 @@ namespace gtsam {
     * @return The elimination tree
     */
     SymbolicJunctionTreeUnordered(const SymbolicEliminationTreeUnordered& eliminationTree) :
-      Base(eliminationTree) {}
+      Base(Base::FromEliminationTree(eliminationTree)) {}
 
     /** Copy constructor - makes a deep copy of the tree structure, but only pointers to factors are
      *  copied, factors are not cloned. */
@@ -51,8 +51,8 @@ namespace gtsam {
 
   private:
 
-    /// Private default constructor
+    // Dummy method to export class type
-    SymbolicJunctionTreeUnordered();
+    void noop() const;
   };
 
 }

gtsam/symbolic/tests/testSymbolicEliminationTree.cpp

@@ -77,7 +77,7 @@ TEST(EliminationTree, Create)
   SymbolicEliminationTreeUnordered expected = EliminationTreeUnorderedTester::buildHardcodedTree(fg);
 
   // Build from factor graph
-  vector<size_t> order;
+  OrderingUnordered order;
   order += 0,1,2,3,4;
   SymbolicEliminationTreeUnordered actual(fg, order);
 
@@ -108,7 +108,7 @@ TEST_UNSAFE(EliminationTree, eliminate )
   fg.push_factor(3, 4);
 
   // eliminate
-  vector<size_t> order;
+  OrderingUnordered order;
   order += 0,1,2,3,4;
   SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg,order).eliminate(EliminateSymbolicUnordered).first;
 
@@ -131,7 +131,7 @@ TEST(EliminationTree, disconnected_graph) {
   expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(3,4));
   expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(4));
 
-  vector<size_t> order;
+  OrderingUnordered order;
   order += 0,1,2,3,4;
   SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg, order).eliminate(EliminateSymbolicUnordered).first;