From 45292f7bd6b584da289a3acca395816c85dd405c Mon Sep 17 00:00:00 2001
From: Michael Kaess <michaelkaess@gmail.com>
Date: Sun, 22 Nov 2009 17:40:24 +0000
Subject: [PATCH] Cliques implemented; unit test on orphans for removePath; bug
 fix in removePath/orphans

---
 cpp/BayesTree-inl.h   | 20 ++++++++++++++++++--
 cpp/BayesTree.h       |  4 ++--
 cpp/testBayesTree.cpp | 12 +++++++++---
 3 files changed, 29 insertions(+), 7 deletions(-)

diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h
index afefe83de..6ae445139 100644
--- a/cpp/BayesTree-inl.h
+++ b/cpp/BayesTree-inl.h
@@ -171,6 +171,20 @@ namespace gtsam {
 		return marginalize<Factor,Conditional>(*bn,keys12);
 	}
 
+	/* ************************************************************************* */
+	template<class Conditional>
+	void BayesTree<Conditional>::Cliques::print(const std::string& s) const {
+		cout << s << " Cliques: ";
+		BOOST_FOREACH(sharedClique clique, *this)
+				clique->print();
+	}
+
+	/* ************************************************************************* */
+	template<class Conditional>
+	bool BayesTree<Conditional>::Cliques::equals(const Cliques& other, double tol) const {
+		return other == *this;
+	}
+
 	/* ************************************************************************* */
 	template<class Conditional>
 	typename BayesTree<Conditional>::sharedClique BayesTree<Conditional>::addClique
@@ -357,14 +371,16 @@ namespace gtsam {
 		// base case is NULL, if so we do nothing and return empties above
 		if (clique!=NULL) {
 
+			// remove me
+			removeClique(clique);
+
 			// remove path above me
 			boost::tie(factors,orphans) = removePath<Factor>(clique->parent_);
 
 			// add children to list of orphans
 			orphans.insert(orphans.begin(), clique->children_.begin(), clique->children_.end());
 
-			// remove me and add my factors
-			removeClique(clique);
+			// add myself to factors
 			factors.push_back(*clique);
 		}
 
diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h
index 4eb911990..9e2813de7 100644
--- a/cpp/BayesTree.h
+++ b/cpp/BayesTree.h
@@ -86,8 +86,8 @@ namespace gtsam {
 
 		// A convenience class for a list of shared cliques
 		struct Cliques : public std::list<sharedClique>, public Testable<Cliques> {
-			void print(const std::string& s = "") const {}
-			bool equals(const Cliques& other, double tol = 1e-9) const { return false; }
+			void print(const std::string& s = "Cliques") const;
+			bool equals(const Cliques& other, double tol = 1e-9) const;
 		};
 
 	private:
diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp
index eff4f74c1..ffc5942e1 100644
--- a/cpp/testBayesTree.cpp
+++ b/cpp/testBayesTree.cpp
@@ -331,18 +331,24 @@ TEST( BayesTree, removePath )
 	expected.push_factor("A","B");
 	expected.push_factor("A");
 	expected.push_factor("A","C");
+	SymbolicBayesTree::Cliques expectedOrphans;
+  expectedOrphans += bayesTree["D"], bayesTree["E"];
 
 	FactorGraph<SymbolicFactor> factors;
 	SymbolicBayesTree::Cliques orphans;
 	boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["C"]);
   CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
+  CHECK(assert_equal(expectedOrphans, orphans));
 
   // remove E: factor graph with EB; E|B removed from second orphan tree
-	SymbolicFactorGraph expected3;
-  expected3.push_factor("B","E");
+	SymbolicFactorGraph expected2;
+  expected2.push_factor("B","E");
+  SymbolicBayesTree::Cliques expectedOrphans2;
+  expectedOrphans2 += bayesTree["F"];
 
   boost::tie(factors,orphans) = bayesTree.removePath<SymbolicFactor>(bayesTree["E"]);
-  CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected3, factors));
+  CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors));
+  CHECK(assert_equal(expectedOrphans2, orphans));
 }
 
 /* ************************************************************************* */