overload += operator in FactorGraph

gtsam/inference/FactorGraph.h

@@ -29,10 +29,6 @@
 
 #include <Eigen/Core>  // for Eigen::aligned_allocator
 
-#ifdef GTSAM_USE_BOOST_FEATURES
-#include <boost/assign/list_inserter.hpp>
-#endif
-
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
 #include <boost/serialization/nvp.hpp>
 #include <boost/serialization/vector.hpp>
@@ -53,45 +49,6 @@ class BayesTree;
 
 class HybridValues;
 
-/** Helper */
-template <class C>
-class CRefCallPushBack {
-  C& obj;
-
- public:
-  explicit CRefCallPushBack(C& obj) : obj(obj) {}
-  template <typename A>
-  void operator()(const A& a) {
-    obj.push_back(a);
-  }
-};
-
-/** Helper */
-template <class C>
-class RefCallPushBack {
-  C& obj;
-
- public:
-  explicit RefCallPushBack(C& obj) : obj(obj) {}
-  template <typename A>
-  void operator()(A& a) {
-    obj.push_back(a);
-  }
-};
-
-/** Helper */
-template <class C>
-class CRefCallAddCopy {
-  C& obj;
-
- public:
-  explicit CRefCallAddCopy(C& obj) : obj(obj) {}
-  template <typename A>
-  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
+  /// Append factor to factor graph
-  /// `+=` works well with boost::assign list inserter.
   template <class DERIVEDFACTOR>
-  typename std::enable_if<
+  typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value,
-      std::is_base_of<FactorType, DERIVEDFACTOR>::value,
+                          This>::type&
-      boost::assign::list_inserter<RefCallPushBack<This>>>::type
   operator+=(std::shared_ptr<DERIVEDFACTOR> factor) {
-    return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(
+    push_back(factor);
-        factor);
+    return *this;
+  }
+
+  /**
+   * @brief Overload comma operator to allow for append chaining.
+   *
+   * E.g. fg += factor1, factor2, ...
+   */
+  template <class DERIVEDFACTOR>
+  typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value, This>::type& operator,(
+      std::shared_ptr<DERIVEDFACTOR> 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 <class FACTOR_OR_CONTAINER>
-  boost::assign::list_inserter<CRefCallPushBack<This>> operator+=(
+  This& operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) {
-      const FACTOR_OR_CONTAINER& factorOrContainer) {
+    push_back(factorOrContainer);
-    return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(
+    return *this;
-        factorOrContainer);
   }
-#endif
 
   /// @}
   /// @name Specialized versions

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<JacobianFactor>(0, 10 * I_2x2, -1.0 * Vector::Ones(2), unit2);
+  auto f2 = make_shared<JacobianFactor>(0, -10 * I_2x2, 1, 10 * I_2x2,
+                                        Vector2(2.0, -1.0), unit2);
+  auto f3 = make_shared<JacobianFactor>(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: