Merge pull request #1558 from borglab/ordering-helpers

Overload `+=`, `,` operator

gtsam/base/Group.h

@@ -22,13 +22,6 @@
 
 #include <gtsam/base/Testable.h>
 
-#ifdef GTSAM_USE_BOOST_FEATURES
-#include <boost/concept_check.hpp>
-#include <boost/concept/requires.hpp>
-#include <boost/type_traits/is_base_of.hpp>
-#include <boost/static_assert.hpp>
-#endif
-
 #include <utility>
 
 namespace gtsam {

gtsam/base/timing.cpp

@@ -19,9 +19,10 @@
 #include <gtsam/base/debug.h>
 #include <gtsam/base/timing.h>
 
+#include <algorithm>
+#include <cassert>
 #include <cmath>
 #include <cstddef>
-#include <cassert>
 #include <iomanip>
 #include <iostream>
 #include <map>

gtsam/base/types.h

@@ -19,16 +19,11 @@
 
 #pragma once
 
+#include <gtsam/config.h>  // for GTSAM_USE_TBB
 #include <gtsam/dllexport.h>
-#ifdef GTSAM_USE_BOOST_FEATURES
-#include <boost/concept/assert.hpp>
-#include <boost/range/concepts.hpp>
-#endif
-#include <gtsam/config.h> // for GTSAM_USE_TBB
 
 #include <cstddef>
 #include <cstdint>
-
 #include <exception>
 #include <string>
 

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/inference/Ordering.cpp

@@ -281,6 +281,18 @@ void Ordering::print(const std::string& str,
   cout.flush();
 }
 
+/* ************************************************************************* */
+Ordering::This& Ordering::operator+=(Key key) {
+  this->push_back(key);
+  return *this;
+}
+
+/* ************************************************************************* */
+Ordering::This& Ordering::operator,(Key key) {
+  this->push_back(key);
+  return *this;
+}
+
 /* ************************************************************************* */
 Ordering::This& Ordering::operator+=(KeyVector& keys) {
   this->insert(this->end(), keys.begin(), keys.end());

gtsam/inference/Ordering.h

@@ -25,10 +25,6 @@
 #include <gtsam/inference/MetisIndex.h>
 #include <gtsam/base/FastSet.h>
 
-#ifdef GTSAM_USE_BOOST_FEATURES
-#include <boost/assign/list_inserter.hpp>
-#endif
-
 #include <algorithm>
 #include <vector>
 
@@ -61,15 +57,13 @@ public:
       Base(keys.begin(), keys.end()) {
   }
 
-#ifdef GTSAM_USE_BOOST_FEATURES
+  /// Add new variables to the ordering as
-  /// Add new variables to the ordering as ordering += key1, key2, ...  Equivalent to calling
+  /// `ordering += key1, key2, ...`.
-  /// push_back.
+  This& operator+=(Key key);
-  boost::assign::list_inserter<boost::assign_detail::call_push_back<This> > operator+=(
+
-      Key key) {
+  /// Overloading the comma operator allows for chaining appends
-    return boost::assign::make_list_inserter(
+  // e.g. keys += key1, key2
-        boost::assign_detail::call_push_back<This>(*this))(key);
+  This& operator,(Key key);
-  }
-#endif
 
   /**
    * @brief Append new keys to the ordering as `ordering += keys`.

gtsam/inference/tests/testOrdering.cpp

@@ -196,6 +196,20 @@ TEST(Ordering, csr_format_3) {
   EXPECT(adjExpected == adjAcutal);
 }
 
+/* ************************************************************************* */
+TEST(Ordering, AppendKey) {
+  using symbol_shorthand::X;
+  Ordering actual;
+  actual += X(0);
+
+  Ordering expected1{X(0)};
+  EXPECT(assert_equal(expected1, actual));
+
+  actual += X(1), X(2), X(3);
+  Ordering expected2{X(0), X(1), X(2), X(3)};
+  EXPECT(assert_equal(expected2, actual));
+}
+
 /* ************************************************************************* */
 TEST(Ordering, AppendVector) {
   using symbol_shorthand::X;

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: