From 0286bc27eb6b41684f8976a4a3429c6f3ad2dc6d Mon Sep 17 00:00:00 2001
From: Michael Kaess <michaelkaess@gmail.com>
Date: Sat, 21 Nov 2009 03:38:13 +0000
Subject: [PATCH] removePath mostly working

---
 cpp/BayesTree-inl.h   | 31 ++++++++++++++++++++++++
 cpp/BayesTree.h       | 26 +++++++++++++++++++-
 cpp/testBayesTree.cpp | 55 +++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 111 insertions(+), 1 deletion(-)

diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h
index 2c2e9f208..2391beee6 100644
--- a/cpp/BayesTree-inl.h
+++ b/cpp/BayesTree-inl.h
@@ -316,6 +316,37 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
+	template<class Conditional>
+	template<class Factor>
+	FactorGraph<Factor>
+	BayesTree<Conditional>::removePath(const string& key) {
+		sharedClique clique = (*this)[key];
+
+#if 0
+		cout << "removing:" << endl;
+		clique->print();
+		cout << "from" << endl;
+		typedef std::pair<string, sharedClique> sometype;
+		BOOST_FOREACH(sometype clique, nodes_) {
+			clique.second->print();
+		}
+#endif
+
+		// convert clique to factor
+		FactorGraph<Factor> factors(*clique);
+
+		while (!(clique->isRoot())) {
+			sharedClique old_clique = clique;
+			clique = old_clique->parent_;
+		  removeClique(old_clique);
+			factors.push_back(*clique);
+		}
+		removeClique(clique);
+
+		return factors;
+	}
+
+	/* ************************************************************************* */
 
 }
 /// namespace gtsam
diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h
index 4db78290e..6dcc70eb2 100644
--- a/cpp/BayesTree.h
+++ b/cpp/BayesTree.h
@@ -79,7 +79,7 @@ namespace gtsam {
 			/** return the joint P(C1,C2), where C1==this. TODO: not a method? */
 			template<class Factor>
 			FactorGraph<Factor> joint(shared_ptr C2, shared_ptr root);
-		};
+};
 
 		typedef boost::shared_ptr<Clique> sharedClique;
 
@@ -104,6 +104,25 @@ namespace gtsam {
 			return new_clique;
 		}
 
+		/** remove a clique: warning, can result in a forest */
+		void removeClique(sharedClique clique) {
+		  if (clique->parent_ != NULL) {
+		    clique->parent_->children_.remove(clique);
+		  } else {
+		  	// we remove the root clique: have to make another clique the root
+		  	if (clique->children_.empty()) {
+		  		root_.reset();
+		  	} else {
+		  	  root_ = *(clique->children_.begin());
+		  	}
+		  }
+		  BOOST_FOREACH(sharedClique child, clique->children_) {
+		  	child->parent_.reset();
+		  }
+			std::string key = *(clique->keys().begin());
+			nodes_.erase(key);
+		}
+
 	public:
 
 		/** Create an empty Bayes Tree */
@@ -160,6 +179,11 @@ namespace gtsam {
 		template<class Factor>
 		BayesNet<Conditional> jointBayesNet(const std::string& key1, const std::string& key2) const;
 
+		/** IMPERATIVE! Return the factor containing all nodes contaminated by key; also,
+		 * removes those entries from the Bayes Tree, resulting in a forest of orphans
+		 */
+		template<class Factor>
+		FactorGraph<Factor> removePath(const std::string& key);
 	}; // BayesTree
 
 } /// namespace gtsam
diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp
index 07f31fdcf..2fa274d99 100644
--- a/cpp/testBayesTree.cpp
+++ b/cpp/testBayesTree.cpp
@@ -11,6 +11,7 @@ using namespace boost::assign;
 
 #include "SymbolicBayesNet.h"
 #include "GaussianBayesNet.h"
+#include "SymbolicFactorGraph.h"
 #include "Ordering.h"
 #include "BayesTree-inl.h"
 #include "smallExample.h"
@@ -302,6 +303,60 @@ TEST( BayesTree, balanced_smoother_joint )
 	CHECK(assert_equal(expected4,actual4,1e-4));
 }
 
+/* ************************************************************************* *
+   Bayes Tree, for testing conversion to a forest of orphans needed for incremental.
+       A,B
+   C|A    E|B
+   D|C    F|E
+/* ************************************************************************* */
+TEST( BayesTree, removePath )
+{
+	SymbolicConditional::shared_ptr A(new SymbolicConditional("A")),
+			B(new SymbolicConditional("B", "A")),
+			C(new SymbolicConditional("C", "A")),
+			D(new SymbolicConditional("D", "C")),
+			E(new SymbolicConditional("E", "B")),
+			F(new SymbolicConditional("F", "E"));
+	SymbolicBayesTree bayesTree;
+	bayesTree.insert(A);
+	bayesTree.insert(B);
+	bayesTree.insert(C);
+	bayesTree.insert(D);
+	bayesTree.insert(E);
+	bayesTree.insert(F);
+
+	// remove C, expected outcome: factor graph with ABC, Bayes Tree now contains two orphan trees: D|C and E|B,F|E
+	SymbolicFactorGraph expected;
+	list<string> keys;
+	keys += "A","C";
+	boost::shared_ptr<SymbolicFactor> _CA(new SymbolicFactor(keys));
+	expected.push_back(_CA);
+	keys.clear();
+	keys += "A","B";
+	boost::shared_ptr<SymbolicFactor> _BA(new SymbolicFactor(keys));
+	expected.push_back(_BA);
+	keys.clear();
+	keys += "A";
+	boost::shared_ptr<SymbolicFactor> _A(new SymbolicFactor(keys));
+	expected.push_back(_A);
+	SymbolicFactorGraph actual = bayesTree.removePath<SymbolicFactor>("C");
+  CHECK(assert_equal(expected, actual));
+
+	// remove A, nothing should happen (already removed)
+	SymbolicFactorGraph expected2; // empty factor
+  actual = bayesTree.removePath<SymbolicFactor>("A");
+//  CHECK(assert_equal(expected2, actual));
+
+  // remove E: factor graph with EB; E|B removed from second orphan tree
+	SymbolicFactorGraph expected3;
+	keys.clear();
+	keys += "C","A";
+	boost::shared_ptr<SymbolicFactor> CA(new SymbolicFactor(keys));
+  expected3.push_back(CA);
+  actual = bayesTree.removePath<SymbolicFactor>("E");
+//  CHECK(assert_equal(expected3, actual));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;