diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index ab4f2d259..5186a4c62 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -78,6 +78,41 @@ TEST(SymbolicBayesTree, clear) CHECK(assert_equal(expected, bayesTree)); } +/* ************************************************************************* */ +TEST(SymbolicJunctionTree, 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)); + + SymbolicBayesTree expected; + expected.insertRoot( + MakeClique(list_of(X(3)) (X(4)), 2, list_of + (MakeClique(list_of(L(2)) (X(5)) (X(4)), 2, list_of + (MakeClique(list_of(L(3)) (X(4)) (X(5)), 1)))) + (MakeClique(list_of(X(2)) (X(3)), 1, list_of + (MakeClique(list_of(L(1)) (X(1)) (X(2)), 2)))))); + + Ordering order = list_of(L(1)) (X(1)) (X(2)) (L(2)) (L(3)) (X(5)) (X(3)) (X(4)); + + SymbolicBayesTree actual = *graph.eliminateMultifrontal(order); + + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* * Bayes Tree for testing conversion to a forest of orphans needed for incremental. A,B diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index 38ec08411..f2404b631 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -30,6 +31,7 @@ using namespace gtsam; using namespace gtsam::symbol_shorthand; using namespace std; +using boost::assign::list_of; class EliminationTreeTester { public: @@ -64,8 +66,37 @@ public: tree.roots_.push_back(root); return tree; } + + template + static SymbolicEliminationTree MakeTree(const ROOTS& 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(); + 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(); + node->key = key; + SymbolicFactorGraph factorsAsGraph = factors; + node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end()); + node->children.assign(children.begin(), children.end()); + return node; +} + /* ************************************************************************* */ TEST(EliminationTree, Create) @@ -81,6 +112,43 @@ TEST(EliminationTree, Create) CHECK(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST(EliminationTree, Create2) +{ + // 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)); + + SymbolicEliminationTree expected = EliminationTreeTester::MakeTree(list_of + (MakeNode(X(4), SymbolicFactorGraph(), list_of + (MakeNode(X(3), list_of(SymbolicFactor(X(3), X(4))), list_of + (MakeNode(X(2), list_of(SymbolicFactor(X(2), X(3))), list_of + (MakeNode(X(1), list_of(SymbolicFactor(X(1), X(2))), list_of + (MakeNode(L(1), list_of(SymbolicFactor(X(1), L(1))) (SymbolicFactor(X(2), L(1))))))))))) + (MakeNode(X(5), list_of(SymbolicFactor(X(4), X(5))), list_of + (MakeNode(L(2), list_of(SymbolicFactor(X(4), L(2))) (SymbolicFactor(L(2), X(5))))) + (MakeNode(L(3), list_of(SymbolicFactor(X(4), L(3))) (SymbolicFactor(X(5), L(3)))))))))); + + Ordering order = list_of(L(1)) (X(1)) (X(2)) (L(2)) (L(3)) (X(5)) (X(3)) (X(4)); + + SymbolicEliminationTree actual(graph, order); + + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr;