Cliques implemented; unit test on orphans for removePath; bug fix in removePath/orphans

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
+			// add myself to factors
-			removeClique(clique);
 			factors.push_back(*clique);
 		}
 

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 {}
+			void print(const std::string& s = "Cliques") const;
-			bool equals(const Cliques& other, double tol = 1e-9) const { return false; }
+			bool equals(const Cliques& other, double tol = 1e-9) const;
 		};
 
 	private:

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;
+	SymbolicFactorGraph expected2;
-  expected3.push_factor("B","E");
+  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));
 }
 
 /* ************************************************************************* */