From 276394d1d81402e58b8bc6f4260eae3071870e42 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 8 Jan 2023 11:13:09 -0800 Subject: [PATCH] Moved ListOf helper and MakeKeys to common header. --- gtsam/symbolic/tests/symbolicExampleGraphs.h | 63 +++++++--- .../symbolic/tests/testSymbolicBayesTree.cpp | 110 ++++++------------ .../tests/testSymbolicEliminationTree.cpp | 79 +++++-------- 3 files changed, 108 insertions(+), 144 deletions(-) diff --git a/gtsam/symbolic/tests/symbolicExampleGraphs.h b/gtsam/symbolic/tests/symbolicExampleGraphs.h index fa90f54ef..47d40bb10 100644 --- a/gtsam/symbolic/tests/symbolicExampleGraphs.h +++ b/gtsam/symbolic/tests/symbolicExampleGraphs.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010-2023, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * @file symbolicExampleGraphs.cpp + * @file symbolicExampleGraphs.h * @date sept 15, 2012 * @author Frank Dellaert * @author Michael Kaess @@ -30,6 +30,25 @@ namespace gtsam { namespace { + // A small helper class to replace Boost's `list_of` function. + 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; + const SymbolicFactorGraph simpleTestGraph1 { boost::make_shared(0,1), boost::make_shared(0,2), @@ -100,23 +119,33 @@ namespace gtsam { boost::make_shared(_L_, _B_), boost::make_shared(_B_)}; + using sharedClique = SymbolicBayesTreeClique::shared_ptr; + using Children = ListOf; + + inline sharedClique LeafClique(const KeyVector& keys, + DenseIndex nrFrontals) { + return boost::make_shared( + boost::make_shared( + SymbolicConditional::FromKeys(keys, nrFrontals))); + } + + inline sharedClique NodeClique(const KeyVector& keys, DenseIndex nrFrontals, + const std::vector& children) { + sharedClique clique = LeafClique(keys, nrFrontals); + clique->children.assign(children.begin(), children.end()); + for (auto&& child : children) child->parent_ = clique; + return clique; + } + SymbolicBayesTree __asiaBayesTree() { SymbolicBayesTree result; - result.insertRoot(boost::make_shared( - boost::make_shared( - SymbolicConditional::FromKeys(KeyVector{_E_, _L_, _B_}, 3)))); - result.addClique(boost::make_shared( - boost::make_shared( - SymbolicConditional::FromKeys(KeyVector{_S_, _B_, _L_}, 1))), - result.roots().front()); - result.addClique(boost::make_shared( - boost::make_shared( - SymbolicConditional::FromKeys(KeyVector{_T_, _E_, _L_}, 1))), - result.roots().front()); - result.addClique(boost::make_shared( - boost::make_shared( - SymbolicConditional::FromKeys(KeyVector{_X_, _E_}, 1))), - result.roots().front()); + result.insertRoot(LeafClique(KeyVector{_E_, _L_, _B_}, 3)); + result.addClique(LeafClique(KeyVector{_S_, _B_, _L_}, 1), + result.roots().front()); + result.addClique(LeafClique(KeyVector{_T_, _E_, _L_}, 1), + result.roots().front()); + result.addClique(LeafClique(KeyVector{_X_, _E_}, 1), + result.roots().front()); return result; } diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index c5ea5a792..0cbcb4264 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -34,48 +34,6 @@ 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 -sharedClique MakeClique(const KeyVector& keys, DenseIndex nrFrontals) { - return boost::make_shared( - boost::make_shared( - 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) { SymbolicBayesTree bayesTree = asiaBayesTree; @@ -107,14 +65,14 @@ TEST(SymbolicBayesTree, clique_structure) { graph.emplace_shared(X(5), L(3)); SymbolicBayesTree expected; - expected.insertRoot(MakeClique( + expected.insertRoot(NodeClique( MakeKeys(X(2))(X(3)), 2, - MakeCliques(MakeClique( + Children(NodeClique( MakeKeys(X(4))(X(3)), 1, - MakeCliques(MakeClique( + Children(NodeClique( 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)))); + Children(LeafClique(MakeKeys(L(3))(X(4))(X(5)), 1))))))( + LeafClique(MakeKeys(X(1))(L(1))(X(2)), 2)))); Ordering order{X(1), L(3), L(1), X(5), X(2), L(2), X(4), X(3)}; @@ -135,12 +93,12 @@ TEST(BayesTree, removePath) { _F_ = F(0); SymbolicBayesTree bayesTreeOrig; - bayesTreeOrig.insertRoot(MakeClique( + bayesTreeOrig.insertRoot(NodeClique( 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)))))); + Children(NodeClique(MakeKeys(_C_)(_A_), 1, + Children(LeafClique(MakeKeys(_D_)(_C_), 1))))( + NodeClique(MakeKeys(_E_)(_B_), 1, + Children(LeafClique(MakeKeys(_F_)(_E_), 1)))))); SymbolicBayesTree bayesTree = bayesTreeOrig; @@ -519,8 +477,8 @@ TEST(SymbolicBayesTree, thinTree) { /* ************************************************************************* */ TEST(SymbolicBayesTree, forest_joint) { // Create forest - sharedClique root1 = MakeClique(MakeKeys(1), 1); - sharedClique root2 = MakeClique(MakeKeys(2), 1); + sharedClique root1 = LeafClique(MakeKeys(1), 1); + sharedClique root2 = LeafClique(MakeKeys(2), 1); SymbolicBayesTree bayesTree; bayesTree.insertRoot(root1); bayesTree.insertRoot(root2); @@ -605,24 +563,24 @@ TEST(SymbolicBayesTree, linear_smoother_shortcuts) { TEST(SymbolicBayesTree, complicatedMarginal) { // Create the conditionals to go in the BayesTree sharedClique cur; - sharedClique root = MakeClique(MakeKeys(11)(12), 2); + sharedClique root = LeafClique(MakeKeys(11)(12), 2); cur = root; - root->children.push_back(MakeClique(MakeKeys(9)(10)(11)(12), 2)); + root->children.push_back(LeafClique(MakeKeys(9)(10)(11)(12), 2)); root->children.back()->parent_ = root; - root->children.push_back(MakeClique(MakeKeys(7)(8)(11), 2)); + root->children.push_back(LeafClique(MakeKeys(7)(8)(11), 2)); root->children.back()->parent_ = root; cur = root->children.back(); - cur->children.push_back(MakeClique(MakeKeys(5)(6)(7)(8), 2)); + cur->children.push_back(LeafClique(MakeKeys(5)(6)(7)(8), 2)); cur->children.back()->parent_ = cur; cur = cur->children.back(); - cur->children.push_back(MakeClique(MakeKeys(3)(4)(6), 2)); + cur->children.push_back(LeafClique(MakeKeys(3)(4)(6), 2)); cur->children.back()->parent_ = cur; - cur->children.push_back(MakeClique(MakeKeys(1)(2)(5), 2)); + cur->children.push_back(LeafClique(MakeKeys(1)(2)(5), 2)); cur->children.back()->parent_ = cur; // Create Bayes Tree @@ -707,12 +665,12 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // | | | - P( 0 | 1 5) SymbolicBayesTree expected; expected.insertRoot( - MakeClique(MakeKeys(4)(2)(3), 3, - MakeCliques(MakeClique( + NodeClique(MakeKeys(4)(2)(3), 3, + Children(NodeClique( MakeKeys(1)(2)(4), 1, - MakeCliques(MakeClique( + Children(NodeClique( MakeKeys(5)(1)(4), 1, - MakeCliques(MakeClique(MakeKeys(0)(1)(5), 1)))))))); + Children(LeafClique(MakeKeys(0)(1)(5), 1)))))))); SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); @@ -737,23 +695,23 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) { // | - P( 2 | 1 3) SymbolicBayesTree expected; #if defined(__APPLE__) - expected.insertRoot(MakeClique( + expected.insertRoot(NodeClique( 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)))); + Children(NodeClique(MakeKeys(4)(0)(3), 1, + Children(LeafClique(MakeKeys(5)(0)(4), 1))))( + LeafClique(MakeKeys(2)(1)(3), 1)))); #elif defined(_WIN32) - expected.insertRoot(MakeClique( + expected.insertRoot(NodeClique( 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)))); + Children(NodeClique(MakeKeys(4)(3)(5), 1, + Children(LeafClique(MakeKeys(0)(2)(5), 1))))( + LeafClique(MakeKeys(1)(0)(2), 1)))); #else - expected.insertRoot(MakeClique( + expected.insertRoot(NodeClique( 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)))); + Children(NodeClique(MakeKeys(0)(1)(4), 1, + Children(LeafClique(MakeKeys(5)(0)(4), 1))))( + LeafClique(MakeKeys(3)(2)(4), 1)))); #endif SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); EXPECT(assert_equal(expected, actual)); diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index 9234d8a51..e1c0d9cf8 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -73,43 +73,22 @@ class EliminationTreeTester { } }; -template -static sharedNode MakeNode(Key key, const FACTORS& factors) { - sharedNode node = boost::make_shared(); +static sharedNode Leaf(Key key, const SymbolicFactorGraph& factors) { + sharedNode node(new SymbolicEliminationTree::Node()); node->key = key; - SymbolicFactorGraph factorsAsGraph = factors; - node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end()); + node->factors = factors; return node; } -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()); +static sharedNode Node(Key key, const SymbolicFactorGraph& factors, + const std::vector& children) { + sharedNode node = Leaf(key, factors); node->children.assign(children.begin(), children.end()); 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; +// Use list_of replacement defined in symbolicExampleGraphs.h +using ChildNodes = ListOf; /* ************************************************************************* */ TEST(EliminationTree, Create) { @@ -143,28 +122,26 @@ TEST(EliminationTree, Create2) { graph += SymbolicFactor(X(4), L(3)); graph += SymbolicFactor(X(5), L(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)))))))))))))); + SymbolicEliminationTree expected = EliminationTreeTester::MakeTree(ChildNodes( + Node(X(3), SymbolicFactorGraph(), + ChildNodes(Node( + X(2), SymbolicFactorGraph(SymbolicFactor(X(2), X(3))), + ChildNodes(Node( + L(1), SymbolicFactorGraph(SymbolicFactor(X(2), L(1))), + ChildNodes(Leaf( + X(1), SymbolicFactorGraph(SymbolicFactor(X(1), L(1)))( + SymbolicFactor(X(1), X(2)))))))))( + Node(X(4), SymbolicFactorGraph(SymbolicFactor(X(3), X(4))), + ChildNodes(Node( + L(2), SymbolicFactorGraph(SymbolicFactor(X(4), L(2))), + ChildNodes(Node( + X(5), + SymbolicFactorGraph(SymbolicFactor(X(4), X(5)))( + SymbolicFactor(L(2), X(5))), + ChildNodes(Leaf( + 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);