12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Moved ListOf helper and MakeKeys to common header.

release/4.3a0
Frank Dellaert 2023-01-08 11:13:09 -08:00
parent 9675761f71
commit 276394d1d8
3 changed files with 108 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
gtsam/symbolic/tests/symbolicExampleGraphs.h
View File

@ -1,6 +1,6 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010-2023, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list) * 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 * @date sept 15, 2012
* @author Frank Dellaert * @author Frank Dellaert
* @author Michael Kaess * @author Michael Kaess
@ -30,6 +30,25 @@
namespace gtsam { namespace gtsam {
namespace { namespace {
// A small helper class to replace Boost's `list_of` function.
template <typename T>
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<T>() { return result; }
private:
std::vector<T> result;
};
using MakeKeys = ListOf<Key>;
const SymbolicFactorGraph simpleTestGraph1 { const SymbolicFactorGraph simpleTestGraph1 {
boost::make_shared<SymbolicFactor>(0,1), boost::make_shared<SymbolicFactor>(0,1),
boost::make_shared<SymbolicFactor>(0,2), boost::make_shared<SymbolicFactor>(0,2),
@ -100,22 +119,32 @@ namespace gtsam {
boost::make_shared<SymbolicConditional>(_L_, _B_), boost::make_shared<SymbolicConditional>(_L_, _B_),
boost::make_shared<SymbolicConditional>(_B_)}; boost::make_shared<SymbolicConditional>(_B_)};
using sharedClique = SymbolicBayesTreeClique::shared_ptr;
using Children = ListOf<sharedClique>;
inline sharedClique LeafClique(const KeyVector& keys,
DenseIndex nrFrontals) {
return boost::make_shared<SymbolicBayesTreeClique>(
boost::make_shared<SymbolicConditional>(
SymbolicConditional::FromKeys(keys, nrFrontals)));
}
inline sharedClique NodeClique(const KeyVector& keys, DenseIndex nrFrontals,
const std::vector<sharedClique>& 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 __asiaBayesTree() {
SymbolicBayesTree result; SymbolicBayesTree result;
result.insertRoot(boost::make_shared<SymbolicBayesTreeClique>( result.insertRoot(LeafClique(KeyVector{_E_, _L_, _B_}, 3));
boost::make_shared<SymbolicConditional>( result.addClique(LeafClique(KeyVector{_S_, _B_, _L_}, 1),
SymbolicConditional::FromKeys(KeyVector{_E_, _L_, _B_}, 3))));
result.addClique(boost::make_shared<SymbolicBayesTreeClique>(
boost::make_shared<SymbolicConditional>(
SymbolicConditional::FromKeys(KeyVector{_S_, _B_, _L_}, 1))),
result.roots().front()); result.roots().front());
result.addClique(boost::make_shared<SymbolicBayesTreeClique>( result.addClique(LeafClique(KeyVector{_T_, _E_, _L_}, 1),
boost::make_shared<SymbolicConditional>(
SymbolicConditional::FromKeys(KeyVector{_T_, _E_, _L_}, 1))),
result.roots().front()); result.roots().front());
result.addClique(boost::make_shared<SymbolicBayesTreeClique>( result.addClique(LeafClique(KeyVector{_X_, _E_}, 1),
boost::make_shared<SymbolicConditional>(
SymbolicConditional::FromKeys(KeyVector{_X_, _E_}, 1))),
result.roots().front()); result.roots().front());
return result; return result;
} }

110
gtsam/symbolic/tests/testSymbolicBayesTree.cpp
View File

@ -34,48 +34,6 @@ using namespace gtsam::symbol_shorthand;
static bool debug = false; static bool debug = false;
using sharedClique = SymbolicBayesTreeClique::shared_ptr;
template <typename T>
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<T>() { return result; }
private:
std::vector<T> result;
};
using MakeKeys = ListOf<Key>;
using MakeCliques = ListOf<sharedClique>;
namespace {
/* ************************************************************************* */
// Helper functions for below
sharedClique MakeClique(const KeyVector& keys, DenseIndex nrFrontals) {
return boost::make_shared<SymbolicBayesTreeClique>(
boost::make_shared<SymbolicConditional>(
SymbolicConditional::FromKeys(keys, nrFrontals)));
}
sharedClique MakeClique(const KeyVector& keys, DenseIndex nrFrontals,
const std::vector<sharedClique>& children) {
sharedClique clique = boost::make_shared<SymbolicBayesTreeClique>(
boost::make_shared<SymbolicConditional>(
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; SymbolicBayesTree bayesTree = asiaBayesTree;
@ -107,14 +65,14 @@ TEST(SymbolicBayesTree, clique_structure) {
graph.emplace_shared<SymbolicFactor>(X(5), L(3)); graph.emplace_shared<SymbolicFactor>(X(5), L(3));
SymbolicBayesTree expected; SymbolicBayesTree expected;
expected.insertRoot(MakeClique( expected.insertRoot(NodeClique(
MakeKeys(X(2))(X(3)), 2, MakeKeys(X(2))(X(3)), 2,
MakeCliques(MakeClique( Children(NodeClique(
MakeKeys(X(4))(X(3)), 1, MakeKeys(X(4))(X(3)), 1,
MakeCliques(MakeClique( Children(NodeClique(
MakeKeys(X(5))(L(2))(X(4)), 2, MakeKeys(X(5))(L(2))(X(4)), 2,
MakeCliques(MakeClique(MakeKeys(L(3))(X(4))(X(5)), 1))))))( Children(LeafClique(MakeKeys(L(3))(X(4))(X(5)), 1))))))(
MakeClique(MakeKeys(X(1))(L(1))(X(2)), 2)))); 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)}; 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); _F_ = F(0);
SymbolicBayesTree bayesTreeOrig; SymbolicBayesTree bayesTreeOrig;
bayesTreeOrig.insertRoot(MakeClique( bayesTreeOrig.insertRoot(NodeClique(
MakeKeys(_A_)(_B_), 2, MakeKeys(_A_)(_B_), 2,
MakeCliques(MakeClique(MakeKeys(_C_)(_A_), 1, Children(NodeClique(MakeKeys(_C_)(_A_), 1,
MakeCliques(MakeClique(MakeKeys(_D_)(_C_), 1))))( Children(LeafClique(MakeKeys(_D_)(_C_), 1))))(
MakeClique(MakeKeys(_E_)(_B_), 1, NodeClique(MakeKeys(_E_)(_B_), 1,
MakeCliques(MakeClique(MakeKeys(_F_)(_E_), 1)))))); Children(LeafClique(MakeKeys(_F_)(_E_), 1))))));
SymbolicBayesTree bayesTree = bayesTreeOrig; SymbolicBayesTree bayesTree = bayesTreeOrig;
@ -519,8 +477,8 @@ TEST(SymbolicBayesTree, thinTree) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SymbolicBayesTree, forest_joint) { TEST(SymbolicBayesTree, forest_joint) {
// Create forest // Create forest
sharedClique root1 = MakeClique(MakeKeys(1), 1); sharedClique root1 = LeafClique(MakeKeys(1), 1);
sharedClique root2 = MakeClique(MakeKeys(2), 1); sharedClique root2 = LeafClique(MakeKeys(2), 1);
SymbolicBayesTree bayesTree; SymbolicBayesTree bayesTree;
bayesTree.insertRoot(root1); bayesTree.insertRoot(root1);
bayesTree.insertRoot(root2); bayesTree.insertRoot(root2);
@ -605,24 +563,24 @@ TEST(SymbolicBayesTree, linear_smoother_shortcuts) {
TEST(SymbolicBayesTree, complicatedMarginal) { TEST(SymbolicBayesTree, complicatedMarginal) {
// Create the conditionals to go in the BayesTree // Create the conditionals to go in the BayesTree
sharedClique cur; sharedClique cur;
sharedClique root = MakeClique(MakeKeys(11)(12), 2); sharedClique root = LeafClique(MakeKeys(11)(12), 2);
cur = root; 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.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; root->children.back()->parent_ = root;
cur = root->children.back(); 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->children.back()->parent_ = cur;
cur = cur->children.back(); 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.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; cur->children.back()->parent_ = cur;
// Create Bayes Tree // Create Bayes Tree
@ -707,12 +665,12 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) {
// | | | - P( 0 | 1 5) // | | | - P( 0 | 1 5)
SymbolicBayesTree expected; SymbolicBayesTree expected;
expected.insertRoot( expected.insertRoot(
MakeClique(MakeKeys(4)(2)(3), 3, NodeClique(MakeKeys(4)(2)(3), 3,
MakeCliques(MakeClique( Children(NodeClique(
MakeKeys(1)(2)(4), 1, MakeKeys(1)(2)(4), 1,
MakeCliques(MakeClique( Children(NodeClique(
MakeKeys(5)(1)(4), 1, MakeKeys(5)(1)(4), 1,
MakeCliques(MakeClique(MakeKeys(0)(1)(5), 1)))))))); Children(LeafClique(MakeKeys(0)(1)(5), 1))))))));
SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
@ -737,23 +695,23 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) {
// | - P( 2 | 1 3) // | - P( 2 | 1 3)
SymbolicBayesTree expected; SymbolicBayesTree expected;
#if defined(__APPLE__) #if defined(__APPLE__)
expected.insertRoot(MakeClique( expected.insertRoot(NodeClique(
MakeKeys(1)(0)(3), 3, MakeKeys(1)(0)(3), 3,
MakeCliques(MakeClique(MakeKeys(4)(0)(3), 1, Children(NodeClique(MakeKeys(4)(0)(3), 1,
MakeCliques(MakeClique(MakeKeys(5)(0)(4), 1))))( Children(LeafClique(MakeKeys(5)(0)(4), 1))))(
MakeClique(MakeKeys(2)(1)(3), 1)))); LeafClique(MakeKeys(2)(1)(3), 1))));
#elif defined(_WIN32) #elif defined(_WIN32)
expected.insertRoot(MakeClique( expected.insertRoot(NodeClique(
MakeKeys(3)(5)(2), 3, MakeKeys(3)(5)(2), 3,
MakeCliques(MakeClique(MakeKeys(4)(3)(5), 1, Children(NodeClique(MakeKeys(4)(3)(5), 1,
MakeCliques(MakeClique(MakeKeys(0)(2)(5), 1))))( Children(LeafClique(MakeKeys(0)(2)(5), 1))))(
MakeClique(MakeKeys(1)(0)(2), 1)))); LeafClique(MakeKeys(1)(0)(2), 1))));
#else #else
expected.insertRoot(MakeClique( expected.insertRoot(NodeClique(
MakeKeys(2)(4)(1), 3, MakeKeys(2)(4)(1), 3,
MakeCliques(MakeClique(MakeKeys(0)(1)(4), 1, Children(NodeClique(MakeKeys(0)(1)(4), 1,
MakeCliques(MakeClique(MakeKeys(5)(0)(4), 1))))( Children(LeafClique(MakeKeys(5)(0)(4), 1))))(
MakeClique(MakeKeys(3)(2)(4), 1)))); LeafClique(MakeKeys(3)(2)(4), 1))));
#endif #endif
SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering); SymbolicBayesTree actual = *sfg.eliminateMultifrontal(ordering);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));

55
gtsam/symbolic/tests/testSymbolicEliminationTree.cpp
View File

@ -73,43 +73,22 @@ class EliminationTreeTester {
} }
}; };
template <typename FACTORS> static sharedNode Leaf(Key key, const SymbolicFactorGraph& factors) {
static sharedNode MakeNode(Key key, const FACTORS& factors) { sharedNode node(new SymbolicEliminationTree::Node());
sharedNode node = boost::make_shared<SymbolicEliminationTree::Node>();
node->key = key; node->key = key;
SymbolicFactorGraph factorsAsGraph = factors; node->factors = factors;
node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end());
return node; return node;
} }
template <typename FACTORS> static sharedNode Node(Key key, const SymbolicFactorGraph& factors,
static sharedNode MakeNode(Key key, const FACTORS& factors,
const std::vector<sharedNode>& children) { const std::vector<sharedNode>& children) {
sharedNode node = boost::make_shared<SymbolicEliminationTree::Node>(); sharedNode node = Leaf(key, factors);
node->key = key;
SymbolicFactorGraph factorsAsGraph = factors;
node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end());
node->children.assign(children.begin(), children.end()); node->children.assign(children.begin(), children.end());
return node; return node;
} }
template <typename Class> // Use list_of replacement defined in symbolicExampleGraphs.h
class ListOf { using ChildNodes = ListOf<sharedNode>;
public:
ListOf(Class&& c) { result.push_back(c); }
ListOf& operator()(Class&& c) {
result.push_back(c);
return *this;
}
operator std::vector<Class>() { return result; }
private:
std::vector<Class> result;
};
using Nodes = ListOf<sharedNode>;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(EliminationTree, Create) { TEST(EliminationTree, Create) {
@ -143,25 +122,23 @@ TEST(EliminationTree, Create2) {
graph += SymbolicFactor(X(4), L(3)); graph += SymbolicFactor(X(4), L(3));
graph += SymbolicFactor(X(5), L(3)); graph += SymbolicFactor(X(5), L(3));
SymbolicEliminationTree expected = SymbolicEliminationTree expected = EliminationTreeTester::MakeTree(ChildNodes(
EliminationTreeTester::MakeTree(Nodes(MakeNode( Node(X(3), SymbolicFactorGraph(),
X(3), SymbolicFactorGraph(), ChildNodes(Node(
Nodes(MakeNode(
X(2), SymbolicFactorGraph(SymbolicFactor(X(2), X(3))), X(2), SymbolicFactorGraph(SymbolicFactor(X(2), X(3))),
Nodes(MakeNode( ChildNodes(Node(
L(1), SymbolicFactorGraph(SymbolicFactor(X(2), L(1))), L(1), SymbolicFactorGraph(SymbolicFactor(X(2), L(1))),
Nodes(MakeNode( ChildNodes(Leaf(
X(1), SymbolicFactorGraph(SymbolicFactor(X(1), L(1)))( X(1), SymbolicFactorGraph(SymbolicFactor(X(1), L(1)))(
SymbolicFactor(X(1), X(2)))))))))( SymbolicFactor(X(1), X(2)))))))))(
MakeNode( Node(X(4), SymbolicFactorGraph(SymbolicFactor(X(3), X(4))),
X(4), SymbolicFactorGraph(SymbolicFactor(X(3), X(4))), ChildNodes(Node(
Nodes(MakeNode(
L(2), SymbolicFactorGraph(SymbolicFactor(X(4), L(2))), L(2), SymbolicFactorGraph(SymbolicFactor(X(4), L(2))),
Nodes(MakeNode( ChildNodes(Node(
X(5), X(5),
SymbolicFactorGraph(SymbolicFactor(X(4), X(5)))( SymbolicFactorGraph(SymbolicFactor(X(4), X(5)))(
SymbolicFactor(L(2), X(5))), SymbolicFactor(L(2), X(5))),
Nodes(MakeNode( ChildNodes(Leaf(
L(3), L(3),
SymbolicFactorGraph(SymbolicFactor(X(4), L(3)))( SymbolicFactorGraph(SymbolicFactor(X(4), L(3)))(
SymbolicFactor(X(5), L(3)))))))))))))); SymbolicFactor(X(5), L(3))))))))))))));