From 7dfd999673c07a03e0c09dc351afdc4da1a1834b Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Thu, 6 Jun 2013 15:36:13 +0000
Subject: [PATCH] Changed order of conditionals created during elimination. 
 Added unit test for disconnected graphs

---
 .../inference/EliminationTreeUnordered-inl.h  | 17 ++++----
 .../tests/testSymbolicEliminationTree.cpp     | 40 ++++++++++++-------
 2 files changed, 34 insertions(+), 23 deletions(-)

diff --git a/gtsam/inference/EliminationTreeUnordered-inl.h b/gtsam/inference/EliminationTreeUnordered-inl.h
index 08a28eb84..95a7f640e 100644
--- a/gtsam/inference/EliminationTreeUnordered-inl.h
+++ b/gtsam/inference/EliminationTreeUnordered-inl.h
@@ -175,11 +175,7 @@ namespace gtsam {
     EliminationTreeUnordered<BAYESNET,GRAPH>::eliminate(Eliminate function) const
   {
     // Stack for eliminating nodes.  We use this stack instead of recursive function calls to
-    // avoid call stack overflow due to very long trees that arise from chain-like graphs.  We use
-    // an std::vector for storage here since we do not want frequent reallocations and do not care
-    // about the vector growing to be very large once and not being deallocated until this
-    // function exits, because in the worst case we only store one pointer in this stack for each
-    // variable in the system.
+    // avoid call stack overflow due to very long trees that arise from chain-like graphs.
     // TODO: Check whether this is faster as a vector (then use indices instead of parent pointers).
     typedef EliminationNode<This> EliminationNode;
     std::stack<EliminationNode, FastList<EliminationNode> > eliminationStack;
@@ -192,8 +188,9 @@ namespace gtsam {
     boost::shared_ptr<FactorGraphType> remainingFactors =
       boost::make_shared<FactorGraphType>(remainingFactors_.begin(), remainingFactors_.end());
 
-    // Add roots to the stack
-    BOOST_FOREACH(const sharedNode& root, roots_) {
+    // Add roots to the stack (use reverse foreach so conditionals to appear in elimination order -
+    // doesn't matter for computation but can make printouts easier to interpret by hand)
+    BOOST_REVERSE_FOREACH(const sharedNode& root, roots_) {
       eliminationStack.push(
         EliminationNode(root.get(), root->factors.size() + root->subTrees.size(),
         root->factors.begin(), root->factors.end(), 0)); }
@@ -221,9 +218,11 @@ namespace gtsam {
         // Remove from stack
         eliminationStack.pop();
       } else {
-        // Expand children and mark as expanded
+        // Expand children and mark as expanded (use reverse foreach so conditionals to appear in
+        // elimination order - doesn't matter for computation but can make printouts easier to
+        // interpret by hand)
         node.expanded = true;
-        BOOST_FOREACH(const sharedNode& child, node.treeNode->subTrees) {
+        BOOST_REVERSE_FOREACH(const sharedNode& child, node.treeNode->subTrees) {
           eliminationStack.push(
             EliminationNode(child.get(), child->factors.size() + child->subTrees.size(),
             child->factors.begin(), child->factors.end(), &node)); }
diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp
index 3db2363c4..b62cf7166 100644
--- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp
+++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp
@@ -90,11 +90,11 @@ TEST_UNSAFE(EliminationTree, eliminate )
 {
   // create expected Chordal bayes Net
   SymbolicBayesNetUnordered expected;
-  expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(4));
-  expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(3,4));
-  expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(2,4));
-  expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(1,2,4));
   expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(0,1,2));
+  expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(1,2,4));
+  expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(2,4));
+  expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(3,4));
+  expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(4));
 
   // Create factor graph
   SymbolicFactorGraphUnordered fg;
@@ -109,19 +109,31 @@ TEST_UNSAFE(EliminationTree, eliminate )
   order += 0,1,2,3,4;
   SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg,order).eliminate(EliminateSymbolicUnordered).first;
 
-  CHECK(assert_equal(expected,actual));
+  EXPECT(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);
-//}
+TEST(EliminationTree, disconnected_graph) {
+  SymbolicFactorGraphUnordered fg;
+  fg.push_factor(0, 1);
+  fg.push_factor(0, 2);
+  fg.push_factor(1, 2);
+  fg.push_factor(3, 4);
+
+  // create expected Chordal bayes Net
+  SymbolicBayesNetUnordered expected;
+  expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(0,1,2));
+  expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(1,2));
+  expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(2));
+  expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(3,4));
+  expected.push_back(boost::make_shared<SymbolicConditionalUnordered>(4));
+
+  vector<size_t> order;
+  order += 0,1,2,3,4;
+  SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg, order).eliminate(EliminateSymbolicUnordered).first;
+
+  EXPECT(assert_equal(expected,actual));
+}
 
 /* ************************************************************************* */
 int main() {