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

Heavy lift on removing boost::assign from symbolic tests.

release/4.3a0
Frank Dellaert 2023-01-07 23:17:54 -08:00
parent 7e4b033ece
commit 4e078e41f1
6 changed files with 379 additions and 342 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/symbolic/SymbolicBayesNet.h
View File

@ -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<SymbolicConditional>(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<SymbolicConditional>(c));
return *this;
}
/// Destructor
virtual ~SymbolicBayesNet() {}

16
gtsam/symbolic/SymbolicFactorGraph.h
View File

@ -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<SymbolicFactor>(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<SymbolicFactor>(c));
return *this;
}
/// Destructor
virtual ~SymbolicFactorGraph() {}

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

@ -17,16 +17,12 @@
* @author Viorela Ila
*/
#include <gtsam/symbolic/SymbolicBayesTree.h>
#include <gtsam/symbolic/SymbolicBayesNet.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/symbolic/SymbolicBayesNet.h>
#include <gtsam/symbolic/SymbolicBayesTree.h>
#include <gtsam/symbolic/tests/symbolicExampleGraphs.h>
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/list.hpp>
#include <boost/range/adaptor/indirected.hpp>
using namespace boost::assign;
using boost::adaptors::indirected;
#include <CppUnitLite/TestHarness.h>
@ -38,37 +34,50 @@ 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
template<typename KEYS>
SymbolicBayesTreeClique::shared_ptr MakeClique(const KEYS& keys, DenseIndex nrFrontals)
{
return boost::make_shared<SymbolicBayesTreeClique>(
/* ************************************************************************* */
// Helper functions for below
sharedClique MakeClique(const KeyVector& keys, DenseIndex nrFrontals) {
return boost::make_shared<SymbolicBayesTreeClique>(
boost::make_shared<SymbolicConditional>(
SymbolicConditional::FromKeys(keys, nrFrontals)));
}
template<typename KEYS, typename CHILDREN>
SymbolicBayesTreeClique::shared_ptr MakeClique(
const KEYS& keys, DenseIndex nrFrontals, const CHILDREN& children)
{
SymbolicBayesTreeClique::shared_ptr clique =
boost::make_shared<SymbolicBayesTreeClique>(
boost::make_shared<SymbolicConditional>(
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<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;
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<SymbolicFactor>(X(1), L(1));
graph.emplace_shared<SymbolicFactor>(X(1), X(2));
graph.emplace_shared<SymbolicFactor>(X(2), L(1));
graph.emplace_shared<SymbolicFactor>(X(2), X(3));
graph.emplace_shared<SymbolicFactor>(X(3), X(4));
graph.emplace_shared<SymbolicFactor>(X(4), L(2));
graph.emplace_shared<SymbolicFactor>(X(4), X(5));
graph.emplace_shared<SymbolicFactor>(L(2), X(5));
graph.emplace_shared<SymbolicFactor>(X(4), L(3));
graph.emplace_shared<SymbolicFactor>(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<SymbolicFactor>(_A_, _B_);
expected.emplace_shared<SymbolicFactor>(_C_, _A_);
SymbolicBayesTree::Cliques expectedOrphans =
std::list<sharedClique>{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<SymbolicFactor>(_A_, _B_);
expected2.emplace_shared<SymbolicFactor>(_E_, _B_);
SymbolicBayesTree::Cliques expectedOrphans2 =
std::list<sharedClique>{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<SymbolicFactor>(_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<sharedClique>{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<SymbolicFactor>(_E_, _L_, _B_);
expected.emplace_shared<SymbolicFactor>(_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<sharedClique>{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<IndexFactor> newFactor(new IndexFactor(_S_,_B_));
// boost::shared_ptr<IndexFactor> 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<KeyVector>(MakeKeys(_E_)(_L_)(_B_), 3);
expected +=
SymbolicConditional::FromKeys<KeyVector>(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<sharedClique>{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<IndexFactor> newFactor2(new IndexFactor(_B_));
// boost::shared_ptr<IndexFactor> 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<KeyVector>(MakeKeys(_E_)(_L_)(_B_), 3))(
SymbolicConditional::FromKeys<KeyVector>(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<sharedClique>{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<KeyVector>(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<KeyVector>(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<SymbolicConditional>(14));
bayesNet.emplace_shared<SymbolicConditional>(14);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(13, 14));
bayesNet.push_back(boost::make_shared<SymbolicConditional>(12, 14));
bayesNet.emplace_shared<SymbolicConditional>(13, 14);
bayesNet.emplace_shared<SymbolicConditional>(12, 14);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(11, 13, 14));
bayesNet.push_back(boost::make_shared<SymbolicConditional>(10, 13, 14));
bayesNet.push_back(boost::make_shared<SymbolicConditional>(9, 12, 14));
bayesNet.push_back(boost::make_shared<SymbolicConditional>(8, 12, 14));
bayesNet.emplace_shared<SymbolicConditional>(11, 13, 14);
bayesNet.emplace_shared<SymbolicConditional>(10, 13, 14);
bayesNet.emplace_shared<SymbolicConditional>(9, 12, 14);
bayesNet.emplace_shared<SymbolicConditional>(8, 12, 14);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(7, 11, 13));
bayesNet.push_back(boost::make_shared<SymbolicConditional>(6, 11, 13));
bayesNet.push_back(boost::make_shared<SymbolicConditional>(5, 10, 13));
bayesNet.push_back(boost::make_shared<SymbolicConditional>(4, 10, 13));
bayesNet.emplace_shared<SymbolicConditional>(7, 11, 13);
bayesNet.emplace_shared<SymbolicConditional>(6, 11, 13);
bayesNet.emplace_shared<SymbolicConditional>(5, 10, 13);
bayesNet.emplace_shared<SymbolicConditional>(4, 10, 13);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(3, 9, 12));
bayesNet.push_back(boost::make_shared<SymbolicConditional>(2, 9, 12));
bayesNet.push_back(boost::make_shared<SymbolicConditional>(1, 8, 12));
bayesNet.push_back(boost::make_shared<SymbolicConditional>(0, 8, 12));
bayesNet.emplace_shared<SymbolicConditional>(3, 9, 12);
bayesNet.emplace_shared<SymbolicConditional>(2, 9, 12);
bayesNet.emplace_shared<SymbolicConditional>(1, 8, 12);
bayesNet.emplace_shared<SymbolicConditional>(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<SymbolicConditional>(8));
expected.push_back(boost::make_shared<SymbolicConditional>(2, 8));
expected.emplace_shared<SymbolicConditional>(8);
expected.emplace_shared<SymbolicConditional>(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<SymbolicConditional>(2));
expected.push_back(boost::make_shared<SymbolicConditional>(1, 2));
expected.emplace_shared<SymbolicConditional>(2);
expected.emplace_shared<SymbolicConditional>(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<SymbolicConditional>(2, 6));
expected.push_back(boost::make_shared<SymbolicConditional>(6));
expected.emplace_shared<SymbolicConditional>(2, 6);
expected.emplace_shared<SymbolicConditional>(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<SymbolicConditional>(6));
expected.push_back(boost::make_shared<SymbolicConditional>(4, 6));
expected.emplace_shared<SymbolicConditional>(6);
expected.emplace_shared<SymbolicConditional>(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);
}
/* ************************************************************************* */

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

@ -17,47 +17,45 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <vector>
#include <boost/make_shared.hpp>
#include <boost/assign/list_of.hpp>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/symbolic/SymbolicEliminationTree.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/symbolic/SymbolicEliminationTree.h>
#include <boost/make_shared.hpp>
#include <vector>
#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<typename ROOTS>
static SymbolicEliminationTree MakeTree(const ROOTS& roots)
{
static SymbolicEliminationTree MakeTree(
const std::vector<sharedNode>& roots) {
SymbolicEliminationTree et;
et.roots_.assign(roots.begin(), roots.end());
return et;
}
};
template<typename FACTORS>
static SymbolicEliminationTree::sharedNode MakeNode(Key key, const FACTORS& factors)
{
SymbolicEliminationTree::sharedNode node = boost::make_shared<SymbolicEliminationTree::Node>();
template <typename FACTORS>
static sharedNode MakeNode(Key key, const FACTORS& factors) {
sharedNode node = boost::make_shared<SymbolicEliminationTree::Node>();
node->key = key;
SymbolicFactorGraph factorsAsGraph = factors;
node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end());
return node;
}
template<typename FACTORS, typename CHILDREN>
static SymbolicEliminationTree::sharedNode MakeNode(Key key, const FACTORS& factors, const CHILDREN& children)
{
SymbolicEliminationTree::sharedNode node = boost::make_shared<SymbolicEliminationTree::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());
@ -97,24 +93,39 @@ static SymbolicEliminationTree::sharedNode MakeNode(Key key, const FACTORS& fact
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>;
/* ************************************************************************* */
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));
}

1
gtsam/symbolic/tests/testSymbolicFactor.cpp
View File

@ -26,7 +26,6 @@
using namespace std;
using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */
#ifdef TRACK_ELIMINATE

44
gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
View File

@ -15,33 +15,29 @@
* @author Christian Potthast
**/
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/symbolic/SymbolicBayesNet.h>
#include <gtsam/symbolic/SymbolicBayesTree.h>
#include <gtsam/symbolic/SymbolicConditional.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <gtsam/symbolic/tests/symbolicExampleGraphs.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/list_of.hpp>
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<SymbolicBayesTreeClique>(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<SymbolicConditional>(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<SymbolicConditional>(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));