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); 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 /// Destructor
virtual ~SymbolicBayesNet() {} virtual ~SymbolicBayesNet() {}

16
gtsam/symbolic/SymbolicFactorGraph.h
View File

@ -90,6 +90,22 @@ namespace gtsam {
for (auto&& f : sharedFactors) factors_.push_back(f); 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 /// Destructor
virtual ~SymbolicFactorGraph() {} virtual ~SymbolicFactorGraph() {}

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

@ -17,16 +17,12 @@
* @author Viorela Ila * @author Viorela Ila
*/ */
#include <gtsam/symbolic/SymbolicBayesTree.h>
#include <gtsam/symbolic/SymbolicBayesNet.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/symbolic/SymbolicBayesNet.h>
#include <gtsam/symbolic/SymbolicBayesTree.h>
#include <gtsam/symbolic/tests/symbolicExampleGraphs.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> #include <boost/range/adaptor/indirected.hpp>
using namespace boost::assign;
using boost::adaptors::indirected; using boost::adaptors::indirected;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -38,37 +34,50 @@ 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 { namespace {
/* ************************************************************************* */
/* ************************************************************************* */ // Helper functions for below
// Helper functions for below sharedClique MakeClique(const KeyVector& keys, DenseIndex nrFrontals) {
template<typename KEYS> return boost::make_shared<SymbolicBayesTreeClique>(
SymbolicBayesTreeClique::shared_ptr MakeClique(const KEYS& keys, DenseIndex nrFrontals)
{
return boost::make_shared<SymbolicBayesTreeClique>(
boost::make_shared<SymbolicConditional>( boost::make_shared<SymbolicConditional>(
SymbolicConditional::FromKeys(keys, nrFrontals))); 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;
}
} }
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;
bayesTree.clear(); bayesTree.clear();
@ -79,34 +88,35 @@ TEST(SymbolicBayesTree, clear)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SymbolicBayesTree, clique_structure) TEST(SymbolicBayesTree, clique_structure) {
{
// l1 l2 // l1 l2
// / | / | // / | / |
// x1 --- x2 --- x3 --- x4 --- x5 // x1 --- x2 --- x3 --- x4 --- x5
// \ | // \ |
// l3 // l3
SymbolicFactorGraph graph; SymbolicFactorGraph graph;
graph += SymbolicFactor(X(1), L(1)); graph.emplace_shared<SymbolicFactor>(X(1), L(1));
graph += SymbolicFactor(X(1), X(2)); graph.emplace_shared<SymbolicFactor>(X(1), X(2));
graph += SymbolicFactor(X(2), L(1)); graph.emplace_shared<SymbolicFactor>(X(2), L(1));
graph += SymbolicFactor(X(2), X(3)); graph.emplace_shared<SymbolicFactor>(X(2), X(3));
graph += SymbolicFactor(X(3), X(4)); graph.emplace_shared<SymbolicFactor>(X(3), X(4));
graph += SymbolicFactor(X(4), L(2)); graph.emplace_shared<SymbolicFactor>(X(4), L(2));
graph += SymbolicFactor(X(4), X(5)); graph.emplace_shared<SymbolicFactor>(X(4), X(5));
graph += SymbolicFactor(L(2), X(5)); graph.emplace_shared<SymbolicFactor>(L(2), X(5));
graph += SymbolicFactor(X(4), L(3)); graph.emplace_shared<SymbolicFactor>(X(4), L(3));
graph += SymbolicFactor(X(5), L(3)); graph.emplace_shared<SymbolicFactor>(X(5), L(3));
SymbolicBayesTree expected; SymbolicBayesTree expected;
expected.insertRoot( expected.insertRoot(MakeClique(
MakeClique(list_of(X(2)) (X(3)), 2, list_of MakeKeys(X(2))(X(3)), 2,
(MakeClique(list_of(X(4)) (X(3)), 1, list_of MakeCliques(MakeClique(
(MakeClique(list_of(X(5)) (L(2)) (X(4)), 2, list_of MakeKeys(X(4))(X(3)), 1,
(MakeClique(list_of(L(3)) (X(4)) (X(5)), 1)))))) MakeCliques(MakeClique(
(MakeClique(list_of(X(1)) (L(1)) (X(2)), 2)))); 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); 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 D|C F|E
*/ */
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, removePath ) TEST(BayesTree, removePath) {
{ const Key _A_ = A(0), _B_ = B(0), _C_ = C(0), _D_ = D(0), _E_ = E(0),
const Key _A_=A(0), _B_=B(0), _C_=C(0), _D_=D(0), _E_=E(0), _F_=F(0); _F_ = F(0);
SymbolicBayesTree bayesTreeOrig; SymbolicBayesTree bayesTreeOrig;
bayesTreeOrig.insertRoot( bayesTreeOrig.insertRoot(MakeClique(
MakeClique(list_of(_A_)(_B_), 2, list_of MakeKeys(_A_)(_B_), 2,
(MakeClique(list_of(_C_)(_A_), 1, list_of MakeCliques(MakeClique(MakeKeys(_C_)(_A_), 1,
(MakeClique(list_of(_D_)(_C_), 1)))) MakeCliques(MakeClique(MakeKeys(_D_)(_C_), 1))))(
(MakeClique(list_of(_E_)(_B_), 1, list_of MakeClique(MakeKeys(_E_)(_B_), 1,
(MakeClique(list_of(_F_)(_E_), 1)))))); MakeCliques(MakeClique(MakeKeys(_F_)(_E_), 1))))));
SymbolicBayesTree bayesTree = bayesTreeOrig; SymbolicBayesTree bayesTree = bayesTreeOrig;
// remove C, expected outcome: factor graph with ABC, // remove C, expected outcome: factor graph with ABC,
// Bayes Tree now contains two orphan trees: D|C and E|B,F|E // Bayes Tree now contains two orphan trees: D|C and E|B,F|E
SymbolicFactorGraph expected; SymbolicFactorGraph expected;
expected += SymbolicFactor(_A_,_B_); expected.emplace_shared<SymbolicFactor>(_A_, _B_);
expected += SymbolicFactor(_C_,_A_); expected.emplace_shared<SymbolicFactor>(_C_, _A_);
SymbolicBayesTree::Cliques expectedOrphans; SymbolicBayesTree::Cliques expectedOrphans =
expectedOrphans += bayesTree[_D_], bayesTree[_E_]; std::list<sharedClique>{bayesTree[_D_], bayesTree[_E_]};
SymbolicBayesNet bn; SymbolicBayesNet bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTree::Cliques orphans;
bayesTree.removePath(bayesTree[_C_], &bn, &orphans); bayesTree.removePath(bayesTree[_C_], &bn, &orphans);
SymbolicFactorGraph factors(bn); SymbolicFactorGraph factors(bn);
CHECK(assert_equal(expected, factors)); CHECK(assert_equal(expected, factors));
CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); CHECK(assert_container_equal(expectedOrphans | indirected,
orphans | indirected));
bayesTree = bayesTreeOrig; bayesTree = bayesTreeOrig;
// remove E: factor graph with EB; E|B removed from second orphan tree // remove E: factor graph with EB; E|B removed from second orphan tree
SymbolicFactorGraph expected2; SymbolicFactorGraph expected2;
expected2 += SymbolicFactor(_A_,_B_); expected2.emplace_shared<SymbolicFactor>(_A_, _B_);
expected2 += SymbolicFactor(_E_,_B_); expected2.emplace_shared<SymbolicFactor>(_E_, _B_);
SymbolicBayesTree::Cliques expectedOrphans2; SymbolicBayesTree::Cliques expectedOrphans2 =
expectedOrphans2 += bayesTree[_F_]; std::list<sharedClique>{bayesTree[_F_], bayesTree[_C_]};
expectedOrphans2 += bayesTree[_C_];
SymbolicBayesNet bn2; SymbolicBayesNet bn2;
SymbolicBayesTree::Cliques orphans2; SymbolicBayesTree::Cliques orphans2;
bayesTree.removePath(bayesTree[_E_], &bn2, &orphans2); bayesTree.removePath(bayesTree[_E_], &bn2, &orphans2);
SymbolicFactorGraph factors2(bn2); SymbolicFactorGraph factors2(bn2);
CHECK(assert_equal(expected2, factors2)); 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; SymbolicBayesTree bayesTree = asiaBayesTree;
// Call remove-path with clique B // Call remove-path with clique B
@ -180,16 +190,16 @@ TEST( BayesTree, removePath2 )
// Check expected outcome // Check expected outcome
SymbolicFactorGraph expected; SymbolicFactorGraph expected;
expected += SymbolicFactor(_E_,_L_,_B_); expected.emplace_shared<SymbolicFactor>(_E_, _L_, _B_);
CHECK(assert_equal(expected, factors)); CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans; SymbolicBayesTree::Cliques expectedOrphans =
expectedOrphans += bayesTree[_S_], bayesTree[_T_], bayesTree[_X_]; std::list<sharedClique>{bayesTree[_S_], bayesTree[_T_], bayesTree[_X_]};
CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); CHECK(assert_container_equal(expectedOrphans | indirected,
orphans | indirected));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(BayesTree, removePath3) TEST(BayesTree, removePath3) {
{
SymbolicBayesTree bayesTree = asiaBayesTree; SymbolicBayesTree bayesTree = asiaBayesTree;
// Call remove-path with clique T // Call remove-path with clique T
@ -200,199 +210,185 @@ TEST(BayesTree, removePath3)
// Check expected outcome // Check expected outcome
SymbolicFactorGraph expected; SymbolicFactorGraph expected;
expected += SymbolicFactor(_E_, _L_, _B_); expected.emplace_shared<SymbolicFactor>(_E_, _L_, _B_);
expected += SymbolicFactor(_T_, _E_, _L_); expected.emplace_shared<SymbolicFactor>(_T_, _E_, _L_);
CHECK(assert_equal(expected, factors)); CHECK(assert_equal(expected, factors));
SymbolicBayesTree::Cliques expectedOrphans; SymbolicBayesTree::Cliques expectedOrphans =
expectedOrphans += bayesTree[_S_], bayesTree[_X_]; std::list<sharedClique>{bayesTree[_S_], bayesTree[_X_]};
CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); 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 // Check if subtree exists
if (subtree) { if (subtree) {
cliques.push_back(subtree); cliques.push_back(subtree);
// Recursive call over all child cliques // Recursive call over all child cliques
for(SymbolicBayesTree::sharedClique& childClique: subtree->children) { for (SymbolicBayesTree::sharedClique& childClique : subtree->children) {
getAllCliques(childClique,cliques); getAllCliques(childClique, cliques);
} }
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, shortcutCheck ) TEST(BayesTree, shortcutCheck) {
{ const Key _A_ = 6, _B_ = 5, _C_ = 4, _D_ = 3, _E_ = 2, _F_ = 1, _G_ = 0;
const Key _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0; auto chain =
SymbolicFactorGraph chain = list_of SymbolicFactorGraph(SymbolicFactor(_A_))(SymbolicFactor(_B_, _A_))(
(SymbolicFactor(_A_)) SymbolicFactor(_C_, _A_))(SymbolicFactor(_D_, _C_))(SymbolicFactor(
(SymbolicFactor(_B_, _A_)) _E_, _B_))(SymbolicFactor(_F_, _E_))(SymbolicFactor(_G_, _F_));
(SymbolicFactor(_C_, _A_)) Ordering ordering{_G_, _F_, _E_, _D_, _C_, _B_, _A_};
(SymbolicFactor(_D_, _C_))
(SymbolicFactor(_E_, _B_))
(SymbolicFactor(_F_, _E_))
(SymbolicFactor(_G_, _F_));
Ordering ordering(list_of(_G_)(_F_)(_E_)(_D_)(_C_)(_B_)(_A_));
SymbolicBayesTree bayesTree = *chain.eliminateMultifrontal(ordering); SymbolicBayesTree bayesTree = *chain.eliminateMultifrontal(ordering);
//bayesTree.saveGraph("BT1.dot"); // bayesTree.saveGraph("BT1.dot");
SymbolicBayesTree::sharedClique rootClique = bayesTree.roots().front(); SymbolicBayesTree::sharedClique rootClique = bayesTree.roots().front();
//rootClique->printTree(); // rootClique->printTree();
SymbolicBayesTree::Cliques allCliques; SymbolicBayesTree::Cliques allCliques;
getAllCliques(rootClique,allCliques); getAllCliques(rootClique, allCliques);
for(SymbolicBayesTree::sharedClique& clique: allCliques) { for (SymbolicBayesTree::sharedClique& clique : allCliques) {
//clique->print("Clique#"); // clique->print("Clique#");
SymbolicBayesNet bn = clique->shortcut(rootClique); SymbolicBayesNet bn = clique->shortcut(rootClique);
//bn.print("Shortcut:\n"); // bn.print("Shortcut:\n");
//cout << endl; // cout << endl;
} }
// Check if all the cached shortcuts are cleared // Check if all the cached shortcuts are cleared
rootClique->deleteCachedShortcuts(); rootClique->deleteCachedShortcuts();
for(SymbolicBayesTree::sharedClique& clique: allCliques) { for (SymbolicBayesTree::sharedClique& clique : allCliques) {
bool notCleared = clique->cachedSeparatorMarginal().is_initialized(); bool notCleared = clique->cachedSeparatorMarginal().is_initialized();
CHECK( notCleared == false); CHECK(notCleared == false);
} }
EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals()); EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals());
// for(SymbolicBayesTree::sharedClique& clique: allCliques) { // for(SymbolicBayesTree::sharedClique& clique: allCliques) {
// clique->print("Clique#"); // clique->print("Clique#");
// if(clique->cachedShortcut()){ // if(clique->cachedShortcut()){
// bn = clique->cachedShortcut().get(); // bn = clique->cachedShortcut().get();
// bn.print("Shortcut:\n"); // bn.print("Shortcut:\n");
// } // }
// else // else
// cout << "Not Initialized" << endl; // cout << "Not Initialized" << endl;
// cout << endl; // cout << endl;
// } // }
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, removeTop ) TEST(BayesTree, removeTop) {
{
SymbolicBayesTree bayesTree = asiaBayesTree; SymbolicBayesTree bayesTree = asiaBayesTree;
// create a new factor to be inserted // 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 // Remove the contaminated part of the Bayes tree
SymbolicBayesNet bn; SymbolicBayesNet bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTree::Cliques orphans;
bayesTree.removeTop(list_of(_B_)(_S_), &bn, &orphans); bayesTree.removeTop(MakeKeys(_B_)(_S_), &bn, &orphans);
// Check expected outcome // Check expected outcome
SymbolicBayesNet expected; SymbolicBayesNet expected;
expected += SymbolicConditional::FromKeys(list_of(_E_)(_L_)(_B_), 3); expected +=
expected += SymbolicConditional::FromKeys(list_of(_S_)(_B_)(_L_), 1); SymbolicConditional::FromKeys<KeyVector>(MakeKeys(_E_)(_L_)(_B_), 3);
expected +=
SymbolicConditional::FromKeys<KeyVector>(MakeKeys(_S_)(_B_)(_L_), 1);
CHECK(assert_equal(expected, bn)); CHECK(assert_equal(expected, bn));
SymbolicBayesTree::Cliques expectedOrphans; SymbolicBayesTree::Cliques expectedOrphans =
expectedOrphans += bayesTree[_T_], bayesTree[_X_]; std::list<sharedClique>{bayesTree[_T_], bayesTree[_X_]};
CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); CHECK(assert_container_equal(expectedOrphans | indirected,
orphans | indirected));
// Try removeTop again with a factor that should not change a thing // 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; SymbolicBayesNet bn2;
SymbolicBayesTree::Cliques orphans2; SymbolicBayesTree::Cliques orphans2;
bayesTree.removeTop(list_of(_B_), &bn2, &orphans2); bayesTree.removeTop(MakeKeys(_B_), &bn2, &orphans2);
SymbolicFactorGraph factors2(bn2); SymbolicFactorGraph factors2(bn2);
SymbolicFactorGraph expected2; SymbolicFactorGraph expected2;
CHECK(assert_equal(expected2, factors2)); CHECK(assert_equal(expected2, factors2));
SymbolicBayesTree::Cliques expectedOrphans2; 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; SymbolicBayesTree bayesTree = asiaBayesTree;
// create two factors to be inserted // create two factors to be inserted
//SymbolicFactorGraph newFactors; // SymbolicFactorGraph newFactors;
//newFactors.push_factor(_B_); // newFactors.push_factor(_B_);
//newFactors.push_factor(_S_); // newFactors.push_factor(_S_);
// Remove the contaminated part of the Bayes tree // Remove the contaminated part of the Bayes tree
SymbolicBayesNet bn; SymbolicBayesNet bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTree::Cliques orphans;
bayesTree.removeTop(list_of(_T_), &bn, &orphans); bayesTree.removeTop(MakeKeys(_T_), &bn, &orphans);
// Check expected outcome // Check expected outcome
SymbolicBayesNet expected = list_of auto expected = SymbolicBayesNet(
(SymbolicConditional::FromKeys(list_of(_E_)(_L_)(_B_), 3)) SymbolicConditional::FromKeys<KeyVector>(MakeKeys(_E_)(_L_)(_B_), 3))(
(SymbolicConditional::FromKeys(list_of(_T_)(_E_)(_L_), 1)); SymbolicConditional::FromKeys<KeyVector>(MakeKeys(_T_)(_E_)(_L_), 1));
CHECK(assert_equal(expected, bn)); CHECK(assert_equal(expected, bn));
SymbolicBayesTree::Cliques expectedOrphans; SymbolicBayesTree::Cliques expectedOrphans =
expectedOrphans += bayesTree[_S_], bayesTree[_X_]; std::list<sharedClique>{bayesTree[_S_], bayesTree[_X_]};
CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected)); CHECK(assert_container_equal(expectedOrphans | indirected,
orphans | indirected));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, removeTop3 ) TEST(BayesTree, removeTop3) {
{ auto graph = SymbolicFactorGraph(SymbolicFactor(L(5)))(SymbolicFactor(
SymbolicFactorGraph graph = list_of X(4), L(5)))(SymbolicFactor(X(2), X(4)))(SymbolicFactor(X(3), X(2)));
(SymbolicFactor(L(5))) Ordering ordering{X(3), X(2), X(4), 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)) );
SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering);
// remove all // remove all
SymbolicBayesNet bn; SymbolicBayesNet bn;
SymbolicBayesTree::Cliques orphans; 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 auto expectedBn = SymbolicBayesNet(
(SymbolicConditional::FromKeys(list_of(X(4))(L(5)), 2)) SymbolicConditional::FromKeys<KeyVector>(MakeKeys(X(4))(L(5)), 2))(
(SymbolicConditional(X(2), X(4))) SymbolicConditional(X(2), X(4)))(SymbolicConditional(X(3), X(2)));
(SymbolicConditional(X(3), X(2)));
EXPECT(assert_equal(expectedBn, bn)); EXPECT(assert_equal(expectedBn, bn));
EXPECT(orphans.empty()); EXPECT(orphans.empty());
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, removeTop4 ) TEST(BayesTree, removeTop4) {
{ auto graph = SymbolicFactorGraph(SymbolicFactor(L(5)))(SymbolicFactor(
SymbolicFactorGraph graph = list_of X(4), L(5)))(SymbolicFactor(X(2), X(4)))(SymbolicFactor(X(3), X(2)));
(SymbolicFactor(L(5))) Ordering ordering{X(3), X(2), X(4), 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)) );
SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering);
// remove all // remove all
SymbolicBayesNet bn; SymbolicBayesNet bn;
SymbolicBayesTree::Cliques orphans; 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 auto expectedBn = SymbolicBayesNet(
(SymbolicConditional::FromKeys(list_of(X(4))(L(5)), 2)) SymbolicConditional::FromKeys<KeyVector>(MakeKeys(X(4))(L(5)), 2))(
(SymbolicConditional(X(2), X(4))) SymbolicConditional(X(2), X(4)))(SymbolicConditional(X(3), X(2)));
(SymbolicConditional(X(3), X(2)));
EXPECT(assert_equal(expectedBn, bn)); EXPECT(assert_equal(expectedBn, bn));
EXPECT(orphans.empty()); EXPECT(orphans.empty());
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( BayesTree, removeTop5 ) TEST(BayesTree, removeTop5) {
{
// Remove top called with variables that are not in the Bayes tree // Remove top called with variables that are not in the Bayes tree
SymbolicFactorGraph graph = list_of auto graph = SymbolicFactorGraph(SymbolicFactor(L(5)))(SymbolicFactor(
(SymbolicFactor(L(5))) X(4), L(5)))(SymbolicFactor(X(2), X(4)))(SymbolicFactor(X(3), X(2)));
(SymbolicFactor(X(4), L(5))) Ordering ordering{X(3), X(2), 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)) );
SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering); SymbolicBayesTree bayesTree = *graph.eliminateMultifrontal(ordering);
// Remove nonexistant // Remove nonexistant
SymbolicBayesNet bn; SymbolicBayesNet bn;
SymbolicBayesTree::Cliques orphans; SymbolicBayesTree::Cliques orphans;
bayesTree.removeTop(list_of(X(10)), &bn, &orphans); bayesTree.removeTop(MakeKeys(X(10)), &bn, &orphans);
SymbolicBayesNet expectedBn; SymbolicBayesNet expectedBn;
EXPECT(assert_equal(expectedBn, bn)); 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 // create a thin-tree Bayesnet, a la Jean-Guillaume
SymbolicBayesNet bayesNet; 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.emplace_shared<SymbolicConditional>(13, 14);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(12, 14)); bayesNet.emplace_shared<SymbolicConditional>(12, 14);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(11, 13, 14)); bayesNet.emplace_shared<SymbolicConditional>(11, 13, 14);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(10, 13, 14)); bayesNet.emplace_shared<SymbolicConditional>(10, 13, 14);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(9, 12, 14)); bayesNet.emplace_shared<SymbolicConditional>(9, 12, 14);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(8, 12, 14)); bayesNet.emplace_shared<SymbolicConditional>(8, 12, 14);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(7, 11, 13)); bayesNet.emplace_shared<SymbolicConditional>(7, 11, 13);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(6, 11, 13)); bayesNet.emplace_shared<SymbolicConditional>(6, 11, 13);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(5, 10, 13)); bayesNet.emplace_shared<SymbolicConditional>(5, 10, 13);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(4, 10, 13)); bayesNet.emplace_shared<SymbolicConditional>(4, 10, 13);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(3, 9, 12)); bayesNet.emplace_shared<SymbolicConditional>(3, 9, 12);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(2, 9, 12)); bayesNet.emplace_shared<SymbolicConditional>(2, 9, 12);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(1, 8, 12)); bayesNet.emplace_shared<SymbolicConditional>(1, 8, 12);
bayesNet.push_back(boost::make_shared<SymbolicConditional>(0, 8, 12)); bayesNet.emplace_shared<SymbolicConditional>(0, 8, 12);
if (debug) { if (debug) {
GTSAM_PRINT(bayesNet); GTSAM_PRINT(bayesNet);
@ -430,7 +425,8 @@ TEST( SymbolicBayesTree, thinTree ) {
} }
// create a BayesTree out of a Bayes net // create a BayesTree out of a Bayes net
SymbolicBayesTree bayesTree = *SymbolicFactorGraph(bayesNet).eliminateMultifrontal(); SymbolicBayesTree bayesTree =
*SymbolicFactorGraph(bayesNet).eliminateMultifrontal();
if (debug) { if (debug) {
GTSAM_PRINT(bayesTree); GTSAM_PRINT(bayesTree);
bayesTree.saveGraph("/tmp/SymbolicBayesTree.dot"); bayesTree.saveGraph("/tmp/SymbolicBayesTree.dot");
@ -442,7 +438,7 @@ TEST( SymbolicBayesTree, thinTree ) {
// check shortcut P(S9||R) to root // check shortcut P(S9||R) to root
SymbolicBayesTree::Clique::shared_ptr c = bayesTree[9]; SymbolicBayesTree::Clique::shared_ptr c = bayesTree[9];
SymbolicBayesNet shortcut = c->shortcut(R); 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)); EXPECT(assert_equal(expected, shortcut));
} }
@ -450,9 +446,8 @@ TEST( SymbolicBayesTree, thinTree ) {
// check shortcut P(S8||R) to root // check shortcut P(S8||R) to root
SymbolicBayesTree::Clique::shared_ptr c = bayesTree[8]; SymbolicBayesTree::Clique::shared_ptr c = bayesTree[8];
SymbolicBayesNet shortcut = c->shortcut(R); SymbolicBayesNet shortcut = c->shortcut(R);
SymbolicBayesNet expected = list_of auto expected = SymbolicBayesNet(SymbolicConditional(12, 14))(
(SymbolicConditional(12, 14)) SymbolicConditional(14, 11, 13));
(SymbolicConditional(14, 11, 13));
EXPECT(assert_equal(expected, shortcut)); EXPECT(assert_equal(expected, shortcut));
} }
@ -460,7 +455,7 @@ TEST( SymbolicBayesTree, thinTree ) {
// check shortcut P(S4||R) to root // check shortcut P(S4||R) to root
SymbolicBayesTree::Clique::shared_ptr c = bayesTree[4]; SymbolicBayesTree::Clique::shared_ptr c = bayesTree[4];
SymbolicBayesNet shortcut = c->shortcut(R); 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)); EXPECT(assert_equal(expected, shortcut));
} }
@ -468,8 +463,8 @@ TEST( SymbolicBayesTree, thinTree ) {
// check shortcut P(S2||R) to root // check shortcut P(S2||R) to root
SymbolicBayesTree::Clique::shared_ptr c = bayesTree[2]; SymbolicBayesTree::Clique::shared_ptr c = bayesTree[2];
SymbolicBayesNet shortcut = c->shortcut(R); SymbolicBayesNet shortcut = c->shortcut(R);
SymbolicBayesNet expected = list_of(SymbolicConditional(9, 11, 12, 13)) auto expected = SymbolicBayesNet(SymbolicConditional(9, 11, 12, 13))(
(SymbolicConditional(12, 11, 13)); SymbolicConditional(12, 11, 13));
EXPECT(assert_equal(expected, shortcut)); EXPECT(assert_equal(expected, shortcut));
} }
@ -477,28 +472,28 @@ TEST( SymbolicBayesTree, thinTree ) {
// check shortcut P(S0||R) to root // check shortcut P(S0||R) to root
SymbolicBayesTree::Clique::shared_ptr c = bayesTree[0]; SymbolicBayesTree::Clique::shared_ptr c = bayesTree[0];
SymbolicBayesNet shortcut = c->shortcut(R); SymbolicBayesNet shortcut = c->shortcut(R);
SymbolicBayesNet expected = list_of(SymbolicConditional(8, 11, 12, 13)) auto expected = SymbolicBayesNet(SymbolicConditional(8, 11, 12, 13))(
(SymbolicConditional(12, 11, 13)); SymbolicConditional(12, 11, 13));
EXPECT(assert_equal(expected, shortcut)); EXPECT(assert_equal(expected, shortcut));
} }
SymbolicBayesNet::shared_ptr actualJoint; SymbolicBayesNet::shared_ptr actualJoint;
// Check joint P(8,2) // Check joint P(8,2)
if (false) { // TODO, not disjoint if (false) { // TODO, not disjoint
actualJoint = bayesTree.jointBayesNet(8, 2); actualJoint = bayesTree.jointBayesNet(8, 2);
SymbolicBayesNet expected; SymbolicBayesNet expected;
expected.push_back(boost::make_shared<SymbolicConditional>(8)); expected.emplace_shared<SymbolicConditional>(8);
expected.push_back(boost::make_shared<SymbolicConditional>(2, 8)); expected.emplace_shared<SymbolicConditional>(2, 8);
EXPECT(assert_equal(expected, *actualJoint)); EXPECT(assert_equal(expected, *actualJoint));
} }
// Check joint P(1,2) // Check joint P(1,2)
if (false) { // TODO, not disjoint if (false) { // TODO, not disjoint
actualJoint = bayesTree.jointBayesNet(1, 2); actualJoint = bayesTree.jointBayesNet(1, 2);
SymbolicBayesNet expected; SymbolicBayesNet expected;
expected.push_back(boost::make_shared<SymbolicConditional>(2)); expected.emplace_shared<SymbolicConditional>(2);
expected.push_back(boost::make_shared<SymbolicConditional>(1, 2)); expected.emplace_shared<SymbolicConditional>(1, 2);
EXPECT(assert_equal(expected, *actualJoint)); EXPECT(assert_equal(expected, *actualJoint));
} }
@ -506,35 +501,33 @@ TEST( SymbolicBayesTree, thinTree ) {
if (true) { if (true) {
actualJoint = bayesTree.jointBayesNet(2, 6); actualJoint = bayesTree.jointBayesNet(2, 6);
SymbolicBayesNet expected; SymbolicBayesNet expected;
expected.push_back(boost::make_shared<SymbolicConditional>(2, 6)); expected.emplace_shared<SymbolicConditional>(2, 6);
expected.push_back(boost::make_shared<SymbolicConditional>(6)); expected.emplace_shared<SymbolicConditional>(6);
EXPECT(assert_equal(expected, *actualJoint)); EXPECT(assert_equal(expected, *actualJoint));
} }
// Check joint P(4,6) // Check joint P(4,6)
if (false) { // TODO, not disjoint if (false) { // TODO, not disjoint
actualJoint = bayesTree.jointBayesNet(4, 6); actualJoint = bayesTree.jointBayesNet(4, 6);
SymbolicBayesNet expected; SymbolicBayesNet expected;
expected.push_back(boost::make_shared<SymbolicConditional>(6)); expected.emplace_shared<SymbolicConditional>(6);
expected.push_back(boost::make_shared<SymbolicConditional>(4, 6)); expected.emplace_shared<SymbolicConditional>(4, 6);
EXPECT(assert_equal(expected, *actualJoint)); EXPECT(assert_equal(expected, *actualJoint));
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SymbolicBayesTree, forest_joint) TEST(SymbolicBayesTree, forest_joint) {
{
// Create forest // Create forest
SymbolicBayesTreeClique::shared_ptr root1 = MakeClique(list_of(1), 1); sharedClique root1 = MakeClique(MakeKeys(1), 1);
SymbolicBayesTreeClique::shared_ptr root2 = MakeClique(list_of(2), 1); sharedClique root2 = MakeClique(MakeKeys(2), 1);
SymbolicBayesTree bayesTree; SymbolicBayesTree bayesTree;
bayesTree.insertRoot(root1); bayesTree.insertRoot(root1);
bayesTree.insertRoot(root2); bayesTree.insertRoot(root2);
// Check joint // Check joint
SymbolicBayesNet expected = list_of auto expected =
(SymbolicConditional(1)) SymbolicBayesNet(SymbolicConditional(1))(SymbolicConditional(2));
(SymbolicConditional(2));
SymbolicBayesNet actual = *bayesTree.jointBayesNet(1, 2); SymbolicBayesNet actual = *bayesTree.jointBayesNet(1, 2);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
@ -550,7 +543,7 @@ TEST(SymbolicBayesTree, forest_joint)
C6 0 : 1 C6 0 : 1
**************************************************************************** */ **************************************************************************** */
TEST( SymbolicBayesTree, linear_smoother_shortcuts ) { TEST(SymbolicBayesTree, linear_smoother_shortcuts) {
// Create smoother with 7 nodes // Create smoother with 7 nodes
SymbolicFactorGraph smoother; SymbolicFactorGraph smoother;
smoother.push_factor(0); smoother.push_factor(0);
@ -581,7 +574,8 @@ TEST( SymbolicBayesTree, linear_smoother_shortcuts ) {
{ {
// check shortcut P(S2||R) to root // 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 shortcut = c->shortcut(R);
SymbolicBayesNet expected; SymbolicBayesNet expected;
EXPECT(assert_equal(expected, shortcut)); EXPECT(assert_equal(expected, shortcut));
@ -589,45 +583,46 @@ TEST( SymbolicBayesTree, linear_smoother_shortcuts ) {
{ {
// check shortcut P(S3||R) to root // 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 shortcut = c->shortcut(R);
SymbolicBayesNet expected = list_of(SymbolicConditional(4, 5)); auto expected = SymbolicBayesNet(SymbolicConditional(4, 5));
EXPECT(assert_equal(expected, shortcut)); EXPECT(assert_equal(expected, shortcut));
} }
{ {
// check shortcut P(S4||R) to root // 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 shortcut = c->shortcut(R);
SymbolicBayesNet expected = list_of(SymbolicConditional(3, 5)); auto expected = SymbolicBayesNet(SymbolicConditional(3, 5));
EXPECT(assert_equal(expected, shortcut)); EXPECT(assert_equal(expected, shortcut));
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
// from testSymbolicJunctionTree, which failed at one point // from testSymbolicJunctionTree, which failed at one point
TEST(SymbolicBayesTree, complicatedMarginal) TEST(SymbolicBayesTree, complicatedMarginal) {
{
// Create the conditionals to go in the BayesTree // Create the conditionals to go in the BayesTree
SymbolicBayesTreeClique::shared_ptr cur; sharedClique cur;
SymbolicBayesTreeClique::shared_ptr root = MakeClique(list_of(11)(12), 2); sharedClique root = MakeClique(MakeKeys(11)(12), 2);
cur = root; 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.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; root->children.back()->parent_ = root;
cur = root->children.back(); 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->children.back()->parent_ = cur;
cur = cur->children.back(); 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.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; cur->children.back()->parent_ = cur;
// Create Bayes Tree // Create Bayes Tree
@ -656,9 +651,8 @@ TEST(SymbolicBayesTree, complicatedMarginal)
{ {
SymbolicBayesTree::Clique::shared_ptr c = bt[5]; SymbolicBayesTree::Clique::shared_ptr c = bt[5];
SymbolicBayesNet shortcut = c->shortcut(root); SymbolicBayesNet shortcut = c->shortcut(root);
SymbolicBayesNet expected = list_of auto expected = SymbolicBayesNet(SymbolicConditional(7, 8, 11))(
(SymbolicConditional(7, 8, 11)) SymbolicConditional(8, 11));
(SymbolicConditional(8, 11));
EXPECT(assert_equal(expected, shortcut)); EXPECT(assert_equal(expected, shortcut));
} }
@ -666,7 +660,7 @@ TEST(SymbolicBayesTree, complicatedMarginal)
{ {
SymbolicBayesTree::Clique::shared_ptr c = bt[3]; SymbolicBayesTree::Clique::shared_ptr c = bt[3];
SymbolicBayesNet shortcut = c->shortcut(root); SymbolicBayesNet shortcut = c->shortcut(root);
SymbolicBayesNet expected = list_of(SymbolicConditional(6, 11)); auto expected = SymbolicBayesNet(SymbolicConditional(6, 11));
EXPECT(assert_equal(expected, shortcut)); EXPECT(assert_equal(expected, shortcut));
} }
@ -674,7 +668,7 @@ TEST(SymbolicBayesTree, complicatedMarginal)
{ {
SymbolicBayesTree::Clique::shared_ptr c = bt[1]; SymbolicBayesTree::Clique::shared_ptr c = bt[1];
SymbolicBayesNet shortcut = c->shortcut(root); SymbolicBayesNet shortcut = c->shortcut(root);
SymbolicBayesNet expected = list_of(SymbolicConditional(5, 11)); auto expected = SymbolicBayesNet(SymbolicConditional(5, 11));
EXPECT(assert_equal(expected, shortcut)); EXPECT(assert_equal(expected, shortcut));
} }
@ -689,12 +683,10 @@ TEST(SymbolicBayesTree, complicatedMarginal)
SymbolicFactor::shared_ptr actual = bt.marginalFactor(6); SymbolicFactor::shared_ptr actual = bt.marginalFactor(6);
EXPECT(assert_equal(SymbolicFactor(6), *actual, 1e-1)); EXPECT(assert_equal(SymbolicFactor(6), *actual, 1e-1));
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SymbolicBayesTree, COLAMDvsMETIS) { TEST(SymbolicBayesTree, COLAMDvsMETIS) {
// create circular graph // create circular graph
SymbolicFactorGraph sfg; SymbolicFactorGraph sfg;
sfg.push_factor(0, 1); sfg.push_factor(0, 1);
@ -707,7 +699,7 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) {
// COLAMD // COLAMD
{ {
Ordering ordering = Ordering::Create(Ordering::COLAMD, sfg); 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( 4 2 3)
// | - P( 1 | 2 4) // | - P( 1 | 2 4)
@ -715,12 +707,12 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) {
// | | | - P( 0 | 1 5) // | | | - P( 0 | 1 5)
SymbolicBayesTree expected; SymbolicBayesTree expected;
expected.insertRoot( expected.insertRoot(
MakeClique(list_of(4)(2)(3), 3, MakeClique(MakeKeys(4)(2)(3), 3,
list_of( MakeCliques(MakeClique(
MakeClique(list_of(1)(2)(4), 1, MakeKeys(1)(2)(4), 1,
list_of( MakeCliques(MakeClique(
MakeClique(list_of(5)(1)(4), 1, MakeKeys(5)(1)(4), 1,
list_of(MakeClique(list_of(0)(1)(5), 1)))))))); MakeCliques(MakeClique(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));
@ -732,11 +724,11 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) {
Ordering ordering = Ordering::Create(Ordering::METIS, sfg); Ordering ordering = Ordering::Create(Ordering::METIS, sfg);
// Linux and Mac split differently when using mettis // Linux and Mac split differently when using mettis
#if defined(__APPLE__) #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) #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 #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 #endif
// - P( 1 0 3) // - P( 1 0 3)
@ -745,26 +737,23 @@ TEST(SymbolicBayesTree, COLAMDvsMETIS) {
// | - P( 2 | 1 3) // | - P( 2 | 1 3)
SymbolicBayesTree expected; SymbolicBayesTree expected;
#if defined(__APPLE__) #if defined(__APPLE__)
expected.insertRoot( expected.insertRoot(MakeClique(
MakeClique(list_of(1)(0)(3), 3, MakeKeys(1)(0)(3), 3,
list_of( MakeCliques(MakeClique(MakeKeys(4)(0)(3), 1,
MakeClique(list_of(4)(0)(3), 1, MakeCliques(MakeClique(MakeKeys(5)(0)(4), 1))))(
list_of(MakeClique(list_of(5)(0)(4), 1))))( MakeClique(MakeKeys(2)(1)(3), 1))));
MakeClique(list_of(2)(1)(3), 1))));
#elif defined(_WIN32) #elif defined(_WIN32)
expected.insertRoot( expected.insertRoot(MakeClique(
MakeClique(list_of(3)(5)(2), 3, MakeKeys(3)(5)(2), 3,
list_of( MakeCliques(MakeClique(MakeKeys(4)(3)(5), 1,
MakeClique(list_of(4)(3)(5), 1, MakeCliques(MakeClique(MakeKeys(0)(2)(5), 1))))(
list_of(MakeClique(list_of(0)(2)(5), 1))))( MakeClique(MakeKeys(1)(0)(2), 1))));
MakeClique(list_of(1)(0)(2), 1))));
#else #else
expected.insertRoot( expected.insertRoot(MakeClique(
MakeClique(list_of(2)(4)(1), 3, MakeKeys(2)(4)(1), 3,
list_of( MakeCliques(MakeClique(MakeKeys(0)(1)(4), 1,
MakeClique(list_of(0)(1)(4), 1, MakeCliques(MakeClique(MakeKeys(5)(0)(4), 1))))(
list_of(MakeClique(list_of(5)(0)(4), 1))))( MakeClique(MakeKeys(3)(2)(4), 1))));
MakeClique(list_of(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));
@ -778,4 +767,3 @@ int main() {
return TestRegistry::runAllTests(tr); return TestRegistry::runAllTests(tr);
} }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -17,47 +17,45 @@
*/ */
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <vector>
#include <boost/make_shared.hpp>
#include <boost/assign/list_of.hpp>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/symbolic/SymbolicEliminationTree.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/symbolic/SymbolicEliminationTree.h>
#include <boost/make_shared.hpp>
#include <vector>
#include "symbolicExampleGraphs.h" #include "symbolicExampleGraphs.h"
using namespace gtsam; using namespace gtsam;
using namespace gtsam::symbol_shorthand; using namespace gtsam::symbol_shorthand;
using namespace std; using namespace std;
using boost::assign::list_of; using sharedNode = SymbolicEliminationTree::sharedNode;
class EliminationTreeTester { class EliminationTreeTester {
public: public:
// build hardcoded tree // build hardcoded tree
static SymbolicEliminationTree buildHardcodedTree(const SymbolicFactorGraph& fg) { static SymbolicEliminationTree buildHardcodedTree(
const SymbolicFactorGraph& fg) {
SymbolicEliminationTree::sharedNode leaf0(new SymbolicEliminationTree::Node); sharedNode leaf0(new SymbolicEliminationTree::Node);
leaf0->key = 0; leaf0->key = 0;
leaf0->factors.push_back(fg[0]); leaf0->factors.push_back(fg[0]);
leaf0->factors.push_back(fg[1]); leaf0->factors.push_back(fg[1]);
SymbolicEliminationTree::sharedNode node1(new SymbolicEliminationTree::Node); sharedNode node1(new SymbolicEliminationTree::Node);
node1->key = 1; node1->key = 1;
node1->factors.push_back(fg[2]); node1->factors.push_back(fg[2]);
node1->children.push_back(leaf0); node1->children.push_back(leaf0);
SymbolicEliminationTree::sharedNode node2(new SymbolicEliminationTree::Node); sharedNode node2(new SymbolicEliminationTree::Node);
node2->key = 2; node2->key = 2;
node2->factors.push_back(fg[3]); node2->factors.push_back(fg[3]);
node2->children.push_back(node1); node2->children.push_back(node1);
SymbolicEliminationTree::sharedNode leaf3(new SymbolicEliminationTree::Node); sharedNode leaf3(new SymbolicEliminationTree::Node);
leaf3->key = 3; leaf3->key = 3;
leaf3->factors.push_back(fg[4]); leaf3->factors.push_back(fg[4]);
SymbolicEliminationTree::sharedNode root(new SymbolicEliminationTree::Node); sharedNode root(new SymbolicEliminationTree::Node);
root->key = 4; root->key = 4;
root->children.push_back(leaf3); root->children.push_back(leaf3);
root->children.push_back(node2); root->children.push_back(node2);
@ -67,29 +65,27 @@ public:
return tree; return tree;
} }
template<typename ROOTS> static SymbolicEliminationTree MakeTree(
static SymbolicEliminationTree MakeTree(const ROOTS& roots) const std::vector<sharedNode>& roots) {
{
SymbolicEliminationTree et; SymbolicEliminationTree et;
et.roots_.assign(roots.begin(), roots.end()); et.roots_.assign(roots.begin(), roots.end());
return et; return et;
} }
}; };
template<typename FACTORS> template <typename FACTORS>
static SymbolicEliminationTree::sharedNode MakeNode(Key key, const FACTORS& factors) static sharedNode MakeNode(Key key, const FACTORS& factors) {
{ sharedNode node = boost::make_shared<SymbolicEliminationTree::Node>();
SymbolicEliminationTree::sharedNode node = boost::make_shared<SymbolicEliminationTree::Node>();
node->key = key; node->key = key;
SymbolicFactorGraph factorsAsGraph = factors; SymbolicFactorGraph factorsAsGraph = factors;
node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end()); node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end());
return node; return node;
} }
template<typename FACTORS, typename CHILDREN> template <typename FACTORS>
static SymbolicEliminationTree::sharedNode MakeNode(Key key, const FACTORS& factors, const CHILDREN& children) static sharedNode MakeNode(Key key, const FACTORS& factors,
{ const std::vector<sharedNode>& children) {
SymbolicEliminationTree::sharedNode node = boost::make_shared<SymbolicEliminationTree::Node>(); sharedNode node = boost::make_shared<SymbolicEliminationTree::Node>();
node->key = key; node->key = key;
SymbolicFactorGraph factorsAsGraph = factors; SymbolicFactorGraph factorsAsGraph = factors;
node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end()); node->factors.assign(factorsAsGraph.begin(), factorsAsGraph.end());
@ -97,24 +93,39 @@ static SymbolicEliminationTree::sharedNode MakeNode(Key key, const FACTORS& fact
return node; 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 = SymbolicEliminationTree expected =
EliminationTreeTester::buildHardcodedTree(simpleTestGraph1); EliminationTreeTester::buildHardcodedTree(simpleTestGraph1);
// Build from factor graph // Build from factor graph
Ordering order; Ordering order;
order += 0,1,2,3,4; order += 0, 1, 2, 3, 4;
SymbolicEliminationTree actual(simpleTestGraph1, order); SymbolicEliminationTree actual(simpleTestGraph1, order);
CHECK(assert_equal(expected, actual)); CHECK(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(EliminationTree, Create2) TEST(EliminationTree, Create2) {
{
// l1 l2 // l1 l2
// / | / | // / | / |
// x1 --- x2 --- x3 --- x4 --- x5 // x1 --- x2 --- x3 --- x4 --- x5
@ -132,20 +143,31 @@ 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 = EliminationTreeTester::MakeTree(list_of SymbolicEliminationTree expected =
(MakeNode(X(3), SymbolicFactorGraph(), list_of EliminationTreeTester::MakeTree(Nodes(MakeNode(
(MakeNode(X(2), list_of(SymbolicFactor(X(2), X(3))), list_of X(3), SymbolicFactorGraph(),
(MakeNode(L(1), list_of(SymbolicFactor(X(2), L(1))), list_of Nodes(MakeNode(
(MakeNode(X(1), list_of(SymbolicFactor(X(1), L(1))) (SymbolicFactor(X(1), X(2))))))))) X(2), SymbolicFactorGraph(SymbolicFactor(X(2), X(3))),
(MakeNode(X(4), list_of(SymbolicFactor(X(3), X(4))), list_of Nodes(MakeNode(
(MakeNode(L(2), list_of(SymbolicFactor(X(4), L(2))), list_of L(1), SymbolicFactorGraph(SymbolicFactor(X(2), L(1))),
(MakeNode(X(5), list_of(SymbolicFactor(X(4), X(5))) (SymbolicFactor(L(2), X(5))), list_of Nodes(MakeNode(
(MakeNode(L(3), list_of(SymbolicFactor(X(4), L(3))) (SymbolicFactor(X(5), L(3)))))))))))))); X(1), SymbolicFactorGraph(SymbolicFactor(X(1), L(1)))(
SymbolicFactor(X(1), X(2)))))))))(
Ordering order = list_of(X(1)) (L(3)) (L(1)) (X(5)) (X(2)) (L(2)) (X(4)) (X(3)); 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); SymbolicEliminationTree actual(graph, order);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
} }

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

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

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

@ -15,33 +15,29 @@
* @author Christian Potthast * @author Christian Potthast
**/ **/
#include <gtsam/symbolic/SymbolicFactorGraph.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/symbolic/SymbolicBayesNet.h> #include <gtsam/symbolic/SymbolicBayesNet.h>
#include <gtsam/symbolic/SymbolicBayesTree.h> #include <gtsam/symbolic/SymbolicBayesTree.h>
#include <gtsam/symbolic/SymbolicConditional.h> #include <gtsam/symbolic/SymbolicConditional.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <gtsam/symbolic/tests/symbolicExampleGraphs.h> #include <gtsam/symbolic/tests/symbolicExampleGraphs.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/assign/list_of.hpp>
using namespace boost::assign;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace boost::assign;
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SymbolicFactorGraph, keys1) { TEST(SymbolicFactorGraph, keys1) {
KeySet expected {0, 1, 2, 3, 4}; KeySet expected{0, 1, 2, 3, 4};
KeySet actual = simpleTestGraph1.keys(); KeySet actual = simpleTestGraph1.keys();
EXPECT(expected == actual); EXPECT(expected == actual);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SymbolicFactorGraph, keys2) { TEST(SymbolicFactorGraph, keys2) {
KeySet expected {0, 1, 2, 3, 4, 5}; KeySet expected{0, 1, 2, 3, 4, 5};
KeySet actual = simpleTestGraph2.keys(); KeySet actual = simpleTestGraph2.keys();
EXPECT(expected == actual); EXPECT(expected == actual);
} }
@ -61,12 +57,12 @@ TEST(SymbolicFactorGraph, eliminateFullSequential) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SymbolicFactorGraph, eliminatePartialSequential) { TEST(SymbolicFactorGraph, eliminatePartialSequential) {
// Eliminate 0 and 1 // Eliminate 0 and 1
const Ordering order {0, 1}; const Ordering order{0, 1};
const SymbolicBayesNet expectedBayesNet = const auto expectedBayesNet = SymbolicBayesNet(SymbolicConditional(0, 1, 2))(
list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3, 4)); 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)); SymbolicFactor(4, 5))(SymbolicFactor(2, 3, 4));
SymbolicBayesNet::shared_ptr actualBayesNet; SymbolicBayesNet::shared_ptr actualBayesNet;
@ -106,9 +102,9 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) {
expectedBayesTree.insertRoot( expectedBayesTree.insertRoot(
boost::make_shared<SymbolicBayesTreeClique>(root)); boost::make_shared<SymbolicBayesTreeClique>(root));
SymbolicFactorGraph expectedFactorGraph = const auto expectedFactorGraph =
list_of(SymbolicFactor(0, 1))(SymbolicFactor(0, 2))(SymbolicFactor(1, 3))( SymbolicFactorGraph(SymbolicFactor(0, 1))(SymbolicFactor(0, 2))(
SymbolicFactor(2, 3))(SymbolicFactor(1)); SymbolicFactor(1, 3))(SymbolicFactor(2, 3))(SymbolicFactor(1));
SymbolicBayesTree::shared_ptr actualBayesTree; SymbolicBayesTree::shared_ptr actualBayesTree;
SymbolicFactorGraph::shared_ptr actualFactorGraph; SymbolicFactorGraph::shared_ptr actualFactorGraph;
@ -137,12 +133,12 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) { TEST(SymbolicFactorGraph, marginalMultifrontalBayesNet) {
SymbolicBayesNet expectedBayesNet = auto expectedBayesNet =
list_of(SymbolicConditional(0, 1, 2))(SymbolicConditional(1, 2, 3))( SymbolicBayesNet(SymbolicConditional(0, 1, 2))(SymbolicConditional(
SymbolicConditional(2, 3))(SymbolicConditional(3)); 1, 2, 3))(SymbolicConditional(2, 3))(SymbolicConditional(3));
SymbolicBayesNet actual1 = *simpleTestGraph2.marginalMultifrontalBayesNet( SymbolicBayesNet actual1 =
Ordering{0, 1, 2, 3}); *simpleTestGraph2.marginalMultifrontalBayesNet(Ordering{0, 1, 2, 3});
EXPECT(assert_equal(expectedBayesNet, actual1)); EXPECT(assert_equal(expectedBayesNet, actual1));
} }
@ -192,7 +188,7 @@ TEST(SymbolicFactorGraph, marginals) {
{ {
// jointBayesNet // jointBayesNet
Ordering ord {0, 4, 3}; Ordering ord{0, 4, 3};
auto actual = fg.eliminatePartialSequential(ord); auto actual = fg.eliminatePartialSequential(ord);
SymbolicBayesNet expectedBN; SymbolicBayesNet expectedBN;
expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2); expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
@ -203,7 +199,7 @@ TEST(SymbolicFactorGraph, marginals) {
{ {
// jointBayesNet // jointBayesNet
Ordering ord {0, 2, 3}; Ordering ord{0, 2, 3};
auto actual = fg.eliminatePartialSequential(ord); auto actual = fg.eliminatePartialSequential(ord);
SymbolicBayesNet expectedBN; SymbolicBayesNet expectedBN;
expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2); expectedBN.emplace_shared<SymbolicConditional>(0, 1, 2);
@ -302,7 +298,7 @@ TEST(SymbolicFactorGraph, add_factors) {
expected.push_factor(1); expected.push_factor(1);
expected.push_factor(11); expected.push_factor(11);
expected.push_factor(2); expected.push_factor(2);
const FactorIndices expectedIndices {1, 3}; const FactorIndices expectedIndices{1, 3};
const FactorIndices actualIndices = fg1.add_factors(fg2, true); const FactorIndices actualIndices = fg1.add_factors(fg2, true);
EXPECT(assert_equal(expected, fg1)); EXPECT(assert_equal(expected, fg1));
@ -310,7 +306,7 @@ TEST(SymbolicFactorGraph, add_factors) {
expected.push_factor(1); expected.push_factor(1);
expected.push_factor(2); expected.push_factor(2);
const FactorIndices expectedIndices2 {4, 5}; const FactorIndices expectedIndices2{4, 5};
const FactorIndices actualIndices2 = fg1.add_factors(fg2, false); const FactorIndices actualIndices2 = fg1.add_factors(fg2, false);
EXPECT(assert_equal(expected, fg1)); EXPECT(assert_equal(expected, fg1));