new templated function combine

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;
+}
+
 /* ************************************************************************* */
 }

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