From 532be063f9ec68d8b6ac9e5c8ca64724034abd64 Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Sun, 8 Nov 2009 02:50:19 +0000
Subject: [PATCH] new templated function combine

---
 cpp/FactorGraph-inl.h | 16 ++++++++++++++++
 cpp/FactorGraph.h     | 13 ++++++++++++-
 2 files changed, 28 insertions(+), 1 deletion(-)

diff --git a/cpp/FactorGraph-inl.h b/cpp/FactorGraph-inl.h
index 7040d61e3..48d8aaee8 100644
--- a/cpp/FactorGraph-inl.h
+++ b/cpp/FactorGraph-inl.h
@@ -259,5 +259,21 @@ _eliminate(FactorGraph<Factor>& factorGraph, const Ordering& ordering)
 	return bayesNet;
 }
 
+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;
+
+	// add the second factors_ in the graph
+	typename FG::const_iterator factor = fg2.begin();
+	for (; factor!= fg2.end(); factor++)
+		fg.push_back(*factor);
+
+	return fg;
+}
+
 /* ************************************************************************* */
 }
diff --git a/cpp/FactorGraph.h b/cpp/FactorGraph.h
index e04d0819b..caaa2384d 100644
--- a/cpp/FactorGraph.h
+++ b/cpp/FactorGraph.h
@@ -130,7 +130,9 @@ namespace gtsam {
    * and adds a new factor on the separator to the factor graph
    */
 	template<class Factor, class Conditional>
-	boost::shared_ptr<Conditional> _eliminateOne(FactorGraph<Factor>& factorGraph, const std::string& key);
+	boost::shared_ptr<Conditional> _eliminateOne(
+			FactorGraph<Factor>& factorGraph,
+			const std::string& key);
 
 	/**
 	 * eliminate factor graph using the given (not necessarily complete)
@@ -140,5 +142,14 @@ namespace gtsam {
 	boost::shared_ptr<BayesNet<Conditional> >
 	_eliminate(FactorGraph<Factor>& factorGraph, const Ordering& ordering);
 
+  /**
+   * static function that combines two factor graphs
+   * @param const &fg1 Linear factor graph
+   * @param const &fg2 Linear factor graph
+   * @return a new combined factor graph
+   */
+	template<class Factor>
+	FactorGraph<Factor> combine(const FactorGraph<Factor>& fg1, const FactorGraph<Factor>& fg2);
+
 } // namespace gtsam