From 35af122e83f8c0246efe8ae3f01bcbe09f817353 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 22 Nov 2009 23:50:01 +0000
Subject: [PATCH] Added a new version of removeTop and simplified update

---
 cpp/BayesTree-inl.h   | 28 +++++++++++++++++++++------
 cpp/BayesTree.h       |  6 ++++++
 cpp/testBayesTree.cpp | 45 +++++++++++++++++++++++++++----------------
 3 files changed, 56 insertions(+), 23 deletions(-)

diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h
index dabb1961d..4444f27ab 100644
--- a/cpp/BayesTree-inl.h
+++ b/cpp/BayesTree-inl.h
@@ -390,6 +390,7 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
+	// TODO: add to factors and orphans
 	template<class Conditional>
 	template<class Factor>
   pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques>
@@ -421,20 +422,35 @@ namespace gtsam {
 	/* ************************************************************************* */
 	template<class Conditional>
 	template<class Factor>
-	void BayesTree<Conditional>::update(const FactorGraph<Factor>& newFactors) {
+  pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques>
+	BayesTree<Conditional>::removeTop(const FactorGraph<Factor>& newFactors) {
+
 		// Remove the contaminated part of the Bayes tree
 		FactorGraph<Factor> factors;
-		typename BayesTree<Conditional>::Cliques orphans;
+		Cliques orphans;
 		BOOST_FOREACH(boost::shared_ptr<Factor> factor, newFactors) {
 
 			FactorGraph<Factor> factors1;
-			typename BayesTree<Conditional>::Cliques orphans1;
-			boost::tie(factors1, orphans1) = removeTop<Factor>(factor);
+			Cliques orphans1;
+			boost::tie(factors1, orphans1) = this->removeTop<Factor>(factor);
 
 			factors.push_back(factors1);
 			orphans.splice (orphans.begin(), orphans1);
 		}
 
+		return make_pair(factors,orphans);
+	}
+
+	/* ************************************************************************* */
+	template<class Conditional>
+	template<class Factor>
+	void BayesTree<Conditional>::update(const FactorGraph<Factor>& newFactors) {
+
+		// Remove the contaminated part of the Bayes tree
+		FactorGraph<Factor> factors;
+		Cliques orphans;
+		boost::tie(factors, orphans) = removeTop<Factor>(newFactors);
+
 		// add the factors themselves
 		factors.push_back(newFactors);
 
@@ -450,9 +466,9 @@ namespace gtsam {
 			insert(*rit);
 
 		// add orphans to the bottom of the new tree
-		BOOST_FOREACH(typename BayesTree<Conditional>::sharedClique orphan, orphans) {
+		BOOST_FOREACH(sharedClique orphan, orphans) {
 			string key = orphan->separator_.front(); // todo: assumes there is a separator...
-			typename BayesTree<Conditional>::sharedClique parent = (*this)[key];
+			sharedClique parent = (*this)[key];
 			parent->children_ += orphan;
 		}
 
diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h
index 39e57f9c3..a903eef55 100644
--- a/cpp/BayesTree.h
+++ b/cpp/BayesTree.h
@@ -177,6 +177,12 @@ namespace gtsam {
 		template<class Factor>
 		std::pair<FactorGraph<Factor>, Cliques> removeTop(const boost::shared_ptr<Factor>& newFactor);
 
+		/**
+		 * Call removeTop for several factors in a factorGraph
+		 */
+		template<class Factor>
+		std::pair<FactorGraph<Factor>, Cliques> removeTop(const FactorGraph<Factor>& newFactors);
+
 		/**
 		 * iSAM.
 		 */
diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp
index 8f4483a6d..d1ba116b1 100644
--- a/cpp/testBayesTree.cpp
+++ b/cpp/testBayesTree.cpp
@@ -438,26 +438,37 @@ TEST( BayesTree, removeTop )
 }
 
 
+/* ************************************************************************* */
+TEST( BayesTree, removeTop2 )
+{
+	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
+
+	// create two factors to be inserted
+	SymbolicFactorGraph newFactors;
+	newFactors.push_factor("B");
+	newFactors.push_factor("S");
+
+	// Remove the contaminated part of the Bayes tree
+	FactorGraph<SymbolicFactor> factors;
+	SymbolicBayesTree::Cliques orphans;
+	boost::tie(factors,orphans) = bayesTree.removeTop<SymbolicFactor>(newFactors);
+
+	// Check expected outcome
+	SymbolicFactorGraph expected;
+	expected.push_factor("B","L","E");
+	expected.push_factor("B","L");
+	expected.push_factor("B");
+	expected.push_factor("L","B","S");
+  CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
+	SymbolicBayesTree::Cliques expectedOrphans;
+  expectedOrphans += bayesTree["T"], bayesTree["X"];
+  // CHECK(assert_equal(expectedOrphans, orphans)); fails !
+}
+
 /* ************************************************************************* */
 TEST( BayesTree, iSAM )
 {
-	// Conditionals for ASIA example from the tutorial with A and D evidence
-	SymbolicConditional::shared_ptr
-		B(new SymbolicConditional("B")),
-		L(new SymbolicConditional("L", "B")),
-		E(new SymbolicConditional("E", "B", "L")),
-		S(new SymbolicConditional("S", "L", "B")),
-		T(new SymbolicConditional("T", "E", "L")),
-		X(new SymbolicConditional("X", "E"));
-
-	// Create using insert
-	SymbolicBayesTree bayesTree;
-	bayesTree.insert(B);
-	bayesTree.insert(L);
-	bayesTree.insert(E);
-	bayesTree.insert(S);
-	bayesTree.insert(T);
-	bayesTree.insert(X);
+	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
 
 	// Now we modify the Bayes tree by inserting a new factor over B and S