From 73179dbabc155d1912c84ee2d5a342e217fac508 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 17 Jun 2013 21:16:11 +0000
Subject: [PATCH] Added partial multifrontal elimination unit test

---
 .../tests/testSymbolicEliminationTree.cpp     |  5 +-
 .../tests/testSymbolicFactorGraph.cpp         | 68 +++++++++++++------
 2 files changed, 51 insertions(+), 22 deletions(-)

diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp
index a34ea09d2..4e3e0e117 100644
--- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp
+++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp
@@ -152,7 +152,7 @@ TEST(EliminationTree, eliminateAsiaExample)
     (boost::make_shared<SymbolicConditionalUnordered>(_L_, _B_))
     (boost::make_shared<SymbolicConditionalUnordered>(_B_));
 
-  SymbolicBayesNetUnordered actual = *createAsiaGraph().eliminateSequential(
+  SymbolicBayesNetUnordered actual = *asiaGraph.eliminateSequential(
     EliminateSymbolicUnordered, asiaOrdering);
 
   EXPECT(assert_equal(expected, actual));
@@ -176,7 +176,8 @@ TEST(EliminationTree, disconnected_graph) {
 
   OrderingUnordered order;
   order += 0,1,2,3,4;
-  SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg, order).eliminate(EliminateSymbolicUnordered).first;
+  SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg, order)
+    .eliminate(EliminateSymbolicUnordered).first;
   
   EXPECT(assert_equal(expected,actual));
 }
diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
index 9349850f6..95fff5618 100644
--- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
+++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
@@ -19,38 +19,66 @@
 
 #include <gtsam/symbolic/SymbolicFactorGraphUnordered.h>
 #include <gtsam/symbolic/SymbolicBayesNetUnordered.h>
+#include <boost/assign/list_of.hpp>
 
 using namespace std;
 using namespace gtsam;
+using namespace boost::assign;
+
+namespace {
+  const SymbolicFactorGraphUnordered simpleTestGraph = list_of
+    (boost::make_shared<SymbolicFactorUnordered>(0,1))
+    (boost::make_shared<SymbolicFactorUnordered>(0,2))
+    (boost::make_shared<SymbolicFactorUnordered>(1,3))
+    (boost::make_shared<SymbolicFactorUnordered>(1,4))
+    (boost::make_shared<SymbolicFactorUnordered>(2,3))
+    (boost::make_shared<SymbolicFactorUnordered>(4,5));
+}
 
 /* ************************************************************************* */
-TEST(FactorGraph, eliminateFrontals) {
+TEST(SymbolicFactorGraph, eliminatePartialSequential)
+{
+  SymbolicBayesNetUnordered expectedBayesNet;
+  expectedBayesNet.add(SymbolicConditionalUnordered::FromKeys(list_of(0)(1)(2), 1));
+  expectedBayesNet.add(SymbolicConditionalUnordered::FromKeys(list_of(1)(2)(3)(4), 1));
 
-  SymbolicFactorGraph sfgOrig;
-  sfgOrig.push_factor(0,1);
-  sfgOrig.push_factor(0,2);
-  sfgOrig.push_factor(1,3);
-  sfgOrig.push_factor(1,4);
-  sfgOrig.push_factor(2,3);
-  sfgOrig.push_factor(4,5);
-
-  IndexConditional::shared_ptr actualCond;
-  SymbolicFactorGraph actualSfg;
-  boost::tie(actualCond, actualSfg) = sfgOrig.eliminateFrontals(2);
-
-  vector<Index> condIndices;
-  condIndices += 0,1,2,3,4;
-  IndexConditional expectedCond(condIndices, 2);
-
-  SymbolicFactorGraph expectedSfg;
+  SymbolicFactorGraphUnordered expectedSfg;
   expectedSfg.push_factor(2,3);
   expectedSfg.push_factor(4,5);
   expectedSfg.push_factor(2,3,4);
 
-  EXPECT(assert_equal(expectedSfg, actualSfg));
-  EXPECT(assert_equal(expectedCond, *actualCond));
+  SymbolicBayesNetUnordered::shared_ptr actualBayesNet;
+  SymbolicFactorGraphUnordered::shared_ptr actualSfg;
+  boost::tie(actualBayesNet, actualSfg) = simpleTestGraph.eliminatePartialSequential(
+    EliminateSymbolicUnordered, OrderingUnordered(list_of(0)(1)));
+
+  EXPECT(assert_equal(expectedSfg, *actualSfg));
+  EXPECT(assert_equal(expectedBayesNet, *actualBayesNet));
 }
 
+/* ************************************************************************* */
+TEST(SymbolicFactorGraph, eliminatePartialMultifrontal)
+{
+  SymbolicBayesTreeUnordered expectedBayesTree;
+  SymbolicConditionalUnordered::shared_ptr root = boost::make_shared<SymbolicConditionalUnordered>(
+    SymbolicConditionalUnordered::FromKeys(list_of(5)(4)(1), 2));
+  expectedBayesTree.insertRoot(boost::make_shared<SymbolicBayesTreeCliqueUnordered>(root));
+
+  SymbolicFactorGraphUnordered expectedFactorGraph;
+  expectedFactorGraph.push_factor(0,1);
+  expectedFactorGraph.push_factor(0,2);
+  expectedFactorGraph.push_factor(1,3);
+  expectedFactorGraph.push_factor(2,3);
+  expectedFactorGraph.push_factor(1);
+
+  SymbolicBayesTreeUnordered::shared_ptr actualBayesTree;
+  SymbolicFactorGraphUnordered::shared_ptr actualFactorGraph;
+  boost::tie(actualBayesTree, actualFactorGraph) = simpleTestGraph.eliminatePartialMultifrontal(
+    EliminateSymbolicUnordered, OrderingUnordered(list_of(4)(5)));
+
+  EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph));
+  EXPECT(assert_equal(expectedBayesTree, *actualBayesTree));
+}
 /* ************************************************************************* */
 //TEST(SymbolicFactorGraph, eliminateFrontals) {
 //