From 2a4e90a283a115aafdc9b3fb690384ec78d2799c Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sat, 5 Dec 2009 02:00:20 +0000
Subject: [PATCH] Re-factored append_factor to use FOREACH_PAIR

---
 cpp/GaussianFactor.cpp | 28 +++++++++++-----------------
 cpp/GaussianFactor.h   |  3 +--
 2 files changed, 12 insertions(+), 19 deletions(-)

diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp
index 923fc729f..66add2def 100644
--- a/cpp/GaussianFactor.cpp
+++ b/cpp/GaussianFactor.cpp
@@ -244,35 +244,29 @@ GaussianFactor::sparse(const Ordering& ordering, const Dimensions& variables) co
 }
 
 /* ************************************************************************* */
-void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, const size_t m,
-		const size_t pos) {
-	bool verbose = false;
-	if (verbose) cout << "GaussianFactor::append_factor" << endl;
+void GaussianFactor::append_factor(GaussianFactor::shared_ptr f, size_t m, size_t pos) {
 
 	// iterate over all matrices from the factor f
-	GaussianFactor::const_iterator it = f->begin();
-	for (; it != f->end(); it++) {
-		string key = it->first;
-
-		// find rows and columns
-		const size_t mrhs = it->second.size1(), n = it->second.size2();
+	string key; Matrix A;
+	FOREACH_PAIR( key, A, f->As_) {
 
 		// find the corresponding matrix among As
 		iterator mine = As_.find(key);
 		const bool exists = mine != As_.end();
 
-		// create the matrix or use existing
-		if(exists) {
-		  copy(it->second.data().begin(), it->second.data().end(), (mine->second).data().begin()+pos*n);
-		}
+		// find rows and columns
+		const size_t n = A.size2();
+
+		// use existing or create new matrix
+		if (exists)
+		  copy(A.data().begin(), A.data().end(), (mine->second).data().begin()+pos*n);
 		else {
 			Matrix Z = zeros(m, n);
-			copy(it->second.data().begin(), it->second.data().end(), Z.data().begin()+pos*n);
+			copy(A.data().begin(), A.data().end(), Z.data().begin()+pos*n);
 			insert(key, Z);
 		}
-	}
-	if (verbose) cout << "GaussianFactor::append_factor done" << endl;
 
+	} // FOREACH
 }
 
 /* ************************************************************************* */
diff --git a/cpp/GaussianFactor.h b/cpp/GaussianFactor.h
index 200739109..1bf849031 100644
--- a/cpp/GaussianFactor.h
+++ b/cpp/GaussianFactor.h
@@ -241,8 +241,7 @@ public:
 	 * @param m final number of rows of f, needs to be known in advance
 	 * @param pos where to insert in the m-sized matrices
 	 */
-	inline void append_factor(GaussianFactor::shared_ptr f, const size_t m,
-			const size_t pos);
+	inline void append_factor(GaussianFactor::shared_ptr f, size_t m, size_t pos);
 
 }; // GaussianFactor