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

63
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
* 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 <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 {
boost::make_shared<SymbolicFactor>(0,1),
boost::make_shared<SymbolicFactor>(0,2),
@ -100,23 +119,33 @@ namespace gtsam {
boost::make_shared<SymbolicConditional>(_L_, _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 result;
result.insertRoot(boost::make_shared<SymbolicBayesTreeClique>(
boost::make_shared<SymbolicConditional>(
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.addClique(boost::make_shared<SymbolicBayesTreeClique>(
boost::make_shared<SymbolicConditional>(
SymbolicConditional::FromKeys(KeyVector{_T_, _E_, _L_}, 1))),
result.roots().front());
result.addClique(boost::make_shared<SymbolicBayesTreeClique>(
boost::make_shared<SymbolicConditional>(
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;
}

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

@ -34,48 +34,6 @@ using namespace gtsam::symbol_shorthand;
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) {
SymbolicBayesTree bayesTree = asiaBayesTree;
@ -107,14 +65,14 @@ TEST(SymbolicBayesTree, clique_structure) {
graph.emplace_shared<SymbolicFactor>(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));

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

@ -73,43 +73,22 @@ class EliminationTreeTester {
}
};
template <typename FACTORS>
static sharedNode MakeNode(Key key, const FACTORS& factors) {
sharedNode node = boost::make_shared<SymbolicEliminationTree::Node>();
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 <typename FACTORS>
static sharedNode MakeNode(Key key, const FACTORS& factors,
const std::vector<sharedNode>& children) {
sharedNode node = boost::make_shared<SymbolicEliminationTree::Node>();
node->key = key;
SymbolicFactorGraph factorsAsGraph = factors;
node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end());
static sharedNode Node(Key key, const SymbolicFactorGraph& factors,
const std::vector<sharedNode>& children) {
sharedNode node = Leaf(key, factors);
node->children.assign(children.begin(), children.end());
return node;
}
template <typename Class>
class ListOf {
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>;
// Use list_of replacement defined in symbolicExampleGraphs.h
using ChildNodes = ListOf<sharedNode>;
/* ************************************************************************* */
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);