diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h
index 0aa4af2e16..34ca8ab7fd 100644
--- a/gtsam/inference/FactorGraph-inst.h
+++ b/gtsam/inference/FactorGraph-inst.h
@@ -26,74 +26,105 @@
 #include <boost/bind.hpp>
 
 #include <stdio.h>
+#include <algorithm>
+#include <iostream>  // for cout :-(
 #include <sstream>
-#include <iostream> // for cout :-(
+#include <string>
 
 namespace gtsam {
 
-  /* ************************************************************************* */
-  template<class FACTOR>
-  void FactorGraph<FACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
-    std::cout << s << std::endl;
-    std::cout << "size: " << size() << std::endl;
-    for (size_t i = 0; i < factors_.size(); i++) {
-      std::stringstream ss;
-      ss << "factor " << i << ": ";
-      if (factors_[i])
-        factors_[i]->print(ss.str(), formatter);
-    }
+/* ************************************************************************* */
+template <class FACTOR>
+void FactorGraph<FACTOR>::print(const std::string& s,
+                                const KeyFormatter& formatter) const {
+  std::cout << s << std::endl;
+  std::cout << "size: " << size() << std::endl;
+  for (size_t i = 0; i < factors_.size(); i++) {
+    std::stringstream ss;
+    ss << "factor " << i << ": ";
+    if (factors_[i]) factors_[i]->print(ss.str(), formatter);
   }
-
-  /* ************************************************************************* */
-  template<class FACTOR>
-  bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
-    /** check whether the two factor graphs have the same number of factors_ */
-    if (factors_.size() != fg.size()) return false;
-
-    /** check whether the factors_ are the same */
-    for (size_t i = 0; i < factors_.size(); i++) {
-      // TODO: Doesn't this force order of factor insertion?
-      sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
-      if (f1 == NULL && f2 == NULL) continue;
-      if (f1 == NULL || f2 == NULL) return false;
-      if (!f1->equals(*f2, tol)) return false;
-    }
-    return true;
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
+  // check whether the two factor graphs have the same number of factors.
+  if (factors_.size() != fg.size()) return false;
+
+  // check whether the factors are the same, in same order.
+  for (size_t i = 0; i < factors_.size(); i++) {
+    sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
+    if (f1 == NULL && f2 == NULL) continue;
+    if (f1 == NULL || f2 == NULL) return false;
+    if (!f1->equals(*f2, tol)) return false;
   }
-
-  /* ************************************************************************* */
-  template<class FACTOR>
-  size_t FactorGraph<FACTOR>::nrFactors() const {
-    size_t size_ = 0;
-    for(const sharedFactor& factor: factors_)
-      if (factor) size_++;
-    return size_;
+  return true;
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+size_t FactorGraph<FACTOR>::nrFactors() const {
+  size_t size_ = 0;
+  for (const sharedFactor& factor : factors_)
+    if (factor) size_++;
+  return size_;
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+KeySet FactorGraph<FACTOR>::keys() const {
+  KeySet keys;
+  for (const sharedFactor& factor : this->factors_) {
+    if (factor) keys.insert(factor->begin(), factor->end());
   }
-
-  /* ************************************************************************* */
-  template<class FACTOR>
-  KeySet FactorGraph<FACTOR>::keys() const {
-    KeySet keys;
-    for(const sharedFactor& factor: this->factors_) {
-      if(factor)
-        keys.insert(factor->begin(), factor->end());
+  return keys;
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+KeyVector FactorGraph<FACTOR>::keyVector() const {
+  KeyVector keys;
+  keys.reserve(2 * size());  // guess at size
+  for (const sharedFactor& factor : factors_)
+    if (factor) keys.insert(keys.end(), factor->begin(), factor->end());
+  std::sort(keys.begin(), keys.end());
+  auto last = std::unique(keys.begin(), keys.end());
+  keys.erase(last, keys.end());
+  return keys;
+}
+
+/* ************************************************************************* */
+template <class FACTOR>
+template <typename CONTAINER, typename>
+FactorIndices FactorGraph<FACTOR>::add_factors(const CONTAINER& factors,
+                                               bool useEmptySlots) {
+  const size_t num_factors = factors.size();
+  FactorIndices newFactorIndices(num_factors);
+  if (useEmptySlots) {
+    size_t i = 0;
+    for (size_t j = 0; j < num_factors; ++j) {
+      // Loop to find the next available factor slot
+      do {
+        if (i >= size())
+          // Make room for remaining factors, happens only once.
+          resize(size() + num_factors - j);
+        else if (at(i))
+          ++i;  // Move on to the next slot or past end.
+        else
+          break;  // We found an empty slot, break to fill it.
+      } while (true);
+
+      // Use the current slot, updating graph and newFactorSlots.
+      at(i) = factors[j];
+      newFactorIndices[j] = i;
     }
-    return keys;
-  }
-
-  /* ************************************************************************* */
-  template <class FACTOR>
-  KeyVector FactorGraph<FACTOR>::keyVector() const {
-    KeyVector keys;
-    keys.reserve(2 * size());  // guess at size
-    for (const sharedFactor& factor: factors_)
-      if (factor)
-        keys.insert(keys.end(), factor->begin(), factor->end());
-    std::sort(keys.begin(), keys.end());
-    auto last = std::unique(keys.begin(), keys.end());
-    keys.erase(last, keys.end());
-    return keys;
+  } else {
+    // We're not looking for unused slots, so just add the factors at the end.
+    for (size_t i = 0; i < num_factors; ++i) newFactorIndices[i] = i + size();
+    push_back(factors);
   }
+  return newFactorIndices;
+}
 
-  /* ************************************************************************* */
-} // namespace gtsam
+}  // namespace gtsam
diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h
index 0ecfd87f19..0959989f9d 100644
--- a/gtsam/inference/FactorGraph.h
+++ b/gtsam/inference/FactorGraph.h
@@ -22,350 +22,382 @@
 
 #pragma once
 
-#include <gtsam/base/Testable.h>
 #include <gtsam/base/FastVector.h>
+#include <gtsam/base/Testable.h>
 #include <gtsam/inference/Key.h>
 
 #include <Eigen/Core>  // for Eigen::aligned_allocator
 
-#include <boost/serialization/nvp.hpp>
-#include <boost/serialization/vector.hpp>
 #include <boost/assign/list_inserter.hpp>
 #include <boost/bind.hpp>
 #include <boost/make_shared.hpp>
+#include <boost/serialization/nvp.hpp>
+#include <boost/serialization/vector.hpp>
 
+#include <string>
 #include <type_traits>
 #include <utility>
 
 namespace gtsam {
+/// Define collection type:
+typedef FastVector<FactorIndex> FactorIndices;
+
+// Forward declarations
+template <class CLIQUE>
+class BayesTree;
+
+/** 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);
+  }
+};
 
-  // Forward declarations
-  template<class CLIQUE> class BayesTree;
-
-  /** Helper */
-  template<class C>
-  class CRefCallPushBack
-  {
-    C& obj;
-  public:
-    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:
-    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:
-    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.
+ * \nosubgrouping
+ */
+template <class FACTOR>
+class FactorGraph {
+ public:
+  typedef FACTOR FactorType;  ///< factor type
+  typedef boost::shared_ptr<FACTOR>
+      sharedFactor;  ///< Shared pointer to a factor
+  typedef sharedFactor value_type;
+  typedef typename FastVector<sharedFactor>::iterator iterator;
+  typedef typename FastVector<sharedFactor>::const_iterator const_iterator;
+
+ private:
+  typedef FactorGraph<FACTOR> This;  ///< Typedef for this class
+  typedef boost::shared_ptr<This>
+      shared_ptr;  ///< Shared pointer for this class
+
+  /// Check if a DERIVEDFACTOR is in fact derived from FactorType.
+  template <typename DERIVEDFACTOR>
+  using IsDerived = typename std::enable_if<
+      std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type;
+
+  /// Check if T has a value_type derived from FactorType.
+  template <typename T>
+  using HasDerivedValueType = typename std::enable_if<
+      std::is_base_of<FactorType, typename T::value_type>::value>::type;
+
+  /// Check if T has a value_type derived from FactorType.
+  template <typename T>
+  using HasDerivedElementType = typename std::enable_if<std::is_base_of<
+      FactorType, typename T::value_type::element_type>::value>::type;
+
+ protected:
+  /** concept check, makes sure FACTOR defines print and equals */
+  GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
+
+  /** Collection of factors */
+  FastVector<sharedFactor> factors_;
+
+  /// @name Standard Constructors
+  /// @{
+
+  /** Default constructor */
+  FactorGraph() {}
+
+  /** Constructor from iterator over factors (shared_ptr or plain objects) */
+  template <typename ITERATOR>
+  FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) {
+    push_back(firstFactor, lastFactor);
+  }
+
+  /** Construct from container of factors (shared_ptr or plain objects) */
+  template <class CONTAINER>
+  explicit FactorGraph(const CONTAINER& factors) {
+    push_back(factors);
+  }
+
+  /// @}
+
+ public:
+  /// @name Adding Single Factors
+  /// @{
 
   /**
-   * A factor graph is a bipartite graph with factor nodes connected to variable nodes.
-   * In this class, however, only factor nodes are kept around.
-   * \nosubgrouping
+   * Reserve space for the specified number of factors if you know in
+   * advance how many there will be (works like FastVector::reserve).
    */
-  template<class FACTOR>
-  class FactorGraph {
-
-  public:
-    typedef FACTOR FactorType;  ///< factor type
-    typedef boost::shared_ptr<FACTOR> sharedFactor;  ///< Shared pointer to a factor
-    typedef sharedFactor value_type;
-    typedef typename FastVector<sharedFactor>::iterator iterator;
-    typedef typename FastVector<sharedFactor>::const_iterator const_iterator;
-
-  private:
-    typedef FactorGraph<FACTOR> This;  ///< Typedef for this class
-    typedef boost::shared_ptr<This> shared_ptr;  ///< Shared pointer for this class
-
-  protected:
-    /** concept check, makes sure FACTOR defines print and equals */
-    GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR)
-
-    /** Collection of factors */
-    FastVector<sharedFactor> factors_;
-
-    /// @name Standard Constructors
-    /// @{
-
-    /** Default constructor */
-    FactorGraph() {}
-
-    /** Constructor from iterator over factors (shared_ptr or plain objects) */
-    template<typename ITERATOR>
-    FactorGraph(ITERATOR firstFactor, ITERATOR lastFactor) { push_back(firstFactor, lastFactor); }
-
-    /** Construct from container of factors (shared_ptr or plain objects) */
-    template<class CONTAINER>
-    explicit FactorGraph(const CONTAINER& factors) { push_back(factors); }
-
-    /// @}
-    /// @name Advanced Constructors
-    /// @{
-
-    // TODO: are these needed?
-
-    ///**
-    // * @brief Constructor from a Bayes net
-    // * @param bayesNet the Bayes net to convert, type CONDITIONAL must yield compatible factor
-    // * @return a factor graph with all the conditionals, as factors
-    // */
-    //template<class CONDITIONAL>
-    //FactorGraph(const BayesNet<CONDITIONAL>& bayesNet);
-
-    ///** convert from Bayes tree */
-    //template<class CONDITIONAL, class CLIQUE>
-    //FactorGraph(const BayesTree<CONDITIONAL, CLIQUE>& bayesTree);
-
-    ///** convert from a derived type */
-    //template<class DERIVEDFACTOR>
-    //FactorGraph(const FactorGraph<DERIVEDFACTOR>& factors) {
-    //  factors_.assign(factors.begin(), factors.end());
-    //}
-
-    /// @}
-
-  public:
-    /// @name Adding Factors
-    /// @{
-
-    /**
-     * Reserve space for the specified number of factors if you know in
-     * advance how many there will be (works like FastVector::reserve).
-     */
-    void reserve(size_t size) { factors_.reserve(size); }
-
-    // TODO: are these needed?
-
-    /** Add a factor directly using a shared_ptr */
-    template<class DERIVEDFACTOR>
-    typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
-    push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
-      factors_.push_back(boost::shared_ptr<FACTOR>(factor)); }
-
-    /** Add a factor directly using a shared_ptr */
-    void push_back(const sharedFactor& factor) {
-      factors_.push_back(factor); }
-
-    /** Emplace a factor */
-	template<class DERIVEDFACTOR, class... Args>
-	typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
-		emplace_shared(Args&&... args) {
-		factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(Eigen::aligned_allocator<DERIVEDFACTOR>(), std::forward<Args>(args)...));
-	}
-
-    /** push back many factors with an iterator over shared_ptr (factors are not copied) */
-    template<typename ITERATOR>
-    typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type::element_type>::value>::type
-      push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
-        factors_.insert(end(), firstFactor, lastFactor); }
-
-    /** push back many factors as shared_ptr's in a container (factors are not copied) */
-    template<typename CONTAINER>
-    typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type::element_type>::value>::type
-      push_back(const CONTAINER& container) {
-        push_back(container.begin(), container.end());
-    }
-
-    /** push back a BayesTree as a collection of factors.  NOTE: This should be hidden in derived
-     *  classes in favor of a type-specialized version that calls this templated function. */
-    template<class CLIQUE>
-    typename std::enable_if<std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
-    push_back(const BayesTree<CLIQUE>& bayesTree) {
-      bayesTree.addFactorsToGraph(*this);
-    }
-
-//#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
-    /** Add a factor by value, will be copy-constructed (use push_back with a shared_ptr to avoid
-    *  the copy). */
-    template<class DERIVEDFACTOR>
-    typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
-      push_back(const DERIVEDFACTOR& factor) {
-        factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
-    }
-//#endif
-
-    /** push back many factors with an iterator over plain factors (factors are copied) */
-    template<typename ITERATOR>
-    typename std::enable_if<std::is_base_of<FactorType, typename ITERATOR::value_type>::value>::type
-      push_back(ITERATOR firstFactor, ITERATOR lastFactor) {
-        for (ITERATOR f = firstFactor; f != lastFactor; ++f)
-          push_back(*f);
-    }
-
-    /** push back many factors as non-pointer objects in a container (factors are copied) */
-    template<typename CONTAINER>
-    typename std::enable_if<std::is_base_of<FactorType, typename CONTAINER::value_type>::value>::type
-      push_back(const CONTAINER& container) {
-        push_back(container.begin(), container.end());
-    }
-
-    /** Add a factor directly using a shared_ptr */
-    template<class DERIVEDFACTOR>
-    typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value,
-      boost::assign::list_inserter<RefCallPushBack<This> > >::type
-      operator+=(boost::shared_ptr<DERIVEDFACTOR> factor) {
-        return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(factor);
-    }
-
-    /** Add a factor directly using a shared_ptr */
-    boost::assign::list_inserter<CRefCallPushBack<This> >
-      operator+=(const sharedFactor& factor) {
-        return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(factor);
-    }
-
-    /** Add a factor or container of factors, including STL collections, BayesTrees, etc. */
-    template<class FACTOR_OR_CONTAINER>
-    boost::assign::list_inserter<CRefCallPushBack<This> >
-      operator+=(const FACTOR_OR_CONTAINER& factorOrContainer) {
-        return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(factorOrContainer);
-    }
-
-    /** Add a factor directly using a shared_ptr */
-    template<class DERIVEDFACTOR>
-    typename std::enable_if<std::is_base_of<FactorType, DERIVEDFACTOR>::value>::type
-      add(boost::shared_ptr<DERIVEDFACTOR> factor) {
-        push_back(factor);
-    }
-
-    /** Add a factor directly using a shared_ptr */
-    void add(const sharedFactor& factor) {
-      push_back(factor);
-    }
-
-    /** Add a factor or container of factors, including STL collections, BayesTrees, etc. */
-    template<class FACTOR_OR_CONTAINER>
-    void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
-      push_back(factorOrContainer);
-    }
-
-    /// @}
-    /// @name Testable
-    /// @{
-
-    /** print out graph */
-    void print(const std::string& s = "FactorGraph",
-      const KeyFormatter& formatter = DefaultKeyFormatter) const;
-
-    /** Check equality */
-    bool equals(const This& fg, double tol = 1e-9) const;
-    /// @}
-
-  public:
-    /// @name Standard Interface
-    /// @{
-
-    /** return the number of factors (including any null factors set by remove() ). */
-    size_t size() const { return factors_.size(); }
-
-    /** Check if the graph is empty (null factors set by remove() will cause this to return false). */
-    bool empty() const { return factors_.empty(); }
-
-    /** Get a specific factor by index (this checks array bounds and may throw an exception, as
-     *  opposed to operator[] which does not).
-     */
-    const sharedFactor at(size_t i) const { return factors_.at(i); }
-
-    /** Get a specific factor by index (this checks array bounds and may throw an exception, as
-     *  opposed to operator[] which does not).
-     */
-    sharedFactor& at(size_t i) { return factors_.at(i); }
-
-    /** Get a specific factor by index (this does not check array bounds, as opposed to at() which
-     *  does).
-     */
-    const sharedFactor operator[](size_t i) const { return at(i); }
-
-    /** Get a specific factor by index (this does not check array bounds, as opposed to at() which
-     *  does).
-     */
-    sharedFactor& operator[](size_t i) { return at(i); }
-
-    /** Iterator to beginning of factors. */
-    const_iterator begin() const { return factors_.begin();}
-
-    /** Iterator to end of factors. */
-    const_iterator end()   const { return factors_.end();  }
-
-    /** Get the first factor */
-    sharedFactor front() const { return factors_.front(); }
-
-    /** Get the last factor */
-    sharedFactor back() const { return factors_.back(); }
+  void reserve(size_t size) { factors_.reserve(size); }
+
+  /// Add a factor directly using a shared_ptr.
+  template <class DERIVEDFACTOR>
+  IsDerived<DERIVEDFACTOR> push_back(boost::shared_ptr<DERIVEDFACTOR> factor) {
+    factors_.push_back(boost::shared_ptr<FACTOR>(factor));
+  }
+
+  /// Emplace a shared pointer to factor of given type.
+  template <class DERIVEDFACTOR, class... Args>
+  IsDerived<DERIVEDFACTOR> emplace_shared(Args&&... args) {
+    factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
+        Eigen::aligned_allocator<DERIVEDFACTOR>(),
+        std::forward<Args>(args)...));
+  }
 
-    /// @}
-    /// @name Modifying Factor Graphs (imperative, discouraged)
-    /// @{
+  /**
+   * Add a factor by value, will be copy-constructed (use push_back with a
+   * shared_ptr to avoid the copy).
+   */
+  template <class DERIVEDFACTOR>
+  IsDerived<DERIVEDFACTOR> push_back(const DERIVEDFACTOR& factor) {
+    factors_.push_back(boost::allocate_shared<DERIVEDFACTOR>(
+        Eigen::aligned_allocator<DERIVEDFACTOR>(), factor));
+  }
+
+  /// `add` is a synonym for push_back.
+  template <class DERIVEDFACTOR>
+  IsDerived<DERIVEDFACTOR> add(boost::shared_ptr<DERIVEDFACTOR> factor) {
+    push_back(factor);
+  }
+
+  /// `+=` works well with boost::assign list inserter.
+  template <class DERIVEDFACTOR>
+  typename std::enable_if<
+      std::is_base_of<FactorType, DERIVEDFACTOR>::value,
+      boost::assign::list_inserter<RefCallPushBack<This>>>::type
+  operator+=(boost::shared_ptr<DERIVEDFACTOR> factor) {
+    return boost::assign::make_list_inserter(RefCallPushBack<This>(*this))(
+        factor);
+  }
+
+  /// @}
+  /// @name Adding via iterators
+  /// @{
 
-    /** non-const STL-style begin() */
-    iterator begin()       { return factors_.begin();}
+  /**
+   * Push back many factors with an iterator over shared_ptr (factors are not
+   * copied)
+   */
+  template <typename ITERATOR>
+  HasDerivedElementType<ITERATOR> push_back(ITERATOR firstFactor,
+                                            ITERATOR lastFactor) {
+    factors_.insert(end(), firstFactor, lastFactor);
+  }
+
+  /// Push back many factors with an iterator (factors are copied)
+  template <typename ITERATOR>
+  HasDerivedValueType<ITERATOR> push_back(ITERATOR firstFactor,
+                                          ITERATOR lastFactor) {
+    for (ITERATOR f = firstFactor; f != lastFactor; ++f) push_back(*f);
+  }
+
+  /// @}
+  /// @name Adding via container
+  /// @{
 
-    /** non-const STL-style end() */
-    iterator end()         { return factors_.end();  }
+  /**
+   * Push back many factors as shared_ptr's in a container (factors are not
+   * copied)
+   */
+  template <typename CONTAINER>
+  HasDerivedElementType<CONTAINER> push_back(const CONTAINER& container) {
+    push_back(container.begin(), container.end());
+  }
 
-    /** Directly resize the number of factors in the graph. If the new size is less than the
-     * original, factors at the end will be removed.  If the new size is larger than the original,
-     * null factors will be appended.
-     */
-    void resize(size_t size) { factors_.resize(size); }
+  /// Push back non-pointer objects in a container (factors are copied).
+  template <typename CONTAINER>
+  HasDerivedValueType<CONTAINER> push_back(const CONTAINER& container) {
+    push_back(container.begin(), container.end());
+  }
 
-    /** delete factor without re-arranging indexes by inserting a NULL pointer */
-    void remove(size_t i) { factors_[i].reset();}
+  /**
+   * Add a factor or container of factors, including STL collections,
+   * BayesTrees, etc.
+   */
+  template <class FACTOR_OR_CONTAINER>
+  void add(const FACTOR_OR_CONTAINER& factorOrContainer) {
+    push_back(factorOrContainer);
+  }
 
-    /** replace a factor by index */
-    void replace(size_t index, sharedFactor factor) { at(index) = factor; }
+  /**
+   * Add a factor or container of factors, including STL collections,
+   * BayesTrees, etc.
+   */
+  template <class FACTOR_OR_CONTAINER>
+  boost::assign::list_inserter<CRefCallPushBack<This>> operator+=(
+      const FACTOR_OR_CONTAINER& factorOrContainer) {
+    return boost::assign::make_list_inserter(CRefCallPushBack<This>(*this))(
+        factorOrContainer);
+  }
 
-    /** Erase factor and rearrange other factors to take up the empty space */
-    iterator erase(iterator item) { return factors_.erase(item); }
+  /// @}
+  /// @name Specialized versions
+  /// @{
 
-    /** Erase factors and rearrange other factors to take up the empty space */
-    iterator erase(iterator first, iterator last) { return factors_.erase(first, last); }
+  /**
+   * Push back a BayesTree as a collection of factors.
+   * NOTE: This should be hidden in derived classes in favor of a
+   * type-specialized version that calls this templated function.
+   */
+  template <class CLIQUE>
+  typename std::enable_if<
+      std::is_base_of<This, typename CLIQUE::FactorGraphType>::value>::type
+  push_back(const BayesTree<CLIQUE>& bayesTree) {
+    bayesTree.addFactorsToGraph(*this);
+  }
 
-    /// @}
-    /// @name Advanced Interface
-    /// @{
+  /**
+   * Add new factors to a factor graph and returns a list of new factor indices,
+   * optionally finding and reusing empty factor slots.
+   */
+  template <typename CONTAINER, typename = HasDerivedElementType<CONTAINER>>
+  FactorIndices add_factors(const CONTAINER& factors,
+                            bool useEmptySlots = false);
 
-    /** return the number of non-null factors */
-    size_t nrFactors() const;
+  /// @}
+  /// @name Testable
+  /// @{
 
-    /** Potentially slow function to return all keys involved, sorted, as a set */
-    KeySet keys() const;
+  /** print out graph */
+  void print(const std::string& s = "FactorGraph",
+             const KeyFormatter& formatter = DefaultKeyFormatter) const;
 
-    /** Potentially slow function to return all keys involved, sorted, as a vector */
-    KeyVector keyVector() const;
+  /** Check equality */
+  bool equals(const This& fg, double tol = 1e-9) const;
+  /// @}
 
-    /** MATLAB interface utility: Checks whether a factor index idx exists in the graph and is a live pointer */
-    inline bool exists(size_t idx) const { return idx < size() && at(idx); }
+ public:
+  /// @name Standard Interface
+  /// @{
 
-  private:
+  /** return the number of factors (including any null factors set by remove()
+   * ). */
+  size_t size() const { return factors_.size(); }
 
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
-      ar & BOOST_SERIALIZATION_NVP(factors_);
-    }
+  /** Check if the graph is empty (null factors set by remove() will cause
+   * this to return false). */
+  bool empty() const { return factors_.empty(); }
 
-    /// @}
+  /** Get a specific factor by index (this checks array bounds and may throw
+   * an exception, as opposed to operator[] which does not).
+   */
+  const sharedFactor at(size_t i) const { return factors_.at(i); }
 
-  }; // FactorGraph
+  /** Get a specific factor by index (this checks array bounds and may throw
+   * an exception, as opposed to operator[] which does not).
+   */
+  sharedFactor& at(size_t i) { return factors_.at(i); }
 
-} // namespace gtsam
+  /** Get a specific factor by index (this does not check array bounds, as
+   * opposed to at() which does).
+   */
+  const sharedFactor operator[](size_t i) const { return at(i); }
+
+  /** Get a specific factor by index (this does not check array bounds, as
+   * opposed to at() which does).
+   */
+  sharedFactor& operator[](size_t i) { return at(i); }
+
+  /** Iterator to beginning of factors. */
+  const_iterator begin() const { return factors_.begin(); }
+
+  /** Iterator to end of factors. */
+  const_iterator end() const { return factors_.end(); }
+
+  /** Get the first factor */
+  sharedFactor front() const { return factors_.front(); }
+
+  /** Get the last factor */
+  sharedFactor back() const { return factors_.back(); }
+
+  /// @}
+  /// @name Modifying Factor Graphs (imperative, discouraged)
+  /// @{
+
+  /** non-const STL-style begin() */
+  iterator begin() { return factors_.begin(); }
+
+  /** non-const STL-style end() */
+  iterator end() { return factors_.end(); }
+
+  /** Directly resize the number of factors in the graph. If the new size is
+   * less than the original, factors at the end will be removed.  If the new
+   * size is larger than the original, null factors will be appended.
+   */
+  void resize(size_t size) { factors_.resize(size); }
+
+  /** delete factor without re-arranging indexes by inserting a NULL pointer
+   */
+  void remove(size_t i) { factors_[i].reset(); }
+
+  /** replace a factor by index */
+  void replace(size_t index, sharedFactor factor) { at(index) = factor; }
+
+  /** Erase factor and rearrange other factors to take up the empty space */
+  iterator erase(iterator item) { return factors_.erase(item); }
+
+  /** Erase factors and rearrange other factors to take up the empty space */
+  iterator erase(iterator first, iterator last) {
+    return factors_.erase(first, last);
+  }
+
+  /// @}
+  /// @name Advanced Interface
+  /// @{
+
+  /** return the number of non-null factors */
+  size_t nrFactors() const;
+
+  /** Potentially slow function to return all keys involved, sorted, as a set
+   */
+  KeySet keys() const;
+
+  /** Potentially slow function to return all keys involved, sorted, as a
+   * vector
+   */
+  KeyVector keyVector() const;
+
+  /** MATLAB interface utility: Checks whether a factor index idx exists in
+   * the graph and is a live pointer */
+  inline bool exists(size_t idx) const { return idx < size() && at(idx); }
+
+ private:
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    ar& BOOST_SERIALIZATION_NVP(factors_);
+  }
+
+  /// @}
+};  // FactorGraph
+}  // namespace gtsam
 
 #include <gtsam/inference/FactorGraph-inst.h>
diff --git a/gtsam/linear/GaussianBayesTree.cpp b/gtsam/linear/GaussianBayesTree.cpp
index 2365388d6d..8d0fafb618 100644
--- a/gtsam/linear/GaussianBayesTree.cpp
+++ b/gtsam/linear/GaussianBayesTree.cpp
@@ -35,12 +35,15 @@ namespace gtsam {
   namespace internal
   {
     /* ************************************************************************* */
-    double logDeterminant(const GaussianBayesTreeClique::shared_ptr& clique, double& parentSum)
-    {
-      parentSum += clique->conditional()->R().diagonal().unaryExpr(std::ptr_fun<double,double>(log)).sum();
-      assert(false);
-      return 0;
-    }
+  double logDeterminant(const GaussianBayesTreeClique::shared_ptr& clique,
+                        double& parentSum) {
+    parentSum += clique->conditional()
+                     ->R()
+                     .diagonal()
+                     .unaryExpr(std::ptr_fun<double, double>(log))
+                     .sum();
+    return 0;
+  }
   }
 
   /* ************************************************************************* */
diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
index 3fd318456d..8f4eb3c08d 100644
--- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
+++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
@@ -15,14 +15,16 @@
  *  @author Christian Potthast
  **/
 
-#include <CppUnitLite/TestHarness.h>
-
 #include <gtsam/symbolic/SymbolicFactorGraph.h>
+
+#include <gtsam/base/TestableAssertions.h>
 #include <gtsam/symbolic/SymbolicBayesNet.h>
 #include <gtsam/symbolic/SymbolicBayesTree.h>
 #include <gtsam/symbolic/SymbolicConditional.h>
 #include <gtsam/symbolic/tests/symbolicExampleGraphs.h>
 
+#include <CppUnitLite/TestHarness.h>
+
 #include <boost/assign/std/set.hpp>
 
 using namespace std;
@@ -46,11 +48,10 @@ TEST(SymbolicFactorGraph, keys2) {
 }
 
 /* ************************************************************************* */
-TEST(SymbolicFactorGraph, eliminateFullSequential)
-{
+TEST(SymbolicFactorGraph, eliminateFullSequential) {
   // Test with simpleTestGraph1
   Ordering order;
-  order += 0,1,2,3,4;
+  order += 0, 1, 2, 3, 4;
   SymbolicBayesNet actual1 = *simpleTestGraph1.eliminateSequential(order);
   EXPECT(assert_equal(simpleTestGraph1BayesNet, actual1));
 
@@ -60,24 +61,20 @@ TEST(SymbolicFactorGraph, eliminateFullSequential)
 }
 
 /* ************************************************************************* */
-TEST(SymbolicFactorGraph, eliminatePartialSequential)
-{
+TEST(SymbolicFactorGraph, eliminatePartialSequential) {
   // Eliminate 0 and 1
   const Ordering order = list_of(0)(1);
 
-  const SymbolicBayesNet expectedBayesNet = list_of
-    (SymbolicConditional(0,1,2))
-    (SymbolicConditional(1,2,3,4));
+  const SymbolicBayesNet expectedBayesNet =
+      list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3, 4));
 
-  const SymbolicFactorGraph expectedSfg = list_of
-    (SymbolicFactor(2,3))
-    (SymbolicFactor(4,5))
-    (SymbolicFactor(2,3,4));
+  const SymbolicFactorGraph expectedSfg = list_of(SymbolicFactor(2, 3))(
+      SymbolicFactor(4, 5))(SymbolicFactor(2, 3, 4));
 
   SymbolicBayesNet::shared_ptr actualBayesNet;
   SymbolicFactorGraph::shared_ptr actualSfg;
   boost::tie(actualBayesNet, actualSfg) =
-    simpleTestGraph2.eliminatePartialSequential(Ordering(list_of(0)(1)));
+      simpleTestGraph2.eliminatePartialSequential(Ordering(list_of(0)(1)));
 
   EXPECT(assert_equal(expectedSfg, *actualSfg));
   EXPECT(assert_equal(expectedBayesNet, *actualBayesNet));
@@ -85,75 +82,71 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential)
   SymbolicBayesNet::shared_ptr actualBayesNet2;
   SymbolicFactorGraph::shared_ptr actualSfg2;
   boost::tie(actualBayesNet2, actualSfg2) =
-    simpleTestGraph2.eliminatePartialSequential(list_of(0)(1).convert_to_container<KeyVector >());
+      simpleTestGraph2.eliminatePartialSequential(
+          list_of(0)(1).convert_to_container<KeyVector>());
 
   EXPECT(assert_equal(expectedSfg, *actualSfg2));
   EXPECT(assert_equal(expectedBayesNet, *actualBayesNet2));
 }
 
 /* ************************************************************************* */
-TEST(SymbolicFactorGraph, eliminateFullMultifrontal)
-{
-  Ordering ordering; ordering += 0,1,2,3;
-  SymbolicBayesTree actual1 =
-    *simpleChain.eliminateMultifrontal(ordering);
+TEST(SymbolicFactorGraph, eliminateFullMultifrontal) {
+  Ordering ordering;
+  ordering += 0, 1, 2, 3;
+  SymbolicBayesTree actual1 = *simpleChain.eliminateMultifrontal(ordering);
   EXPECT(assert_equal(simpleChainBayesTree, actual1));
 
-  SymbolicBayesTree actual2 =
-    *asiaGraph.eliminateMultifrontal(asiaOrdering);
+  SymbolicBayesTree actual2 = *asiaGraph.eliminateMultifrontal(asiaOrdering);
   EXPECT(assert_equal(asiaBayesTree, actual2));
 }
 
 /* ************************************************************************* */
-TEST(SymbolicFactorGraph, eliminatePartialMultifrontal)
-{
+TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) {
   SymbolicBayesTree expectedBayesTree;
-  SymbolicConditional::shared_ptr root = boost::make_shared<SymbolicConditional>(
-    SymbolicConditional::FromKeys(list_of(4)(5)(1), 2));
-  expectedBayesTree.insertRoot(boost::make_shared<SymbolicBayesTreeClique>(root));
+  SymbolicConditional::shared_ptr root =
+      boost::make_shared<SymbolicConditional>(
+          SymbolicConditional::FromKeys(list_of(4)(5)(1), 2));
+  expectedBayesTree.insertRoot(
+      boost::make_shared<SymbolicBayesTreeClique>(root));
 
-  SymbolicFactorGraph expectedFactorGraph = list_of
-    (SymbolicFactor(0,1))
-    (SymbolicFactor(0,2))
-    (SymbolicFactor(1,3))
-    (SymbolicFactor(2,3))
-    (SymbolicFactor(1));
+  SymbolicFactorGraph expectedFactorGraph =
+      list_of(SymbolicFactor(0, 1))(SymbolicFactor(0, 2))(SymbolicFactor(1, 3))(
+          SymbolicFactor(2, 3))(SymbolicFactor(1));
 
   SymbolicBayesTree::shared_ptr actualBayesTree;
   SymbolicFactorGraph::shared_ptr actualFactorGraph;
   boost::tie(actualBayesTree, actualFactorGraph) =
-    simpleTestGraph2.eliminatePartialMultifrontal(Ordering(list_of(4)(5)));
+      simpleTestGraph2.eliminatePartialMultifrontal(Ordering(list_of(4)(5)));
 
   EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph));
   EXPECT(assert_equal(expectedBayesTree, *actualBayesTree));
 
   SymbolicBayesTree expectedBayesTree2;
-  SymbolicBayesTreeClique::shared_ptr root2 = boost::make_shared<SymbolicBayesTreeClique>(
-    boost::make_shared<SymbolicConditional>(4,1));
+  SymbolicBayesTreeClique::shared_ptr root2 =
+      boost::make_shared<SymbolicBayesTreeClique>(
+          boost::make_shared<SymbolicConditional>(4, 1));
   root2->children.push_back(boost::make_shared<SymbolicBayesTreeClique>(
-    boost::make_shared<SymbolicConditional>(5,4)));
+      boost::make_shared<SymbolicConditional>(5, 4)));
   expectedBayesTree2.insertRoot(root2);
 
   SymbolicBayesTree::shared_ptr actualBayesTree2;
   SymbolicFactorGraph::shared_ptr actualFactorGraph2;
   boost::tie(actualBayesTree2, actualFactorGraph2) =
-    simpleTestGraph2.eliminatePartialMultifrontal(list_of<Key>(4)(5).convert_to_container<KeyVector >());
+      simpleTestGraph2.eliminatePartialMultifrontal(
+          list_of<Key>(4)(5).convert_to_container<KeyVector>());
 
   EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph2));
   EXPECT(assert_equal(expectedBayesTree2, *actualBayesTree2));
 }
 
 /* ************************************************************************* */
-TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet)
-{
-  SymbolicBayesNet expectedBayesNet = list_of
-    (SymbolicConditional(0, 1, 2))
-    (SymbolicConditional(1, 2, 3))
-    (SymbolicConditional(2, 3))
-    (SymbolicConditional(3));
+TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) {
+  SymbolicBayesNet expectedBayesNet =
+      list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3))(
+          SymbolicConditional(2, 3))(SymbolicConditional(3));
 
   SymbolicBayesNet actual1 = *simpleTestGraph2.marginalMultifrontalBayesNet(
-    Ordering(list_of(0)(1)(2)(3)));
+      Ordering(list_of(0)(1)(2)(3)));
   EXPECT(assert_equal(expectedBayesNet, actual1));
 }
 
@@ -167,104 +160,75 @@ TEST(SymbolicFactorGraph, eliminate_disconnected_graph) {
 
   // create expected Chordal bayes Net
   SymbolicBayesNet expected;
-  expected.push_back(boost::make_shared<SymbolicConditional>(0,1,2));
-  expected.push_back(boost::make_shared<SymbolicConditional>(1,2));
+  expected.push_back(boost::make_shared<SymbolicConditional>(0, 1, 2));
+  expected.push_back(boost::make_shared<SymbolicConditional>(1, 2));
   expected.push_back(boost::make_shared<SymbolicConditional>(2));
-  expected.push_back(boost::make_shared<SymbolicConditional>(3,4));
+  expected.push_back(boost::make_shared<SymbolicConditional>(3, 4));
   expected.push_back(boost::make_shared<SymbolicConditional>(4));
 
   Ordering order;
-  order += 0,1,2,3,4;
+  order += 0, 1, 2, 3, 4;
   SymbolicBayesNet actual = *fg.eliminateSequential(order);
 
-  EXPECT(assert_equal(expected,actual));
+  EXPECT(assert_equal(expected, actual));
 }
 
 /* ************************************************************************* */
-//TEST(SymbolicFactorGraph, marginals)
-//{
-//  // Create factor graph
-//  SymbolicFactorGraph fg;
-//  fg.push_factor(0, 1);
-//  fg.push_factor(0, 2);
-//  fg.push_factor(1, 4);
-//  fg.push_factor(2, 4);
-//  fg.push_factor(3, 4);
-//
-//  // eliminate
-//  SymbolicSequentialSolver solver(fg);
-//  SymbolicBayesNet::shared_ptr actual = solver.eliminate();
-//  SymbolicBayesNet expected;
-//  expected.push_front(boost::make_shared<IndexConditional>(4));
-//  expected.push_front(boost::make_shared<IndexConditional>(3, 4));
-//  expected.push_front(boost::make_shared<IndexConditional>(2, 4));
-//  expected.push_front(boost::make_shared<IndexConditional>(1, 2, 4));
-//  expected.push_front(boost::make_shared<IndexConditional>(0, 1, 2));
-//  EXPECT(assert_equal(expected,*actual));
-//
-//  {
-//    // jointBayesNet
-//    vector<Index> js;
-//    js.push_back(0);
-//    js.push_back(4);
-//    js.push_back(3);
-//    SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js);
-//    SymbolicBayesNet expectedBN;
-//    expectedBN.push_front(boost::make_shared<IndexConditional>(3));
-//    expectedBN.push_front(boost::make_shared<IndexConditional>(4, 3));
-//    expectedBN.push_front(boost::make_shared<IndexConditional>(0, 4));
-//    EXPECT( assert_equal(expectedBN,*actualBN));
-//
-//    // jointFactorGraph
-//    SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js);
-//    SymbolicFactorGraph expectedFG;
-//    expectedFG.push_factor(0, 4);
-//    expectedFG.push_factor(4, 3);
-//    expectedFG.push_factor(3);
-//    EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG)));
-//  }
-//
-//  {
-//    // jointBayesNet
-//    vector<Index> js;
-//    js.push_back(0);
-//    js.push_back(2);
-//    js.push_back(3);
-//    SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js);
-//    SymbolicBayesNet expectedBN;
-//    expectedBN.push_front(boost::make_shared<IndexConditional>(2));
-//    expectedBN.push_front(boost::make_shared<IndexConditional>(3, 2));
-//    expectedBN.push_front(boost::make_shared<IndexConditional>(0, 3, 2));
-//    EXPECT( assert_equal(expectedBN,*actualBN));
-//
-//    // jointFactorGraph
-//    SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js);
-//    SymbolicFactorGraph expectedFG;
-//    expectedFG.push_factor(0, 3, 2);
-//    expectedFG.push_factor(3, 2);
-//    expectedFG.push_factor(2);
-//    EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG)));
-//  }
-//
-//  {
-//    // conditionalBayesNet
-//    vector<Index> js;
-//    js.push_back(0);
-//    js.push_back(2);
-//    js.push_back(3);
-//    size_t nrFrontals = 2;
-//    SymbolicBayesNet::shared_ptr actualBN = //
-//      solver.conditionalBayesNet(js, nrFrontals);
-//    SymbolicBayesNet expectedBN;
-//    expectedBN.push_front(boost::make_shared<IndexConditional>(2, 3));
-//    expectedBN.push_front(boost::make_shared<IndexConditional>(0, 2, 3));
-//    EXPECT( assert_equal(expectedBN,*actualBN));
-//  }
-//}
+TEST(SymbolicFactorGraph, marginals) {
+  // Create factor graph
+  SymbolicFactorGraph fg;
+  fg.push_factor(0, 1);
+  fg.push_factor(0, 2);
+  fg.push_factor(1, 4);
+  fg.push_factor(2, 4);
+  fg.push_factor(3, 4);
+
+  // eliminate
+  Ordering ord(list_of(3)(4)(2)(1)(0));
+  auto actual = fg.eliminateSequential(ord);
+  SymbolicBayesNet expected;
+  expected.emplace_shared<SymbolicConditional>(3, 4);
+  expected.emplace_shared<SymbolicConditional>(4, 1, 2);
+  expected.emplace_shared<SymbolicConditional>(2, 0, 1);
+  expected.emplace_shared<SymbolicConditional>(1, 0);
+  expected.emplace_shared<SymbolicConditional>(0);
+  EXPECT(assert_equal(expected, *actual));
+
+  {
+    // jointBayesNet
+    Ordering ord(list_of(0)(4)(3));
+    auto actual = fg.eliminatePartialSequential(ord);
+    SymbolicBayesNet expectedBN;
+    expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
+    expectedBN.emplace_shared<SymbolicConditional>(4, 1, 2, 3);
+    expectedBN.emplace_shared<SymbolicConditional>(3, 1, 2);
+    EXPECT(assert_equal(expectedBN, *(actual.first)));
+  }
+
+  {
+    // jointBayesNet
+    Ordering ord(list_of(0)(2)(3));
+    auto actual = fg.eliminatePartialSequential(ord);
+    SymbolicBayesNet expectedBN;
+    expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
+    expectedBN.emplace_shared<SymbolicConditional>(2, 1, 4);
+    expectedBN.emplace_shared<SymbolicConditional>(3, 4);
+    EXPECT(assert_equal(expectedBN, *(actual.first)));
+  }
+
+  {
+    // conditionalBayesNet
+    Ordering ord(list_of(0)(2));
+    auto actual = fg.eliminatePartialSequential(ord);
+    SymbolicBayesNet expectedBN;
+    expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
+    expectedBN.emplace_shared<SymbolicConditional>(2, 1, 4);
+    EXPECT(assert_equal(expectedBN, *(actual.first)));
+  }
+}
 
 /* ************************************************************************* */
-TEST( SymbolicFactorGraph, constructFromBayesNet )
-{
+TEST(SymbolicFactorGraph, constructFromBayesNet) {
   // create expected factor graph
   SymbolicFactorGraph expected;
   expected.push_factor(0, 1, 2);
@@ -284,8 +248,7 @@ TEST( SymbolicFactorGraph, constructFromBayesNet )
 }
 
 /* ************************************************************************* */
-TEST( SymbolicFactorGraph, constructFromBayesTree )
-{
+TEST(SymbolicFactorGraph, constructFromBayesTree) {
   // create expected factor graph
   SymbolicFactorGraph expected;
   expected.push_factor(_E_, _L_, _B_);
@@ -300,8 +263,7 @@ TEST( SymbolicFactorGraph, constructFromBayesTree )
 }
 
 /* ************************************************************************* */
-TEST( SymbolicFactorGraph, push_back )
-{
+TEST(SymbolicFactorGraph, push_back) {
   // Create two factor graphs and expected combined graph
   SymbolicFactorGraph fg1, fg2, expected;
 
@@ -321,8 +283,47 @@ TEST( SymbolicFactorGraph, push_back )
   actual.push_back(fg1);
   actual.push_back(fg2);
   CHECK(assert_equal(expected, actual));
+
+  // combine in second way
+  SymbolicFactorGraph actual2 = fg1;
+  actual2.push_back(fg2);
+  CHECK(assert_equal(expected, actual2));
 }
 
 /* ************************************************************************* */
-int main() {  TestResult tr; return TestRegistry::runAllTests(tr); }
+TEST(SymbolicFactorGraph, add_factors) {
+  SymbolicFactorGraph fg1;
+  fg1.push_factor(10);
+  fg1 += SymbolicFactor::shared_ptr();  // empty slot!
+  fg1.push_factor(11);
+
+  SymbolicFactorGraph fg2;
+  fg2.push_factor(1);
+  fg2.push_factor(2);
+
+  SymbolicFactorGraph expected;
+  expected.push_factor(10);
+  expected.push_factor(1);
+  expected.push_factor(11);
+  expected.push_factor(2);
+  const FactorIndices expectedIndices = list_of(1)(3);
+  const FactorIndices actualIndices = fg1.add_factors(fg2, true);
+
+  EXPECT(assert_equal(expected, fg1));
+  EXPECT(assert_container_equality(expectedIndices, actualIndices));
+
+  expected.push_factor(1);
+  expected.push_factor(2);
+  const FactorIndices expectedIndices2 = list_of(4)(5);
+  const FactorIndices actualIndices2 = fg1.add_factors(fg2, false);
+
+  EXPECT(assert_equal(expected, fg1));
+  EXPECT(assert_container_equality(expectedIndices2, actualIndices2));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */