diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index f1dc37c37..327fca49a 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -29,10 +29,6 @@ #include // for Eigen::aligned_allocator -#ifdef GTSAM_USE_BOOST_FEATURES -#include -#endif - #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION #include #include @@ -53,45 +49,6 @@ class BayesTree; class HybridValues; -/** Helper */ -template -class CRefCallPushBack { - C& obj; - - public: - explicit CRefCallPushBack(C& obj) : obj(obj) {} - template - void operator()(const A& a) { - obj.push_back(a); - } -}; - -/** Helper */ -template -class RefCallPushBack { - C& obj; - - public: - explicit RefCallPushBack(C& obj) : obj(obj) {} - template - void operator()(A& a) { - obj.push_back(a); - } -}; - -/** Helper */ -template -class CRefCallAddCopy { - C& obj; - - public: - explicit CRefCallAddCopy(C& obj) : obj(obj) {} - template - void operator()(const A& a) { - obj.addCopy(a); - } -}; - /** * A factor graph is a bipartite graph with factor nodes connected to variable * nodes. In this class, however, only factor nodes are kept around. @@ -215,17 +172,26 @@ class FactorGraph { push_back(factor); } -#ifdef GTSAM_USE_BOOST_FEATURES - /// `+=` works well with boost::assign list inserter. + /// Append factor to factor graph template - typename std::enable_if< - std::is_base_of::value, - boost::assign::list_inserter>>::type + typename std::enable_if::value, + This>::type& operator+=(std::shared_ptr factor) { - return boost::assign::make_list_inserter(RefCallPushBack(*this))( - factor); + push_back(factor); + return *this; + } + + /** + * @brief Overload comma operator to allow for append chaining. + * + * E.g. fg += factor1, factor2, ... + */ + template + typename std::enable_if::value, This>::type& operator,( + std::shared_ptr factor) { + push_back(factor); + return *this; } -#endif /// @} /// @name Adding via iterators @@ -276,18 +242,15 @@ class FactorGraph { push_back(factorOrContainer); } -#ifdef GTSAM_USE_BOOST_FEATURES /** * Add a factor or container of factors, including STL collections, * BayesTrees, etc. */ template - boost::assign::list_inserter> operator+=( - const FACTOR_OR_CONTAINER& factorOrContainer) { - return boost::assign::make_list_inserter(CRefCallPushBack(*this))( - factorOrContainer); + This& operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) { + push_back(factorOrContainer); + return *this; } -#endif /// @} /// @name Specialized versions diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index e9e626296..41ee9471e 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -70,6 +70,28 @@ TEST(GaussianFactorGraph, initialization) { EQUALITY(expectedIJS, actualIJS); } +/* ************************************************************************* */ +TEST(GaussianFactorGraph, Append) { + // Create empty graph + GaussianFactorGraph fg; + SharedDiagonal unit2 = noiseModel::Unit::Create(2); + + auto f1 = + make_shared(0, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2); + auto f2 = make_shared(0, -10 * I_2x2, 1, 10 * I_2x2, + Vector2(2.0, -1.0), unit2); + auto f3 = make_shared(0, -5 * I_2x2, 2, 5 * I_2x2, + Vector2(0.0, 1.0), unit2); + + fg += f1; + fg += f2; + EXPECT_LONGS_EQUAL(2, fg.size()); + + fg = GaussianFactorGraph(); + fg += f1, f2, f3; + EXPECT_LONGS_EQUAL(3, fg.size()); +} + /* ************************************************************************* */ TEST(GaussianFactorGraph, sparseJacobian) { // Create factor graph: