From 4e078e41f18cad5256c40c7bee083e18d184b363 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 7 Jan 2023 23:17:54 -0800 Subject: [PATCH] Heavy lift on removing boost::assign from symbolic tests. --- gtsam/symbolic/SymbolicBayesNet.h | 16 + gtsam/symbolic/SymbolicFactorGraph.h | 16 + .../symbolic/tests/testSymbolicBayesTree.cpp | 534 +++++++++--------- .../tests/testSymbolicEliminationTree.cpp | 110 ++-- gtsam/symbolic/tests/testSymbolicFactor.cpp | 1 - .../tests/testSymbolicFactorGraph.cpp | 44 +- 6 files changed, 379 insertions(+), 342 deletions(-) diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 33c77c6de..43f1edd69 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -70,6 +70,22 @@ namespace gtsam { for (auto&& f : sharedFactors) factors_.push_back(f); } + /// Construct from a single conditional + SymbolicBayesNet(SymbolicConditional&& c) { + push_back(boost::make_shared(c)); + } + + /** + * @brief Add a single conditional and return a reference. + * This allows for chaining, e.g., + * SymbolicBayesNet bn = + * SymbolicBayesNet(SymbolicConditional(...))(SymbolicConditional(...)); + */ + SymbolicBayesNet& operator()(SymbolicConditional&& c) { + push_back(boost::make_shared(c)); + return *this; + } + /// Destructor virtual ~SymbolicBayesNet() {} diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 95892eec8..6b4c456fa 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -90,6 +90,22 @@ namespace gtsam { for (auto&& f : sharedFactors) factors_.push_back(f); } + /// Construct from a single factor + SymbolicFactorGraph(SymbolicFactor&& c) { + push_back(boost::make_shared(c)); + } + + /** + * @brief Add a single factor and return a reference. + * This allows for chaining, e.g., + * SymbolicFactorGraph bn = + * SymbolicFactorGraph(SymbolicFactor(...))(SymbolicFactor(...)); + */ + SymbolicFactorGraph& operator()(SymbolicFactor&& c) { + push_back(boost::make_shared(c)); + return *this; + } + /// Destructor virtual ~SymbolicFactorGraph() {} diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index ee9b41a5a..c5ea5a792 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -17,16 +17,12 @@ * @author Viorela Ila */ -#include -#include #include +#include +#include #include -#include -#include -#include #include -using namespace boost::assign; using boost::adaptors::indirected; #include @@ -38,37 +34,50 @@ using namespace gtsam::symbol_shorthand; static bool debug = false; +using sharedClique = SymbolicBayesTreeClique::shared_ptr; + +template +class ListOf { + public: + ListOf(const T& c) { result.push_back(c); } + + ListOf& operator()(const T& c) { + result.push_back(c); + return *this; + } + + operator std::vector() { return result; } + + private: + std::vector result; +}; + +using MakeKeys = ListOf; +using MakeCliques = ListOf; + namespace { - - /* ************************************************************************* */ - // Helper functions for below - template - SymbolicBayesTreeClique::shared_ptr MakeClique(const KEYS& keys, DenseIndex nrFrontals) - { - return boost::make_shared( +/* ************************************************************************* */ +// Helper functions for below +sharedClique MakeClique(const KeyVector& keys, DenseIndex nrFrontals) { + return boost::make_shared( boost::make_shared( - SymbolicConditional::FromKeys(keys, nrFrontals))); - } - - template - SymbolicBayesTreeClique::shared_ptr MakeClique( - const KEYS& keys, DenseIndex nrFrontals, const CHILDREN& children) - { - SymbolicBayesTreeClique::shared_ptr clique = - boost::make_shared( - boost::make_shared( - SymbolicConditional::FromKeys(keys, nrFrontals))); - clique->children.assign(children.begin(), children.end()); - for(typename CHILDREN::const_iterator child = children.begin(); child != children.end(); ++child) - (*child)->parent_ = clique; - return clique; - } - + SymbolicConditional::FromKeys(keys, nrFrontals))); } +sharedClique MakeClique(const KeyVector& keys, DenseIndex nrFrontals, + const std::vector& children) { + sharedClique clique = boost::make_shared( + boost::make_shared( + SymbolicConditional::FromKeys(keys, nrFrontals))); + clique->children.assign(children.begin(), children.end()); + for (auto&& child : children) child->parent_ = clique; + return clique; +} + +} // namespace + /* ************************************************************************* */ -TEST(SymbolicBayesTree, clear) -{ +TEST(SymbolicBayesTree, clear) { SymbolicBayesTree bayesTree = asiaBayesTree; bayesTree.clear(); @@ -79,34 +88,35 @@ TEST(SymbolicBayesTree, clear) } /* ************************************************************************* */ -TEST(SymbolicBayesTree, clique_structure) -{ +TEST(SymbolicBayesTree, clique_structure) { // l1 l2 // / | / | // x1 --- x2 --- x3 --- x4 --- x5 // \ | // l3 SymbolicFactorGraph graph; - graph += SymbolicFactor(X(1), L(1)); - graph += SymbolicFactor(X(1), X(2)); - graph += SymbolicFactor(X(2), L(1)); - graph += SymbolicFactor(X(2), X(3)); - graph += SymbolicFactor(X(3), X(4)); - graph += SymbolicFactor(X(4), L(2)); - graph += SymbolicFactor(X(4), X(5)); - graph += SymbolicFactor(L(2), X(5)); - graph += SymbolicFactor(X(4), L(3)); - graph += SymbolicFactor(X(5), L(3)); + graph.emplace_shared(X(1), L(1)); + graph.emplace_shared(X(1), X(2)); + graph.emplace_shared(X(2), L(1)); + graph.emplace_shared(X(2), X(3)); + graph.emplace_shared(X(3), X(4)); + graph.emplace_shared(X(4), L(2)); + graph.emplace_shared(X(4), X(5)); + graph.emplace_shared(L(2), X(5)); + graph.emplace_shared(X(4), L(3)); + graph.emplace_shared(X(5), L(3)); SymbolicBayesTree expected; - expected.insertRoot( - MakeClique(list_of(X(2)) (X(3)), 2, list_of - (MakeClique(list_of(X(4)) (X(3)), 1, list_of - (MakeClique(list_of(X(5)) (L(2)) (X(4)), 2, list_of - (MakeClique(list_of(L(3)) (X(4)) (X(5)), 1)))))) - (MakeClique(list_of(X(1)) (L(1)) (X(2)), 2)))); + expected.insertRoot(MakeClique( + MakeKeys(X(2))(X(3)), 2, + MakeCliques(MakeClique( + MakeKeys(X(4))(X(3)), 1, + MakeCliques(MakeClique( + MakeKeys(X(5))(L(2))(X(4)), 2, + MakeCliques(MakeClique(MakeKeys(L(3))(X(4))(X(5)), 1))))))( + MakeClique(MakeKeys(X(1))(L(1))(X(2)), 2)))); - Ordering order = list_of(X(1)) (L(3)) (L(1)) (X(5)) (X(2)) (L(2)) (X(4)) (X(3)); + Ordering order{X(1), L(3), L(1), X(5), X(2), L(2), X(4), X(3)}; SymbolicBayesTree actual = *graph.eliminateMultifrontal(order); @@ -120,56 +130,56 @@ Bayes Tree for testing conversion to a forest of orphans needed for incremental. D|C F|E */ /* ************************************************************************* */ -TEST( BayesTree, removePath ) -{ - const Key _A_=A(0), _B_=B(0), _C_=C(0), _D_=D(0), _E_=E(0), _F_=F(0); +TEST(BayesTree, removePath) { + const Key _A_ = A(0), _B_ = B(0), _C_ = C(0), _D_ = D(0), _E_ = E(0), + _F_ = F(0); SymbolicBayesTree bayesTreeOrig; - bayesTreeOrig.insertRoot( - MakeClique(list_of(_A_)(_B_), 2, list_of - (MakeClique(list_of(_C_)(_A_), 1, list_of - (MakeClique(list_of(_D_)(_C_), 1)))) - (MakeClique(list_of(_E_)(_B_), 1, list_of - (MakeClique(list_of(_F_)(_E_), 1)))))); + bayesTreeOrig.insertRoot(MakeClique( + MakeKeys(_A_)(_B_), 2, + MakeCliques(MakeClique(MakeKeys(_C_)(_A_), 1, + MakeCliques(MakeClique(MakeKeys(_D_)(_C_), 1))))( + MakeClique(MakeKeys(_E_)(_B_), 1, + MakeCliques(MakeClique(MakeKeys(_F_)(_E_), 1)))))); SymbolicBayesTree bayesTree = bayesTreeOrig; // remove C, expected outcome: factor graph with ABC, // Bayes Tree now contains two orphan trees: D|C and E|B,F|E SymbolicFactorGraph expected; - expected += SymbolicFactor(_A_,_B_); - expected += SymbolicFactor(_C_,_A_); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_D_], bayesTree[_E_]; + expected.emplace_shared(_A_, _B_); + expected.emplace_shared(_C_, _A_); + SymbolicBayesTree::Cliques expectedOrphans = + std::list{bayesTree[_D_], bayesTree[_E_]}; SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; bayesTree.removePath(bayesTree[_C_], &bn, &orphans); SymbolicFactorGraph factors(bn); CHECK(assert_equal(expected, factors)); - CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); + CHECK(assert_container_equal(expectedOrphans | indirected, + orphans | indirected)); bayesTree = bayesTreeOrig; // remove E: factor graph with EB; E|B removed from second orphan tree SymbolicFactorGraph expected2; - expected2 += SymbolicFactor(_A_,_B_); - expected2 += SymbolicFactor(_E_,_B_); - SymbolicBayesTree::Cliques expectedOrphans2; - expectedOrphans2 += bayesTree[_F_]; - expectedOrphans2 += bayesTree[_C_]; + expected2.emplace_shared(_A_, _B_); + expected2.emplace_shared(_E_, _B_); + SymbolicBayesTree::Cliques expectedOrphans2 = + std::list{bayesTree[_F_], bayesTree[_C_]}; SymbolicBayesNet bn2; SymbolicBayesTree::Cliques orphans2; bayesTree.removePath(bayesTree[_E_], &bn2, &orphans2); SymbolicFactorGraph factors2(bn2); CHECK(assert_equal(expected2, factors2)); - CHECK(assert_container_equal(expectedOrphans2|indirected, orphans2|indirected)); + CHECK(assert_container_equal(expectedOrphans2 | indirected, + orphans2 | indirected)); } /* ************************************************************************* */ -TEST( BayesTree, removePath2 ) -{ +TEST(BayesTree, removePath2) { SymbolicBayesTree bayesTree = asiaBayesTree; // Call remove-path with clique B @@ -180,16 +190,16 @@ TEST( BayesTree, removePath2 ) // Check expected outcome SymbolicFactorGraph expected; - expected += SymbolicFactor(_E_,_L_,_B_); + expected.emplace_shared(_E_, _L_, _B_); CHECK(assert_equal(expected, factors)); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_S_], bayesTree[_T_], bayesTree[_X_]; - CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); + SymbolicBayesTree::Cliques expectedOrphans = + std::list{bayesTree[_S_], bayesTree[_T_], bayesTree[_X_]}; + CHECK(assert_container_equal(expectedOrphans | indirected, + orphans | indirected)); } /* ************************************************************************* */ -TEST(BayesTree, removePath3) -{ +TEST(BayesTree, removePath3) { SymbolicBayesTree bayesTree = asiaBayesTree; // Call remove-path with clique T @@ -200,199 +210,185 @@ TEST(BayesTree, removePath3) // Check expected outcome SymbolicFactorGraph expected; - expected += SymbolicFactor(_E_, _L_, _B_); - expected += SymbolicFactor(_T_, _E_, _L_); + expected.emplace_shared(_E_, _L_, _B_); + expected.emplace_shared(_T_, _E_, _L_); CHECK(assert_equal(expected, factors)); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_S_], bayesTree[_X_]; - CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); + SymbolicBayesTree::Cliques expectedOrphans = + std::list{bayesTree[_S_], bayesTree[_X_]}; + CHECK(assert_container_equal(expectedOrphans | indirected, + orphans | indirected)); } -void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, SymbolicBayesTree::Cliques& cliques) { +void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, + SymbolicBayesTree::Cliques& cliques) { // Check if subtree exists if (subtree) { cliques.push_back(subtree); // Recursive call over all child cliques - for(SymbolicBayesTree::sharedClique& childClique: subtree->children) { - getAllCliques(childClique,cliques); + for (SymbolicBayesTree::sharedClique& childClique : subtree->children) { + getAllCliques(childClique, cliques); } } } /* ************************************************************************* */ -TEST( BayesTree, shortcutCheck ) -{ - const Key _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0; - SymbolicFactorGraph chain = list_of - (SymbolicFactor(_A_)) - (SymbolicFactor(_B_, _A_)) - (SymbolicFactor(_C_, _A_)) - (SymbolicFactor(_D_, _C_)) - (SymbolicFactor(_E_, _B_)) - (SymbolicFactor(_F_, _E_)) - (SymbolicFactor(_G_, _F_)); - Ordering ordering(list_of(_G_)(_F_)(_E_)(_D_)(_C_)(_B_)(_A_)); +TEST(BayesTree, shortcutCheck) { + const Key _A_ = 6, _B_ = 5, _C_ = 4, _D_ = 3, _E_ = 2, _F_ = 1, _G_ = 0; + auto chain = + SymbolicFactorGraph(SymbolicFactor(_A_))(SymbolicFactor(_B_, _A_))( + SymbolicFactor(_C_, _A_))(SymbolicFactor(_D_, _C_))(SymbolicFactor( + _E_, _B_))(SymbolicFactor(_F_, _E_))(SymbolicFactor(_G_, _F_)); + Ordering ordering{_G_, _F_, _E_, _D_, _C_, _B_, _A_}; SymbolicBayesTree bayesTree = *chain.eliminateMultifrontal(ordering); - //bayesTree.saveGraph("BT1.dot"); + // bayesTree.saveGraph("BT1.dot"); SymbolicBayesTree::sharedClique rootClique = bayesTree.roots().front(); - //rootClique->printTree(); + // rootClique->printTree(); SymbolicBayesTree::Cliques allCliques; - getAllCliques(rootClique,allCliques); + getAllCliques(rootClique, allCliques); - for(SymbolicBayesTree::sharedClique& clique: allCliques) { - //clique->print("Clique#"); + for (SymbolicBayesTree::sharedClique& clique : allCliques) { + // clique->print("Clique#"); SymbolicBayesNet bn = clique->shortcut(rootClique); - //bn.print("Shortcut:\n"); - //cout << endl; + // bn.print("Shortcut:\n"); + // cout << endl; } // Check if all the cached shortcuts are cleared rootClique->deleteCachedShortcuts(); - for(SymbolicBayesTree::sharedClique& clique: allCliques) { + for (SymbolicBayesTree::sharedClique& clique : allCliques) { bool notCleared = clique->cachedSeparatorMarginal().is_initialized(); - CHECK( notCleared == false); + CHECK(notCleared == false); } EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals()); -// for(SymbolicBayesTree::sharedClique& clique: allCliques) { -// clique->print("Clique#"); -// if(clique->cachedShortcut()){ -// bn = clique->cachedShortcut().get(); -// bn.print("Shortcut:\n"); -// } -// else -// cout << "Not Initialized" << endl; -// cout << endl; -// } + // for(SymbolicBayesTree::sharedClique& clique: allCliques) { + // clique->print("Clique#"); + // if(clique->cachedShortcut()){ + // bn = clique->cachedShortcut().get(); + // bn.print("Shortcut:\n"); + // } + // else + // cout << "Not Initialized" << endl; + // cout << endl; + // } } /* ************************************************************************* */ -TEST( BayesTree, removeTop ) -{ +TEST(BayesTree, removeTop) { SymbolicBayesTree bayesTree = asiaBayesTree; // create a new factor to be inserted - //boost::shared_ptr newFactor(new IndexFactor(_S_,_B_)); + // boost::shared_ptr newFactor(new IndexFactor(_S_,_B_)); // Remove the contaminated part of the Bayes tree SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(_B_)(_S_), &bn, &orphans); + bayesTree.removeTop(MakeKeys(_B_)(_S_), &bn, &orphans); // Check expected outcome SymbolicBayesNet expected; - expected += SymbolicConditional::FromKeys(list_of(_E_)(_L_)(_B_), 3); - expected += SymbolicConditional::FromKeys(list_of(_S_)(_B_)(_L_), 1); + expected += + SymbolicConditional::FromKeys(MakeKeys(_E_)(_L_)(_B_), 3); + expected += + SymbolicConditional::FromKeys(MakeKeys(_S_)(_B_)(_L_), 1); CHECK(assert_equal(expected, bn)); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_T_], bayesTree[_X_]; - CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); + SymbolicBayesTree::Cliques expectedOrphans = + std::list{bayesTree[_T_], bayesTree[_X_]}; + CHECK(assert_container_equal(expectedOrphans | indirected, + orphans | indirected)); // Try removeTop again with a factor that should not change a thing - //boost::shared_ptr newFactor2(new IndexFactor(_B_)); + // boost::shared_ptr newFactor2(new IndexFactor(_B_)); SymbolicBayesNet bn2; SymbolicBayesTree::Cliques orphans2; - bayesTree.removeTop(list_of(_B_), &bn2, &orphans2); + bayesTree.removeTop(MakeKeys(_B_), &bn2, &orphans2); SymbolicFactorGraph factors2(bn2); SymbolicFactorGraph expected2; CHECK(assert_equal(expected2, factors2)); SymbolicBayesTree::Cliques expectedOrphans2; - CHECK(assert_container_equal(expectedOrphans2|indirected, orphans2|indirected)); + CHECK(assert_container_equal(expectedOrphans2 | indirected, + orphans2 | indirected)); } /* ************************************************************************* */ -TEST( BayesTree, removeTop2 ) -{ +TEST(BayesTree, removeTop2) { SymbolicBayesTree bayesTree = asiaBayesTree; // create two factors to be inserted - //SymbolicFactorGraph newFactors; - //newFactors.push_factor(_B_); - //newFactors.push_factor(_S_); + // SymbolicFactorGraph newFactors; + // newFactors.push_factor(_B_); + // newFactors.push_factor(_S_); // Remove the contaminated part of the Bayes tree SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(_T_), &bn, &orphans); + bayesTree.removeTop(MakeKeys(_T_), &bn, &orphans); // Check expected outcome - SymbolicBayesNet expected = list_of - (SymbolicConditional::FromKeys(list_of(_E_)(_L_)(_B_), 3)) - (SymbolicConditional::FromKeys(list_of(_T_)(_E_)(_L_), 1)); + auto expected = SymbolicBayesNet( + SymbolicConditional::FromKeys(MakeKeys(_E_)(_L_)(_B_), 3))( + SymbolicConditional::FromKeys(MakeKeys(_T_)(_E_)(_L_), 1)); CHECK(assert_equal(expected, bn)); - SymbolicBayesTree::Cliques expectedOrphans; - expectedOrphans += bayesTree[_S_], bayesTree[_X_]; - CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); + SymbolicBayesTree::Cliques expectedOrphans = + std::list{bayesTree[_S_], bayesTree[_X_]}; + CHECK(assert_container_equal(expectedOrphans | indirected, + orphans | indirected)); } /* ************************************************************************* */ -TEST( BayesTree, removeTop3 ) -{ - SymbolicFactorGraph graph = list_of - (SymbolicFactor(L(5))) - (SymbolicFactor(X(4), L(5))) - (SymbolicFactor(X(2), X(4))) - (SymbolicFactor(X(3), X(2))); - Ordering ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) ); +TEST(BayesTree, removeTop3) { + auto graph = SymbolicFactorGraph(SymbolicFactor(L(5)))(SymbolicFactor( + X(4), L(5)))(SymbolicFactor(X(2), X(4)))(SymbolicFactor(X(3), X(2))); + Ordering ordering{X(3), X(2), X(4), L(5)}; SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); // remove all SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(L(5))(X(4))(X(2))(X(3)), &bn, &orphans); + bayesTree.removeTop(MakeKeys(L(5))(X(4))(X(2))(X(3)), &bn, &orphans); - SymbolicBayesNet expectedBn = list_of - (SymbolicConditional::FromKeys(list_of(X(4))(L(5)), 2)) - (SymbolicConditional(X(2), X(4))) - (SymbolicConditional(X(3), X(2))); + auto expectedBn = SymbolicBayesNet( + SymbolicConditional::FromKeys(MakeKeys(X(4))(L(5)), 2))( + SymbolicConditional(X(2), X(4)))(SymbolicConditional(X(3), X(2))); EXPECT(assert_equal(expectedBn, bn)); EXPECT(orphans.empty()); } /* ************************************************************************* */ -TEST( BayesTree, removeTop4 ) -{ - SymbolicFactorGraph graph = list_of - (SymbolicFactor(L(5))) - (SymbolicFactor(X(4), L(5))) - (SymbolicFactor(X(2), X(4))) - (SymbolicFactor(X(3), X(2))); - Ordering ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) ); +TEST(BayesTree, removeTop4) { + auto graph = SymbolicFactorGraph(SymbolicFactor(L(5)))(SymbolicFactor( + X(4), L(5)))(SymbolicFactor(X(2), X(4)))(SymbolicFactor(X(3), X(2))); + Ordering ordering{X(3), X(2), X(4), L(5)}; SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); // remove all SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(X(2))(L(5))(X(4))(X(3)), &bn, &orphans); + bayesTree.removeTop(MakeKeys(X(2))(L(5))(X(4))(X(3)), &bn, &orphans); - SymbolicBayesNet expectedBn = list_of - (SymbolicConditional::FromKeys(list_of(X(4))(L(5)), 2)) - (SymbolicConditional(X(2), X(4))) - (SymbolicConditional(X(3), X(2))); + auto expectedBn = SymbolicBayesNet( + SymbolicConditional::FromKeys(MakeKeys(X(4))(L(5)), 2))( + SymbolicConditional(X(2), X(4)))(SymbolicConditional(X(3), X(2))); EXPECT(assert_equal(expectedBn, bn)); EXPECT(orphans.empty()); } /* ************************************************************************* */ -TEST( BayesTree, removeTop5 ) -{ +TEST(BayesTree, removeTop5) { // Remove top called with variables that are not in the Bayes tree - SymbolicFactorGraph graph = list_of - (SymbolicFactor(L(5))) - (SymbolicFactor(X(4), L(5))) - (SymbolicFactor(X(2), X(4))) - (SymbolicFactor(X(3), X(2))); - Ordering ordering(list_of (X(3)) (X(2)) (X(4)) (L(5)) ); + auto graph = SymbolicFactorGraph(SymbolicFactor(L(5)))(SymbolicFactor( + X(4), L(5)))(SymbolicFactor(X(2), X(4)))(SymbolicFactor(X(3), X(2))); + Ordering ordering{X(3), X(2), X(4), L(5)}; SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); // Remove nonexistant SymbolicBayesNet bn; SymbolicBayesTree::Cliques orphans; - bayesTree.removeTop(list_of(X(10)), &bn, &orphans); + bayesTree.removeTop(MakeKeys(X(10)), &bn, &orphans); SymbolicBayesNet expectedBn; EXPECT(assert_equal(expectedBn, bn)); @@ -400,29 +396,28 @@ TEST( BayesTree, removeTop5 ) } /* ************************************************************************* */ -TEST( SymbolicBayesTree, thinTree ) { - +TEST(SymbolicBayesTree, thinTree) { // create a thin-tree Bayesnet, a la Jean-Guillaume SymbolicBayesNet bayesNet; - bayesNet.push_back(boost::make_shared(14)); + bayesNet.emplace_shared(14); - bayesNet.push_back(boost::make_shared(13, 14)); - bayesNet.push_back(boost::make_shared(12, 14)); + bayesNet.emplace_shared(13, 14); + bayesNet.emplace_shared(12, 14); - bayesNet.push_back(boost::make_shared(11, 13, 14)); - bayesNet.push_back(boost::make_shared(10, 13, 14)); - bayesNet.push_back(boost::make_shared(9, 12, 14)); - bayesNet.push_back(boost::make_shared(8, 12, 14)); + bayesNet.emplace_shared(11, 13, 14); + bayesNet.emplace_shared(10, 13, 14); + bayesNet.emplace_shared(9, 12, 14); + bayesNet.emplace_shared(8, 12, 14); - bayesNet.push_back(boost::make_shared(7, 11, 13)); - bayesNet.push_back(boost::make_shared(6, 11, 13)); - bayesNet.push_back(boost::make_shared(5, 10, 13)); - bayesNet.push_back(boost::make_shared(4, 10, 13)); + bayesNet.emplace_shared(7, 11, 13); + bayesNet.emplace_shared(6, 11, 13); + bayesNet.emplace_shared(5, 10, 13); + bayesNet.emplace_shared(4, 10, 13); - bayesNet.push_back(boost::make_shared(3, 9, 12)); - bayesNet.push_back(boost::make_shared(2, 9, 12)); - bayesNet.push_back(boost::make_shared(1, 8, 12)); - bayesNet.push_back(boost::make_shared(0, 8, 12)); + bayesNet.emplace_shared(3, 9, 12); + bayesNet.emplace_shared(2, 9, 12); + bayesNet.emplace_shared(1, 8, 12); + bayesNet.emplace_shared(0, 8, 12); if (debug) { GTSAM_PRINT(bayesNet); @@ -430,7 +425,8 @@ TEST( SymbolicBayesTree, thinTree ) { } // create a BayesTree out of a Bayes net - SymbolicBayesTree bayesTree = *SymbolicFactorGraph(bayesNet).eliminateMultifrontal(); + SymbolicBayesTree bayesTree = + *SymbolicFactorGraph(bayesNet).eliminateMultifrontal(); if (debug) { GTSAM_PRINT(bayesTree); bayesTree.saveGraph("/tmp/SymbolicBayesTree.dot"); @@ -442,7 +438,7 @@ TEST( SymbolicBayesTree, thinTree ) { // check shortcut P(S9||R) to root SymbolicBayesTree::Clique::shared_ptr c = bayesTree[9]; SymbolicBayesNet shortcut = c->shortcut(R); - SymbolicBayesNet expected = list_of(SymbolicConditional(14, 11, 13)); + auto expected = SymbolicBayesNet(SymbolicConditional(14, 11, 13)); EXPECT(assert_equal(expected, shortcut)); } @@ -450,9 +446,8 @@ TEST( SymbolicBayesTree, thinTree ) { // check shortcut P(S8||R) to root SymbolicBayesTree::Clique::shared_ptr c = bayesTree[8]; SymbolicBayesNet shortcut = c->shortcut(R); - SymbolicBayesNet expected = list_of - (SymbolicConditional(12, 14)) - (SymbolicConditional(14, 11, 13)); + auto expected = SymbolicBayesNet(SymbolicConditional(12, 14))( + SymbolicConditional(14, 11, 13)); EXPECT(assert_equal(expected, shortcut)); } @@ -460,7 +455,7 @@ TEST( SymbolicBayesTree, thinTree ) { // check shortcut P(S4||R) to root SymbolicBayesTree::Clique::shared_ptr c = bayesTree[4]; SymbolicBayesNet shortcut = c->shortcut(R); - SymbolicBayesNet expected = list_of(SymbolicConditional(10, 11, 13)); + auto expected = SymbolicBayesNet(SymbolicConditional(10, 11, 13)); EXPECT(assert_equal(expected, shortcut)); } @@ -468,8 +463,8 @@ TEST( SymbolicBayesTree, thinTree ) { // check shortcut P(S2||R) to root SymbolicBayesTree::Clique::shared_ptr c = bayesTree[2]; SymbolicBayesNet shortcut = c->shortcut(R); - SymbolicBayesNet expected = list_of(SymbolicConditional(9, 11, 12, 13)) - (SymbolicConditional(12, 11, 13)); + auto expected = SymbolicBayesNet(SymbolicConditional(9, 11, 12, 13))( + SymbolicConditional(12, 11, 13)); EXPECT(assert_equal(expected, shortcut)); } @@ -477,28 +472,28 @@ TEST( SymbolicBayesTree, thinTree ) { // check shortcut P(S0||R) to root SymbolicBayesTree::Clique::shared_ptr c = bayesTree[0]; SymbolicBayesNet shortcut = c->shortcut(R); - SymbolicBayesNet expected = list_of(SymbolicConditional(8, 11, 12, 13)) - (SymbolicConditional(12, 11, 13)); + auto expected = SymbolicBayesNet(SymbolicConditional(8, 11, 12, 13))( + SymbolicConditional(12, 11, 13)); EXPECT(assert_equal(expected, shortcut)); } SymbolicBayesNet::shared_ptr actualJoint; // Check joint P(8,2) - if (false) { // TODO, not disjoint + if (false) { // TODO, not disjoint actualJoint = bayesTree.jointBayesNet(8, 2); SymbolicBayesNet expected; - expected.push_back(boost::make_shared(8)); - expected.push_back(boost::make_shared(2, 8)); + expected.emplace_shared(8); + expected.emplace_shared(2, 8); EXPECT(assert_equal(expected, *actualJoint)); } // Check joint P(1,2) - if (false) { // TODO, not disjoint + if (false) { // TODO, not disjoint actualJoint = bayesTree.jointBayesNet(1, 2); SymbolicBayesNet expected; - expected.push_back(boost::make_shared(2)); - expected.push_back(boost::make_shared(1, 2)); + expected.emplace_shared(2); + expected.emplace_shared(1, 2); EXPECT(assert_equal(expected, *actualJoint)); } @@ -506,35 +501,33 @@ TEST( SymbolicBayesTree, thinTree ) { if (true) { actualJoint = bayesTree.jointBayesNet(2, 6); SymbolicBayesNet expected; - expected.push_back(boost::make_shared(2, 6)); - expected.push_back(boost::make_shared(6)); + expected.emplace_shared(2, 6); + expected.emplace_shared(6); EXPECT(assert_equal(expected, *actualJoint)); } // Check joint P(4,6) - if (false) { // TODO, not disjoint + if (false) { // TODO, not disjoint actualJoint = bayesTree.jointBayesNet(4, 6); SymbolicBayesNet expected; - expected.push_back(boost::make_shared(6)); - expected.push_back(boost::make_shared(4, 6)); + expected.emplace_shared(6); + expected.emplace_shared(4, 6); EXPECT(assert_equal(expected, *actualJoint)); } } /* ************************************************************************* */ -TEST(SymbolicBayesTree, forest_joint) -{ +TEST(SymbolicBayesTree, forest_joint) { // Create forest - SymbolicBayesTreeClique::shared_ptr root1 = MakeClique(list_of(1), 1); - SymbolicBayesTreeClique::shared_ptr root2 = MakeClique(list_of(2), 1); + sharedClique root1 = MakeClique(MakeKeys(1), 1); + sharedClique root2 = MakeClique(MakeKeys(2), 1); SymbolicBayesTree bayesTree; bayesTree.insertRoot(root1); bayesTree.insertRoot(root2); // Check joint - SymbolicBayesNet expected = list_of - (SymbolicConditional(1)) - (SymbolicConditional(2)); + auto expected = + SymbolicBayesNet(SymbolicConditional(1))(SymbolicConditional(2)); SymbolicBayesNet actual = *bayesTree.jointBayesNet(1, 2); EXPECT(assert_equal(expected, actual)); @@ -550,7 +543,7 @@ TEST(SymbolicBayesTree, forest_joint) C6 0 : 1 **************************************************************************** */ -TEST( SymbolicBayesTree, linear_smoother_shortcuts ) { +TEST(SymbolicBayesTree, linear_smoother_shortcuts) { // Create smoother with 7 nodes SymbolicFactorGraph smoother; smoother.push_factor(0); @@ -581,7 +574,8 @@ TEST( SymbolicBayesTree, linear_smoother_shortcuts ) { { // check shortcut P(S2||R) to root - SymbolicBayesTree::Clique::shared_ptr c = bayesTree[4]; // 4 is frontal in C2 + SymbolicBayesTree::Clique::shared_ptr c = + bayesTree[4]; // 4 is frontal in C2 SymbolicBayesNet shortcut = c->shortcut(R); SymbolicBayesNet expected; EXPECT(assert_equal(expected, shortcut)); @@ -589,45 +583,46 @@ TEST( SymbolicBayesTree, linear_smoother_shortcuts ) { { // check shortcut P(S3||R) to root - SymbolicBayesTree::Clique::shared_ptr c = bayesTree[3]; // 3 is frontal in C3 + SymbolicBayesTree::Clique::shared_ptr c = + bayesTree[3]; // 3 is frontal in C3 SymbolicBayesNet shortcut = c->shortcut(R); - SymbolicBayesNet expected = list_of(SymbolicConditional(4, 5)); + auto expected = SymbolicBayesNet(SymbolicConditional(4, 5)); EXPECT(assert_equal(expected, shortcut)); } { // check shortcut P(S4||R) to root - SymbolicBayesTree::Clique::shared_ptr c = bayesTree[2]; // 2 is frontal in C4 + SymbolicBayesTree::Clique::shared_ptr c = + bayesTree[2]; // 2 is frontal in C4 SymbolicBayesNet shortcut = c->shortcut(R); - SymbolicBayesNet expected = list_of(SymbolicConditional(3, 5)); + auto expected = SymbolicBayesNet(SymbolicConditional(3, 5)); EXPECT(assert_equal(expected, shortcut)); } } /* ************************************************************************* */ // from testSymbolicJunctionTree, which failed at one point -TEST(SymbolicBayesTree, complicatedMarginal) -{ +TEST(SymbolicBayesTree, complicatedMarginal) { // Create the conditionals to go in the BayesTree - SymbolicBayesTreeClique::shared_ptr cur; - SymbolicBayesTreeClique::shared_ptr root = MakeClique(list_of(11)(12), 2); + sharedClique cur; + sharedClique root = MakeClique(MakeKeys(11)(12), 2); cur = root; - root->children += MakeClique(list_of(9)(10)(11)(12), 2); + root->children.push_back(MakeClique(MakeKeys(9)(10)(11)(12), 2)); root->children.back()->parent_ = root; - root->children += MakeClique(list_of(7)(8)(11), 2); + root->children.push_back(MakeClique(MakeKeys(7)(8)(11), 2)); root->children.back()->parent_ = root; cur = root->children.back(); - cur->children += MakeClique(list_of(5)(6)(7)(8), 2); + cur->children.push_back(MakeClique(MakeKeys(5)(6)(7)(8), 2)); cur->children.back()->parent_ = cur; cur = cur->children.back(); - cur->children += MakeClique(list_of(3)(4)(6), 2); + cur->children.push_back(MakeClique(MakeKeys(3)(4)(6), 2)); cur->children.back()->parent_ = cur; - cur->children += MakeClique(list_of(1)(2)(5), 2); + cur->children.push_back(MakeClique(MakeKeys(1)(2)(5), 2)); cur->children.back()->parent_ = cur; // Create Bayes Tree @@ -656,9 +651,8 @@ TEST(SymbolicBayesTree, complicatedMarginal) { SymbolicBayesTree::Clique::shared_ptr c = bt[5]; SymbolicBayesNet shortcut = c->shortcut(root); - SymbolicBayesNet expected = list_of - (SymbolicConditional(7, 8, 11)) - (SymbolicConditional(8, 11)); + auto expected = SymbolicBayesNet(SymbolicConditional(7, 8, 11))( + SymbolicConditional(8, 11)); EXPECT(assert_equal(expected, shortcut)); } @@ -666,7 +660,7 @@ TEST(SymbolicBayesTree, complicatedMarginal) { SymbolicBayesTree::Clique::shared_ptr c = bt[3]; SymbolicBayesNet shortcut = c->shortcut(root); - SymbolicBayesNet expected = list_of(SymbolicConditional(6, 11)); + auto expected = SymbolicBayesNet(SymbolicConditional(6, 11)); EXPECT(assert_equal(expected, shortcut)); } @@ -674,7 +668,7 @@ TEST(SymbolicBayesTree, complicatedMarginal) { SymbolicBayesTree::Clique::shared_ptr c = bt[1]; SymbolicBayesNet shortcut = c->shortcut(root); - SymbolicBayesNet expected = list_of(SymbolicConditional(5, 11)); + auto expected = SymbolicBayesNet(SymbolicConditional(5, 11)); EXPECT(assert_equal(expected, shortcut)); } @@ -689,12 +683,10 @@ TEST(SymbolicBayesTree, complicatedMarginal) SymbolicFactor::shared_ptr actual = bt.marginalFactor(6); EXPECT(assert_equal(SymbolicFactor(6), *actual, 1e-1)); } - } /* ************************************************************************* */ TEST(SymbolicBayesTree, COLAMDvsMETIS) { - // create circular graph SymbolicFactorGraph sfg; sfg.push_factor(0, 1); @@ -707,7 +699,7 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // COLAMD { Ordering ordering = Ordering::Create(Ordering::COLAMD, sfg); - EXPECT(assert_equal(Ordering(list_of(0)(5)(1)(4)(2)(3)), ordering)); + EXPECT(assert_equal(Ordering{0, 5, 1, 4, 2, 3}, ordering)); // - P( 4 2 3) // | - P( 1 | 2 4) @@ -715,12 +707,12 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // | | | - P( 0 | 1 5) SymbolicBayesTree expected; expected.insertRoot( - MakeClique(list_of(4)(2)(3), 3, - list_of( - MakeClique(list_of(1)(2)(4), 1, - list_of( - MakeClique(list_of(5)(1)(4), 1, - list_of(MakeClique(list_of(0)(1)(5), 1)))))))); + MakeClique(MakeKeys(4)(2)(3), 3, + MakeCliques(MakeClique( + MakeKeys(1)(2)(4), 1, + MakeCliques(MakeClique( + MakeKeys(5)(1)(4), 1, + MakeCliques(MakeClique(MakeKeys(0)(1)(5), 1)))))))); SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); @@ -732,11 +724,11 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { Ordering ordering = Ordering::Create(Ordering::METIS, sfg); // Linux and Mac split differently when using mettis #if defined(__APPLE__) - EXPECT(assert_equal(Ordering(list_of(5)(4)(2)(1)(0)(3)), ordering)); + EXPECT(assert_equal(Ordering{5, 4, 2, 1, 0, 3}, ordering)); #elif defined(_WIN32) - EXPECT(assert_equal(Ordering(list_of(4)(3)(1)(0)(5)(2)), ordering)); + EXPECT(assert_equal(Ordering{4, 3, 1, 0, 5, 2}, ordering)); #else - EXPECT(assert_equal(Ordering(list_of(3)(2)(5)(0)(4)(1)), ordering)); + EXPECT(assert_equal(Ordering{3, 2, 0, 5, 4, 1}, ordering)); #endif // - P( 1 0 3) @@ -745,26 +737,23 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // | - P( 2 | 1 3) SymbolicBayesTree expected; #if defined(__APPLE__) - expected.insertRoot( - MakeClique(list_of(1)(0)(3), 3, - list_of( - MakeClique(list_of(4)(0)(3), 1, - list_of(MakeClique(list_of(5)(0)(4), 1))))( - MakeClique(list_of(2)(1)(3), 1)))); + expected.insertRoot(MakeClique( + MakeKeys(1)(0)(3), 3, + MakeCliques(MakeClique(MakeKeys(4)(0)(3), 1, + MakeCliques(MakeClique(MakeKeys(5)(0)(4), 1))))( + MakeClique(MakeKeys(2)(1)(3), 1)))); #elif defined(_WIN32) - expected.insertRoot( - MakeClique(list_of(3)(5)(2), 3, - list_of( - MakeClique(list_of(4)(3)(5), 1, - list_of(MakeClique(list_of(0)(2)(5), 1))))( - MakeClique(list_of(1)(0)(2), 1)))); + expected.insertRoot(MakeClique( + MakeKeys(3)(5)(2), 3, + MakeCliques(MakeClique(MakeKeys(4)(3)(5), 1, + MakeCliques(MakeClique(MakeKeys(0)(2)(5), 1))))( + MakeClique(MakeKeys(1)(0)(2), 1)))); #else - expected.insertRoot( - MakeClique(list_of(2)(4)(1), 3, - list_of( - MakeClique(list_of(0)(1)(4), 1, - list_of(MakeClique(list_of(5)(0)(4), 1))))( - MakeClique(list_of(3)(2)(4), 1)))); + expected.insertRoot(MakeClique( + MakeKeys(2)(4)(1), 3, + MakeCliques(MakeClique(MakeKeys(0)(1)(4), 1, + MakeCliques(MakeClique(MakeKeys(5)(0)(4), 1))))( + MakeClique(MakeKeys(3)(2)(4), 1)))); #endif SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); @@ -778,4 +767,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index 78bb2182c..9234d8a51 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -17,47 +17,45 @@ */ #include - -#include -#include -#include - #include -#include #include +#include + +#include +#include #include "symbolicExampleGraphs.h" using namespace gtsam; using namespace gtsam::symbol_shorthand; using namespace std; -using boost::assign::list_of; +using sharedNode = SymbolicEliminationTree::sharedNode; class EliminationTreeTester { -public: + public: // build hardcoded tree - static SymbolicEliminationTree buildHardcodedTree(const SymbolicFactorGraph& fg) { - - SymbolicEliminationTree::sharedNode leaf0(new SymbolicEliminationTree::Node); + static SymbolicEliminationTree buildHardcodedTree( + const SymbolicFactorGraph& fg) { + sharedNode leaf0(new SymbolicEliminationTree::Node); leaf0->key = 0; leaf0->factors.push_back(fg[0]); leaf0->factors.push_back(fg[1]); - SymbolicEliminationTree::sharedNode node1(new SymbolicEliminationTree::Node); + sharedNode node1(new SymbolicEliminationTree::Node); node1->key = 1; node1->factors.push_back(fg[2]); node1->children.push_back(leaf0); - SymbolicEliminationTree::sharedNode node2(new SymbolicEliminationTree::Node); + sharedNode node2(new SymbolicEliminationTree::Node); node2->key = 2; node2->factors.push_back(fg[3]); node2->children.push_back(node1); - SymbolicEliminationTree::sharedNode leaf3(new SymbolicEliminationTree::Node); + sharedNode leaf3(new SymbolicEliminationTree::Node); leaf3->key = 3; leaf3->factors.push_back(fg[4]); - SymbolicEliminationTree::sharedNode root(new SymbolicEliminationTree::Node); + sharedNode root(new SymbolicEliminationTree::Node); root->key = 4; root->children.push_back(leaf3); root->children.push_back(node2); @@ -67,29 +65,27 @@ public: return tree; } - template - static SymbolicEliminationTree MakeTree(const ROOTS& roots) - { + static SymbolicEliminationTree MakeTree( + const std::vector& roots) { SymbolicEliminationTree et; et.roots_.assign(roots.begin(), roots.end()); return et; } }; -template -static SymbolicEliminationTree::sharedNode MakeNode(Key key, const FACTORS& factors) -{ - SymbolicEliminationTree::sharedNode node = boost::make_shared(); +template +static sharedNode MakeNode(Key key, const FACTORS& factors) { + sharedNode node = boost::make_shared(); node->key = key; SymbolicFactorGraph factorsAsGraph = factors; node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end()); return node; } -template -static SymbolicEliminationTree::sharedNode MakeNode(Key key, const FACTORS& factors, const CHILDREN& children) -{ - SymbolicEliminationTree::sharedNode node = boost::make_shared(); +template +static sharedNode MakeNode(Key key, const FACTORS& factors, + const std::vector& children) { + sharedNode node = boost::make_shared(); node->key = key; SymbolicFactorGraph factorsAsGraph = factors; node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end()); @@ -97,24 +93,39 @@ static SymbolicEliminationTree::sharedNode MakeNode(Key key, const FACTORS& fact return node; } +template +class ListOf { + public: + ListOf(Class&& c) { result.push_back(c); } + + ListOf& operator()(Class&& c) { + result.push_back(c); + return *this; + } + + operator std::vector() { return result; } + + private: + std::vector result; +}; + +using Nodes = ListOf; /* ************************************************************************* */ -TEST(EliminationTree, Create) -{ +TEST(EliminationTree, Create) { SymbolicEliminationTree expected = - EliminationTreeTester::buildHardcodedTree(simpleTestGraph1); + EliminationTreeTester::buildHardcodedTree(simpleTestGraph1); // Build from factor graph Ordering order; - order += 0,1,2,3,4; + order += 0, 1, 2, 3, 4; SymbolicEliminationTree actual(simpleTestGraph1, order); CHECK(assert_equal(expected, actual)); } /* ************************************************************************* */ -TEST(EliminationTree, Create2) -{ +TEST(EliminationTree, Create2) { // l1 l2 // / | / | // x1 --- x2 --- x3 --- x4 --- x5 @@ -132,20 +143,31 @@ TEST(EliminationTree, Create2) graph += SymbolicFactor(X(4), L(3)); graph += SymbolicFactor(X(5), L(3)); - SymbolicEliminationTree expected = EliminationTreeTester::MakeTree(list_of - (MakeNode(X(3), SymbolicFactorGraph(), list_of - (MakeNode(X(2), list_of(SymbolicFactor(X(2), X(3))), list_of - (MakeNode(L(1), list_of(SymbolicFactor(X(2), L(1))), list_of - (MakeNode(X(1), list_of(SymbolicFactor(X(1), L(1))) (SymbolicFactor(X(1), X(2))))))))) - (MakeNode(X(4), list_of(SymbolicFactor(X(3), X(4))), list_of - (MakeNode(L(2), list_of(SymbolicFactor(X(4), L(2))), list_of - (MakeNode(X(5), list_of(SymbolicFactor(X(4), X(5))) (SymbolicFactor(L(2), X(5))), list_of - (MakeNode(L(3), list_of(SymbolicFactor(X(4), L(3))) (SymbolicFactor(X(5), L(3)))))))))))))); - - Ordering order = list_of(X(1)) (L(3)) (L(1)) (X(5)) (X(2)) (L(2)) (X(4)) (X(3)); + SymbolicEliminationTree expected = + EliminationTreeTester::MakeTree(Nodes(MakeNode( + X(3), SymbolicFactorGraph(), + Nodes(MakeNode( + X(2), SymbolicFactorGraph(SymbolicFactor(X(2), X(3))), + Nodes(MakeNode( + L(1), SymbolicFactorGraph(SymbolicFactor(X(2), L(1))), + Nodes(MakeNode( + X(1), SymbolicFactorGraph(SymbolicFactor(X(1), L(1)))( + SymbolicFactor(X(1), X(2)))))))))( + MakeNode( + X(4), SymbolicFactorGraph(SymbolicFactor(X(3), X(4))), + Nodes(MakeNode( + L(2), SymbolicFactorGraph(SymbolicFactor(X(4), L(2))), + Nodes(MakeNode( + X(5), + SymbolicFactorGraph(SymbolicFactor(X(4), X(5)))( + SymbolicFactor(L(2), X(5))), + Nodes(MakeNode( + L(3), + SymbolicFactorGraph(SymbolicFactor(X(4), L(3)))( + SymbolicFactor(X(5), L(3)))))))))))))); + const Ordering order{X(1), L(3), L(1), X(5), X(2), L(2), X(4), X(3)}; SymbolicEliminationTree actual(graph, order); - EXPECT(assert_equal(expected, actual)); } diff --git a/gtsam/symbolic/tests/testSymbolicFactor.cpp b/gtsam/symbolic/tests/testSymbolicFactor.cpp index 105270c9c..a8001232a 100644 --- a/gtsam/symbolic/tests/testSymbolicFactor.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactor.cpp @@ -26,7 +26,6 @@ using namespace std; using namespace gtsam; -using namespace boost::assign; /* ************************************************************************* */ #ifdef TRACK_ELIMINATE diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index 966264290..8d5885d87 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -15,33 +15,29 @@ * @author Christian Potthast **/ -#include - +#include #include #include #include #include +#include #include #include -#include -using namespace boost::assign; - using namespace std; using namespace gtsam; -using namespace boost::assign; /* ************************************************************************* */ TEST(SymbolicFactorGraph, keys1) { - KeySet expected {0, 1, 2, 3, 4}; + KeySet expected{0, 1, 2, 3, 4}; KeySet actual = simpleTestGraph1.keys(); EXPECT(expected == actual); } /* ************************************************************************* */ TEST(SymbolicFactorGraph, keys2) { - KeySet expected {0, 1, 2, 3, 4, 5}; + KeySet expected{0, 1, 2, 3, 4, 5}; KeySet actual = simpleTestGraph2.keys(); EXPECT(expected == actual); } @@ -61,12 +57,12 @@ TEST(SymbolicFactorGraph, eliminateFullSequential) { /* ************************************************************************* */ TEST(SymbolicFactorGraph, eliminatePartialSequential) { // Eliminate 0 and 1 - const Ordering order {0, 1}; + const Ordering order{0, 1}; - const SymbolicBayesNet expectedBayesNet = - list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3, 4)); + const auto expectedBayesNet = SymbolicBayesNet(SymbolicConditional(0, 1, 2))( + SymbolicConditional(1, 2, 3, 4)); - const SymbolicFactorGraph expectedSfg = list_of(SymbolicFactor(2, 3))( + const auto expectedSfg = SymbolicFactorGraph(SymbolicFactor(2, 3))( SymbolicFactor(4, 5))(SymbolicFactor(2, 3, 4)); SymbolicBayesNet::shared_ptr actualBayesNet; @@ -106,9 +102,9 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) { expectedBayesTree.insertRoot( boost::make_shared(root)); - SymbolicFactorGraph expectedFactorGraph = - list_of(SymbolicFactor(0, 1))(SymbolicFactor(0, 2))(SymbolicFactor(1, 3))( - SymbolicFactor(2, 3))(SymbolicFactor(1)); + const auto expectedFactorGraph = + SymbolicFactorGraph(SymbolicFactor(0, 1))(SymbolicFactor(0, 2))( + SymbolicFactor(1, 3))(SymbolicFactor(2, 3))(SymbolicFactor(1)); SymbolicBayesTree::shared_ptr actualBayesTree; SymbolicFactorGraph::shared_ptr actualFactorGraph; @@ -137,12 +133,12 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) { /* ************************************************************************* */ TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) { - SymbolicBayesNet expectedBayesNet = - list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3))( - SymbolicConditional(2, 3))(SymbolicConditional(3)); + auto expectedBayesNet = + SymbolicBayesNet(SymbolicConditional(0, 1, 2))(SymbolicConditional( + 1, 2, 3))(SymbolicConditional(2, 3))(SymbolicConditional(3)); - SymbolicBayesNet actual1 = *simpleTestGraph2.marginalMultifrontalBayesNet( - Ordering{0, 1, 2, 3}); + SymbolicBayesNet actual1 = + *simpleTestGraph2.marginalMultifrontalBayesNet(Ordering{0, 1, 2, 3}); EXPECT(assert_equal(expectedBayesNet, actual1)); } @@ -192,7 +188,7 @@ TEST(SymbolicFactorGraph, marginals) { { // jointBayesNet - Ordering ord {0, 4, 3}; + Ordering ord{0, 4, 3}; auto actual = fg.eliminatePartialSequential(ord); SymbolicBayesNet expectedBN; expectedBN.emplace_shared(0, 1, 2); @@ -203,7 +199,7 @@ TEST(SymbolicFactorGraph, marginals) { { // jointBayesNet - Ordering ord {0, 2, 3}; + Ordering ord{0, 2, 3}; auto actual = fg.eliminatePartialSequential(ord); SymbolicBayesNet expectedBN; expectedBN.emplace_shared(0, 1, 2); @@ -302,7 +298,7 @@ TEST(SymbolicFactorGraph, add_factors) { expected.push_factor(1); expected.push_factor(11); expected.push_factor(2); - const FactorIndices expectedIndices {1, 3}; + const FactorIndices expectedIndices{1, 3}; const FactorIndices actualIndices = fg1.add_factors(fg2, true); EXPECT(assert_equal(expected, fg1)); @@ -310,7 +306,7 @@ TEST(SymbolicFactorGraph, add_factors) { expected.push_factor(1); expected.push_factor(2); - const FactorIndices expectedIndices2 {4, 5}; + const FactorIndices expectedIndices2{4, 5}; const FactorIndices actualIndices2 = fg1.add_factors(fg2, false); EXPECT(assert_equal(expected, fg1));