From b2eee54b5a67e09e90cf34966272909d54076db0 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 19 Nov 2009 06:34:07 +0000
Subject: [PATCH] push_back an entire factor graph

---
 cpp/FactorGraph-inl.h           | 17 +++++++++-------
 cpp/FactorGraph.h               |  3 +++
 cpp/testSymbolicFactorGraph.cpp | 35 +++++++++++++++++++++++++++++++++
 3 files changed, 48 insertions(+), 7 deletions(-)

diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h
index d59af0168..eece27d80 100644
--- a/cpp/FactorGraph-inl.h
+++ b/cpp/FactorGraph-inl.h
@@ -99,6 +99,14 @@ void FactorGraph<Factor>::push_back(sharedFactor factor) {
 	}
 }
 
+/* ************************************************************************* */
+template<class Factor>
+void FactorGraph<Factor>::push_back(const FactorGraph<Factor>& factors) {
+	const_iterator factor = factors.begin();
+	for (; factor!= factors.end(); factor++)
+		push_back(*factor);
+}
+
 /* ************************************************************************* */
 template<class Factor>
 Ordering FactorGraph<Factor>::keys() const {
@@ -230,16 +238,11 @@ removeAndCombineFactors(FactorGraph<Factor>& factorGraph, const string& key)
 /* ************************************************************************* */
 template<class Factor>
 FactorGraph<Factor> combine(const FactorGraph<Factor>& fg1, const FactorGraph<Factor>& fg2) {
-
-	typedef FactorGraph<Factor> FG;
-
 	// create new linear factor graph equal to the first one
-	FG fg = fg1;
+	FactorGraph<Factor> fg = fg1;
 
 	// add the second factors_ in the graph
-	typename FG::const_iterator factor = fg2.begin();
-	for (; factor!= fg2.end(); factor++)
-		fg.push_back(*factor);
+	fg.push_back(fg2);
 
 	return fg;
 }
diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h
index 1d6ffcc42..8fcd183ec 100644
--- a/cpp/FactorGraph.h
+++ b/cpp/FactorGraph.h
@@ -86,6 +86,9 @@ namespace gtsam {
 		/** Add a factor */
 		void push_back(sharedFactor factor);
 
+		/** push back many factors */
+		void push_back(const FactorGraph<Factor>& factors);
+
 		/** return keys in some random order */
 		Ordering keys() const;
 
diff --git a/cpp/testSymbolicFactorGraph.cpp b/cpp/testSymbolicFactorGraph.cpp
index 82953d3e2..e02de0c55 100644
--- a/cpp/testSymbolicFactorGraph.cpp
+++ b/cpp/testSymbolicFactorGraph.cpp
@@ -180,6 +180,41 @@ TEST( SymbolicFactorGraph, constructFromBayesNet )
   CHECK(assert_equal(expected,actual));
 }
 
+/* ************************************************************************* */
+TEST( SymbolicFactorGraph, push_back )
+{
+	// Create two factor graphs and expected combined graph
+	SymbolicFactorGraph fg1, fg2, expected;
+
+	list<string> f1_keys; f1_keys += "x1";
+	SymbolicFactor::shared_ptr f1(new SymbolicFactor(f1_keys));
+	fg1.push_back(f1);
+	expected.push_back(f1);
+
+	list<string> f2_keys; f2_keys.push_back("x1"); f2_keys.push_back("x2");
+	SymbolicFactor::shared_ptr f2(new SymbolicFactor(f2_keys));
+	fg1.push_back(f2);
+	expected.push_back(f2);
+
+	list<string> f3_keys; f3_keys.push_back("l1"); f3_keys.push_back("x1");
+	SymbolicFactor::shared_ptr f3(new SymbolicFactor(f3_keys));
+	fg2.push_back(f3);
+	expected.push_back(f3);
+
+	list<string> f4_keys; f4_keys.push_back("l1"); f4_keys.push_back("x2");
+	SymbolicFactor::shared_ptr f4(new SymbolicFactor(f4_keys));
+	fg2.push_back(f4);
+	expected.push_back(f4);
+
+	// combine
+	SymbolicFactorGraph actual = combine(fg1,fg2);
+	CHECK(assert_equal(expected, actual));
+
+	// combine using push_back
+	fg1.push_back(fg2);
+	CHECK(assert_equal(expected, fg1));
+}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;