From 4951a254533d0af46b2fb1f6ba8f71dbe6f367c1 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Mon, 23 Nov 2009 00:02:06 +0000
Subject: [PATCH] More efficient removeTop

---
 cpp/BayesTree-inl.h   | 20 ++++----------------
 cpp/BayesTree.h       | 15 +++++++++------
 cpp/testBayesTree.cpp | 10 ++++++----
 3 files changed, 19 insertions(+), 26 deletions(-)

diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h
index 4444f27ab..88a0b8069 100644
--- a/cpp/BayesTree-inl.h
+++ b/cpp/BayesTree-inl.h
@@ -393,11 +393,8 @@ namespace gtsam {
 	// TODO: add to factors and orphans
 	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;
+  void BayesTree<Conditional>::removeTop(const boost::shared_ptr<Factor>& newFactor,
+		FactorGraph<Factor> &factors, typename BayesTree<Conditional>::Cliques& orphans) {
 
 		// process each key of the new factor
 		BOOST_FOREACH(string key, newFactor->keys())
@@ -415,8 +412,6 @@ namespace gtsam {
 				orphans.splice (orphans.begin(), orphans1);
 			} catch (std::invalid_argument e) {
 			}
-
-		return make_pair(factors,orphans);
 	}
 
 	/* ************************************************************************* */
@@ -428,15 +423,8 @@ namespace gtsam {
 		// Remove the contaminated part of the Bayes tree
 		FactorGraph<Factor> factors;
 		Cliques orphans;
-		BOOST_FOREACH(boost::shared_ptr<Factor> factor, newFactors) {
-
-			FactorGraph<Factor> factors1;
-			Cliques orphans1;
-			boost::tie(factors1, orphans1) = this->removeTop<Factor>(factor);
-
-			factors.push_back(factors1);
-			orphans.splice (orphans.begin(), orphans1);
-		}
+		BOOST_FOREACH(boost::shared_ptr<Factor> factor, newFactors)
+			this->removeTop<Factor>(factor, factors, orphans);
 
 		return make_pair(factors,orphans);
 	}
diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h
index a903eef55..fc3a52648 100644
--- a/cpp/BayesTree.h
+++ b/cpp/BayesTree.h
@@ -170,18 +170,21 @@ namespace gtsam {
 		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 a list of orphaned subtree roots.
-		 * This is used for incrementally updating a BayesTree given new measurements (factors).
+		 * Given a factor, turn "contaminated" part of the tree back into a factor graph.
+		 * Factors and orphans are added to the in/out arguments.
 		 */
 		template<class Factor>
-		std::pair<FactorGraph<Factor>, Cliques> removeTop(const boost::shared_ptr<Factor>& newFactor);
+		void removeTop(const boost::shared_ptr<Factor>& newFactor,
+				FactorGraph<Factor> &factors, Cliques& orphans);
 
 		/**
-		 * Call removeTop for several factors in a factorGraph
+		 * Given a set of factors, turn "contaminated" part of the tree back into a
+		 * factor graph and return it along with a list of orphaned subtree roots.
+		 * Used for incrementally updating a BayesTree given new measurements (factors).
 		 */
 		template<class Factor>
-		std::pair<FactorGraph<Factor>, Cliques> removeTop(const FactorGraph<Factor>& newFactors);
+		std::pair<FactorGraph<Factor>, Cliques>
+		removeTop(const FactorGraph<Factor>& newFactors);
 
 		/**
 		 * iSAM.
diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp
index d1ba116b1..ad240e22a 100644
--- a/cpp/testBayesTree.cpp
+++ b/cpp/testBayesTree.cpp
@@ -415,7 +415,7 @@ TEST( BayesTree, removeTop )
 	// Remove the contaminated part of the Bayes tree
 	FactorGraph<SymbolicFactor> factors;
 	SymbolicBayesTree::Cliques orphans;
-	boost::tie(factors,orphans) = bayesTree.removeTop<SymbolicFactor>(newFactor);
+	bayesTree.removeTop<SymbolicFactor>(newFactor, factors, orphans);
 
 	// Check expected outcome
 	SymbolicFactorGraph expected;
@@ -430,11 +430,13 @@ TEST( BayesTree, removeTop )
 
   // 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);
+	FactorGraph<SymbolicFactor> factors2;
+	SymbolicBayesTree::Cliques orphans2;
+	bayesTree.removeTop<SymbolicFactor>(newFactor2, factors2, orphans2);
 	SymbolicFactorGraph expected2;
-  CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors));
+  CHECK(assert_equal((FactorGraph<SymbolicFactor>)expected2, factors2));
 	SymbolicBayesTree::Cliques expectedOrphans2;
-  CHECK(assert_equal(expectedOrphans2, orphans));
+  CHECK(assert_equal(expectedOrphans2, orphans2));
 }