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& factorGraph, const Ordering& ordering) return bayesNet; } +template +FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2) { + + typedef FactorGraph 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 - boost::shared_ptr _eliminateOne(FactorGraph& factorGraph, const std::string& key); + boost::shared_ptr _eliminateOne( + FactorGraph& factorGraph, + const std::string& key); /** * eliminate factor graph using the given (not necessarily complete) @@ -140,5 +142,14 @@ namespace gtsam { boost::shared_ptr > _eliminate(FactorGraph& 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 + FactorGraph combine(const FactorGraph& fg1, const FactorGraph& fg2); + } // namespace gtsam