From 00b5b25591f3bafbf53f748931554bb17a059d28 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 21 Nov 2009 06:14:53 +0000
Subject: [PATCH] Awesome recursive version of removePath

---
 cpp/BayesTree-inl.h   | 29 ++++++-----------------------
 cpp/testBayesTree.cpp |  2 +-
 2 files changed, 7 insertions(+), 24 deletions(-)

diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h
index 02bd8ce47..7b4adc69e 100644
--- a/cpp/BayesTree-inl.h
+++ b/cpp/BayesTree-inl.h
@@ -352,32 +352,15 @@ namespace gtsam {
 	FactorGraph<Factor>
 	BayesTree<Conditional>::removePath(sharedClique clique) {
 
-		//if (clique==NULL) return;
+		// base case is NULL, return empty factor graph
+		if (clique==NULL) return FactorGraph<Factor>();
 
-		bool verbose = false;
-		if (verbose) {
-			clique->print("removing");
-			cout << "before" << endl;
-			BOOST_FOREACH(typename Nodes::value_type clique, nodes_)
-				clique.second->print();
-		}
+		// remove path above me
+		FactorGraph<Factor> factors = removePath<Factor>(clique->parent_);
 
-		// convert clique to factor
-		FactorGraph<Factor> factors(*clique);// = removePath(clique->parent_);
-
-		while (!(clique->isRoot())) {
-			sharedClique old_clique = clique;
-			clique = old_clique->parent_;
-		  removeClique(old_clique);
-			factors.push_back(*clique);
-		}
+		// remove me and add my factors
 		removeClique(clique);
-
-		if (verbose) {
-			cout << "after" << endl;
-			BOOST_FOREACH(typename Nodes::value_type clique, nodes_)
-				clique.second->print();
-		}
+		factors.push_back(*clique);
 
 		return factors;
 	}
diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp
index 342ad0b5a..f7768d61c 100644
--- a/cpp/testBayesTree.cpp
+++ b/cpp/testBayesTree.cpp
@@ -328,9 +328,9 @@ TEST( BayesTree, removePath )
 	// remove C, expected outcome: factor graph with ABC,
 	// Bayes Tree now contains two orphan trees: D|C and E|B,F|E
 	SymbolicFactorGraph expected;
-	expected.push_factor("A","C");
 	expected.push_factor("A","B");
 	expected.push_factor("A");
+	expected.push_factor("A","C");
 
 	SymbolicFactorGraph actual = bayesTree.removePath<SymbolicFactor>(bayesTree["C"]);
   CHECK(assert_equal(expected, actual));