New add_factors method

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>
+template <class FACTOR>
-  void FactorGraph<FACTOR>::print(const std::string& s, const KeyFormatter& formatter) const {
+void FactorGraph<FACTOR>::print(const std::string& s,
-    std::cout << s << std::endl;
+                                const KeyFormatter& formatter) const {
-    std::cout << "size: " << size() << std::endl;
+  std::cout << s << std::endl;
-    for (size_t i = 0; i < factors_.size(); i++) {
+  std::cout << "size: " << size() << std::endl;
-      std::stringstream ss;
+  for (size_t i = 0; i < factors_.size(); i++) {
-      ss << "factor " << i << ": ";
+    std::stringstream ss;
-      if (factors_[i])
+    ss << "factor " << i << ": ";
-        factors_[i]->print(ss.str(), formatter);
+    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, 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;
+  }
+  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());
+  }
+  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;
     }
+  } 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
-  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>
-  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());
-    }
-    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;
-  }
-
-  /* ************************************************************************* */
-} // namespace gtsam

gtsam/inference/FactorGraph.h

@@ -273,6 +273,14 @@ class FactorGraph {
     bayesTree.addFactorsToGraph(*this);
   }
 
+  /**
+   * 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);
+
   /// @}
   /// @name Testable
   /// @{

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
+  const SymbolicBayesNet expectedBayesNet =
-    (SymbolicConditional(0,1,2))
+      list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3, 4));
-    (SymbolicConditional(1,2,3,4));
 
-  const SymbolicFactorGraph expectedSfg = list_of
+  const SymbolicFactorGraph expectedSfg = list_of(SymbolicFactor(2, 3))(
-    (SymbolicFactor(2,3))
+      SymbolicFactor(4, 5))(SymbolicFactor(2, 3, 4));
-    (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)
+TEST(SymbolicFactorGraph, eliminateFullMultifrontal) {
-{
+  Ordering ordering;
-  Ordering ordering; ordering += 0,1,2,3;
+  ordering += 0, 1, 2, 3;
-  SymbolicBayesTree actual1 =
+  SymbolicBayesTree actual1 = *simpleChain.eliminateMultifrontal(ordering);
-    *simpleChain.eliminateMultifrontal(ordering);
   EXPECT(assert_equal(simpleChainBayesTree, actual1));
 
-  SymbolicBayesTree actual2 =
+  SymbolicBayesTree actual2 = *asiaGraph.eliminateMultifrontal(asiaOrdering);
-    *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::shared_ptr root =
-    SymbolicConditional::FromKeys(list_of(4)(5)(1), 2));
+      boost::make_shared<SymbolicConditional>(
-  expectedBayesTree.insertRoot(boost::make_shared<SymbolicBayesTreeClique>(root));
+          SymbolicConditional::FromKeys(list_of(4)(5)(1), 2));
+  expectedBayesTree.insertRoot(
+      boost::make_shared<SymbolicBayesTreeClique>(root));
 
-  SymbolicFactorGraph expectedFactorGraph = list_of
+  SymbolicFactorGraph expectedFactorGraph =
-    (SymbolicFactor(0,1))
+      list_of(SymbolicFactor(0, 1))(SymbolicFactor(0, 2))(SymbolicFactor(1, 3))(
-    (SymbolicFactor(0,2))
+          SymbolicFactor(2, 3))(SymbolicFactor(1));
-    (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>(
+  SymbolicBayesTreeClique::shared_ptr root2 =
-    boost::make_shared<SymbolicConditional>(4,1));
+      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)
+TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) {
-{
+  SymbolicBayesNet expectedBayesNet =
-  SymbolicBayesNet expectedBayesNet = list_of
+      list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3))(
-    (SymbolicConditional(0, 1, 2))
+          SymbolicConditional(2, 3))(SymbolicConditional(3));
-    (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>(0, 1, 2));
-  expected.push_back(boost::make_shared<SymbolicConditional>(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)
+TEST(SymbolicFactorGraph, marginals) {
-//{
+  // Create factor graph
-//  // Create factor graph
+  SymbolicFactorGraph fg;
-//  SymbolicFactorGraph fg;
+  fg.push_factor(0, 1);
-//  fg.push_factor(0, 1);
+  fg.push_factor(0, 2);
-//  fg.push_factor(0, 2);
+  fg.push_factor(1, 4);
-//  fg.push_factor(1, 4);
+  fg.push_factor(2, 4);
-//  fg.push_factor(2, 4);
+  fg.push_factor(3, 4);
-//  fg.push_factor(3, 4);
+
-//
+  // eliminate
-//  // eliminate
+  Ordering ord(list_of(3)(4)(2)(1)(0));
-//  SymbolicSequentialSolver solver(fg);
+  auto actual = fg.eliminateSequential(ord);
-//  SymbolicBayesNet::shared_ptr actual = solver.eliminate();
+  SymbolicBayesNet expected;
-//  SymbolicBayesNet expected;
+  expected.emplace_shared<SymbolicConditional>(3, 4);
-//  expected.push_front(boost::make_shared<IndexConditional>(4));
+  expected.emplace_shared<SymbolicConditional>(4, 1, 2);
-//  expected.push_front(boost::make_shared<IndexConditional>(3, 4));
+  expected.emplace_shared<SymbolicConditional>(2, 0, 1);
-//  expected.push_front(boost::make_shared<IndexConditional>(2, 4));
+  expected.emplace_shared<SymbolicConditional>(1, 0);
-//  expected.push_front(boost::make_shared<IndexConditional>(1, 2, 4));
+  expected.emplace_shared<SymbolicConditional>(0);
-//  expected.push_front(boost::make_shared<IndexConditional>(0, 1, 2));
+  EXPECT(assert_equal(expected, *actual));
-//  EXPECT(assert_equal(expected,*actual));
+
-//
+  {
-//  {
+    // jointBayesNet
-//    // jointBayesNet
+    Ordering ord(list_of(0)(4)(3));
-//    vector<Index> js;
+    auto actual = fg.eliminatePartialSequential(ord);
-//    js.push_back(0);
+    SymbolicBayesNet expectedBN;
-//    js.push_back(4);
+    expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
-//    js.push_back(3);
+    expectedBN.emplace_shared<SymbolicConditional>(4, 1, 2, 3);
-//    SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js);
+    expectedBN.emplace_shared<SymbolicConditional>(3, 1, 2);
-//    SymbolicBayesNet expectedBN;
+    EXPECT(assert_equal(expectedBN, *(actual.first)));
-//    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));
+    // jointBayesNet
-//
+    Ordering ord(list_of(0)(2)(3));
-//    // jointFactorGraph
+    auto actual = fg.eliminatePartialSequential(ord);
-//    SymbolicFactorGraph::shared_ptr actualFG = solver.jointFactorGraph(js);
+    SymbolicBayesNet expectedBN;
-//    SymbolicFactorGraph expectedFG;
+    expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
-//    expectedFG.push_factor(0, 4);
+    expectedBN.emplace_shared<SymbolicConditional>(2, 1, 4);
-//    expectedFG.push_factor(4, 3);
+    expectedBN.emplace_shared<SymbolicConditional>(3, 4);
-//    expectedFG.push_factor(3);
+    EXPECT(assert_equal(expectedBN, *(actual.first)));
-//    EXPECT( assert_equal(expectedFG,(SymbolicFactorGraph)(*actualFG)));
+  }
-//  }
+
-//
+  {
-//  {
+    // conditionalBayesNet
-//    // jointBayesNet
+    Ordering ord(list_of(0)(2));
-//    vector<Index> js;
+    auto actual = fg.eliminatePartialSequential(ord);
-//    js.push_back(0);
+    SymbolicBayesNet expectedBN;
-//    js.push_back(2);
+    expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
-//    js.push_back(3);
+    expectedBN.emplace_shared<SymbolicConditional>(2, 1, 4);
-//    SymbolicBayesNet::shared_ptr actualBN = solver.jointBayesNet(js);
+    EXPECT(assert_equal(expectedBN, *(actual.first)));
-//    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, 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);
+}
 /* ************************************************************************* */