From 10a8ac34eceabbe44a608d091714e07c043cb217 Mon Sep 17 00:00:00 2001
From: Michael Kaess <michaelkaess@gmail.com>
Date: Mon, 23 Nov 2009 01:16:58 +0000
Subject: [PATCH] iSAM smoother test

---
 cpp/testBayesTree.cpp | 28 +++++++++++++++++++++++++++-
 1 file changed, 27 insertions(+), 1 deletion(-)

diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp
index ad240e22a..f7d8255b7 100644
--- a/cpp/testBayesTree.cpp
+++ b/cpp/testBayesTree.cpp
@@ -464,7 +464,7 @@ TEST( BayesTree, removeTop2 )
   CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
 	SymbolicBayesTree::Cliques expectedOrphans;
   expectedOrphans += bayesTree["T"], bayesTree["X"];
-  // CHECK(assert_equal(expectedOrphans, orphans)); fails !
+	CHECK(assert_equal(expectedOrphans, orphans));
 }
 
 /* ************************************************************************* */
@@ -502,6 +502,32 @@ TEST( BayesTree, iSAM )
   CHECK(assert_equal(expected,bayesTree));
 }
 
+/* ************************************************************************* */
+TEST( BayesTree, iSAM_smoother )
+{
+	// Create smoother with 7 nodes
+	GaussianFactorGraph smoother = createSmoother(7);
+
+	// iSAM
+	GaussianBayesTree bayesTree;
+	BOOST_FOREACH(boost::shared_ptr<GaussianFactor> factor, (const GaussianFactorGraph)smoother) {
+		GaussianFactorGraph factorGraph;
+		factorGraph.push_back(factor);
+		bayesTree.update(factorGraph);
+	}
+
+	// expected
+	Ordering ordering;
+	for (int t = 1; t <= 7; t++)
+		ordering.push_back(symbol('x', t));
+	// eliminate using the "natural" ordering
+	GaussianBayesNet chordalBayesNet = smoother.eliminate(ordering);
+	// Create the Bayes tree
+	GaussianBayesTree expectedBayesTree(chordalBayesNet);
+
+	CHECK(assert_equal(expectedBayesTree, bayesTree));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;