removeTop now does not bomb when a clique does not exist: it just does nothing

cpp/BayesTree-inl.h

@@ -378,7 +378,7 @@ namespace gtsam {
 			boost::tie(factors,orphans) = removePath<Factor>(clique->parent_);
 
 			// add children to list of orphans
-			orphans.insert(orphans.begin(), clique->children_.begin(), clique->children_.end());
+			orphans.splice (orphans.begin(), clique->children_);
 
 			// add myself to factors
 			factors.push_back(*clique);
@@ -388,7 +388,6 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-
 	template<class Conditional>
 	template<class Factor>
   pair<FactorGraph<Factor>, typename BayesTree<Conditional>::Cliques>
@@ -399,9 +398,7 @@ namespace gtsam {
 
 		// process each key of the new factor
 		BOOST_FOREACH(string key, newFactor->keys())
-			// only add if key is not yet in the factor graph
+			try {
-			if (!factors.involves(key)) {
-
 				// get the clique and remove it from orphans (if it exists)
 				sharedClique clique = (*this)[key];
 				orphans.remove(clique);
@@ -413,11 +410,9 @@ namespace gtsam {
 				// add to global factors and orphans
 				factors.push_back(factors1);
 				orphans.splice (orphans.begin(), orphans1);
+			} catch (std::invalid_argument e) {
 			}
 
-		// now add the new factor
-		factors.push_back(newFactor);
-
 		return make_pair(factors,orphans);
 	}
 

cpp/BayesTree.h

@@ -171,7 +171,7 @@ namespace gtsam {
 
 		/**
 		 * 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.
+		 * and return it along with a list of orphaned subtree roots.
 		 * This is used for incrementally updating a BayesTree given new measurements (factors).
 		 */
 		template<class Factor>

cpp/testBayesTree.cpp

@@ -408,9 +408,7 @@ TEST( BayesTree, removeTop )
 	SymbolicBayesTree bayesTree = createAsiaSymbolicBayesTree();
 
 	// create a new factor to be inserted
-	list<string> keys;
+	boost::shared_ptr<SymbolicFactor> newFactor(new SymbolicFactor("B","S"));
-	keys += "B","S";
-	boost::shared_ptr<SymbolicFactor> newFactor(new SymbolicFactor(keys));
 
 	// Remove the contaminated part of the Bayes tree
 	FactorGraph<SymbolicFactor> factors;
@@ -423,11 +421,18 @@ TEST( BayesTree, removeTop )
 	expected.push_factor("B","L");
 	expected.push_factor("B");
 	expected.push_factor("L","B","S");
-	expected.push_factor("B","S");
   CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected, factors));
 	SymbolicBayesTree::Cliques expectedOrphans;
   expectedOrphans += bayesTree["T"], bayesTree["X"];
   CHECK(assert_equal(expectedOrphans, orphans));
+
+  // Try removeTop again with a factor that should not change a thing
+	boost::shared_ptr<SymbolicFactor> newFactor2(new SymbolicFactor("B"));
+	boost::tie(factors,orphans) = bayesTree.removeTop<SymbolicFactor>(newFactor2);
+	SymbolicFactorGraph expected2;
+  CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors));
+	SymbolicBayesTree::Cliques expectedOrphans2;
+  CHECK(assert_equal(expectedOrphans2, orphans));
 }
 
 /* ************************************************************************* */

cpp/testIncremental.cpp

@@ -27,21 +27,24 @@ typedef BayesTree<GaussianConditional> GaussianBayesTree;
 
 /* ************************************************************************* */
 
-void update(SymbolicBayesTree& bayesTree, const FactorGraph<SymbolicFactor>& factorGraph) {
+void update(SymbolicBayesTree& bayesTree, const FactorGraph<SymbolicFactor>& newFactors) {
 
 	// Remove the contaminated part of the Bayes tree
 	FactorGraph<SymbolicFactor> factors;
 	SymbolicBayesTree::Cliques orphans;
-	BOOST_FOREACH(boost::shared_ptr<SymbolicFactor> factor, factorGraph) {
+	BOOST_FOREACH(boost::shared_ptr<SymbolicFactor> factor, newFactors) {
 
-		FactorGraph<SymbolicFactor> newFactors;
+		FactorGraph<SymbolicFactor> factors1;
-		SymbolicBayesTree::Cliques newOrphans;
+		SymbolicBayesTree::Cliques orphans1;
-		boost::tie(newFactors, newOrphans) = bayesTree.removeTop<SymbolicFactor>(factor);
+		boost::tie(factors1, orphans1) = bayesTree.removeTop<SymbolicFactor>(factor);
 
-		factors.push_back(newFactors);
+		factors.push_back(factors1);
-		orphans.splice (orphans.begin(), newOrphans);
+		orphans.splice (orphans.begin(), orphans1);
 	}
 
+	// add the factors themselves
+	factors.push_back(newFactors);
+
 	// create an ordering for the new and contaminated factors
 	Ordering ordering = factors.getOrdering();
 
@@ -104,6 +107,7 @@ TEST( BayesTree, iSAM )
 	// create new factors to be inserted
 	SymbolicFactorGraph factorGraph;
 	factorGraph.push_factor("B","S");
+	factorGraph.push_factor("B");
 
 	// do incremental inference
 	update(bayesTree, factorGraph);