removeTop with one factor implemented and one test works (orphans not yet checked)

cpp/BayesTree-inl.h

@@ -372,6 +372,38 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
+	template<class Conditional>
+	template<class Factor>
+  pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques>
+	BayesTree<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor) {
+
+		FactorGraph<Factor> factors;
+		Cliques orphans;
+
+		// process each key of the new factor
+		BOOST_FOREACH(string key, newFactor->keys())
+			// only add if key is not yet in the factor graph
+			if (!factors.involves(key)) {
+
+				// get the clique
+				sharedClique clique = (*this)[key];
+
+				// remove path above this clique
+				FactorGraph<Factor> factors1;	Cliques orphans1;
+				boost::tie(factors1,orphans1) = removePath<Factor>(clique->parent_);
+
+				// add to global factors and orphans
+				factors.push_back(factors1);
+				orphans.splice (orphans.begin(), orphans1);
+			}
+
+		// now add the new factor
+		factors.push_back(newFactor);
+
+		return make_pair(factors,orphans);
+	}
+
+	/* ************************************************************************* */
 
 }
 /// namespace gtsam

cpp/BayesTree.h

@@ -162,10 +162,21 @@ namespace gtsam {
 		template<class Factor>
 		BayesNet<Conditional> jointBayesNet(const std::string& key1, const std::string& key2) const;
 
-		/** Remove path from clique to root and return that path as factors plus a list of orphaned subtree roots */
+		/**
+		 * Remove path from clique to root and return that path as factors
+		 * plus a list of orphaned subtree roots. Used in removeTop below.
+		 */
 		template<class Factor>
 		std::pair<FactorGraph<Factor>, Cliques> removePath(sharedClique clique);
 
+		/**
+		 * Given a set of factors, turn "contaminated" part of the tree back into a factor graph
+		 * and return it along with the new factors plus a list of orphaned subtree roots.
+		 * This is used for incrementally updating a BayesTree given new measurements (factors).
+		 */
+		template<class Factor>
+		std::pair<FactorGraph<Factor>, Cliques> removeTop(const boost::shared_ptr<Factor>& newFactor);
+
 	}; // BayesTree
 
 } /// namespace gtsam

cpp/testBayesTree.cpp

@@ -345,6 +345,46 @@ TEST( BayesTree, removePath )
   CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected3, factors));
 }
 
+/* ************************************************************************* */
+TEST( BayesTree, removeTop )
+{
+	// 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);
+
+	// create a new factor to be inserted
+	list<string> keys;
+	keys += "B","S";
+	boost::shared_ptr<SymbolicFactor> newFactor(new SymbolicFactor(keys));
+
+	// Remove the contaminated part of the Bayes tree
+	FactorGraph<SymbolicFactor> factors;
+	SymbolicBayesTree::Cliques orphans;
+	boost::tie(factors,orphans) = bayesTree.removeTop<SymbolicFactor>(newFactor);
+
+	// Check expected outcome
+	SymbolicFactorGraph expected;
+	expected.push_factor("B","L","E");
+	expected.push_factor("B","L");
+	expected.push_factor("B");
+	expected.push_factor("B","S");
+  CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;