diff --git a/gtsam/symbolic/tests/symbolicExampleGraphs.h b/gtsam/symbolic/tests/symbolicExampleGraphs.h index 2b5cf3434..b97e54096 100644 --- a/gtsam/symbolic/tests/symbolicExampleGraphs.h +++ b/gtsam/symbolic/tests/symbolicExampleGraphs.h @@ -49,17 +49,26 @@ namespace gtsam { (boost::make_shared(2,3)) (boost::make_shared(4,5)); - /** 0 - 1 - 2 - 3 */ + /** 1 - 0 - 2 - 3 */ const SymbolicFactorGraphUnordered simpleChain = boost::assign::list_of - (boost::make_shared(0,1)) - (boost::make_shared(1,2)) + (boost::make_shared(1,0)) + (boost::make_shared(0,2)) (boost::make_shared(2,3)); + /* ************************************************************************* * + * 2 3 + * 0 1 : 2 + ****************************************************************************/ SymbolicBayesTreeUnordered __simpleChainBayesTree() { SymbolicBayesTreeUnordered result; - SymbolicBayesTreeCliqueUnordered::shared_ptr root = - boost::make_shared( - boost::make_shared(SymbolicConditionalUnordered::FromKeys(list_of(2)(3),2))); + result.insertRoot(boost::make_shared( + boost::make_shared( + SymbolicConditionalUnordered::FromKeys(boost::assign::list_of(3)(2), 2)))); + result.addClique(boost::make_shared( + boost::make_shared( + SymbolicConditionalUnordered::FromKeys(boost::assign::list_of(1)(0)(2), 2))), + result.roots().front()); + return result; } const SymbolicBayesTreeUnordered simpleChainBayesTree = __simpleChainBayesTree(); @@ -88,6 +97,24 @@ namespace gtsam { (boost::make_shared(_L_, _B_)) (boost::make_shared(_B_)); + SymbolicBayesTreeUnordered __asiaBayesTree() { + SymbolicBayesTreeUnordered result; + result.insertRoot(boost::make_shared( + boost::make_shared( + SymbolicConditionalUnordered::FromKeys(boost::assign::list_of(_B_)(_L_)(_E_)(_S_), 4)))); + result.addClique(boost::make_shared( + boost::make_shared( + SymbolicConditionalUnordered::FromKeys(boost::assign::list_of(_T_)(_E_)(_L_), 1))), + result.roots().front()); + result.addClique(boost::make_shared( + boost::make_shared( + SymbolicConditionalUnordered::FromKeys(boost::assign::list_of(_X_)(_E_), 1))), + result.roots().front()); + return result; + } + + const SymbolicBayesTreeUnordered asiaBayesTree = __asiaBayesTree(); + /* ************************************************************************* */ const OrderingUnordered asiaOrdering = boost::assign::list_of(_X_)(_T_)(_S_)(_E_)(_L_)(_B_); diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index 9dbf809f8..666806cc4 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -48,28 +48,13 @@ static SymbolicConditionalUnordered::shared_ptr ELB( boost::make_shared( SymbolicConditionalUnordered::FromKeys(list_of(_E_)(_L_)(_B_), 3))); -// Bayes Tree for Asia example -static SymbolicBayesTreeUnordered createAsiaSymbolicBayesTree() { - SymbolicFactorGraphUnordered asiaGraph; - asiaGraph.push_factor(_T_); - asiaGraph.push_factor(_S_); - asiaGraph.push_factor(_T_, _E_, _L_); - asiaGraph.push_factor(_L_, _S_); - asiaGraph.push_factor(_S_, _B_); - asiaGraph.push_factor(_E_, _B_); - asiaGraph.push_factor(_E_, _X_); - OrderingUnordered asiaOrdering; asiaOrdering += _X_, _T_, _S_, _E_, _L_, _B_; - SymbolicBayesTreeUnordered bayesTree = *asiaGraph.eliminateMultifrontal(EliminateSymbolicUnordered, asiaOrdering); - return bayesTree; -} - /* ************************************************************************* */ TEST_UNSAFE( BayesTree, constructor ) { // Create using insert - SymbolicBayesTreeUnordered bayesTree = createAsiaSymbolicBayesTree(); + //SymbolicBayesTreeUnordered bayesTree = createAsiaSymbolicBayesTree(); - bayesTree.print("bayesTree: "); + //bayesTree.print("bayesTree: "); // Check Size // LONGS_EQUAL(4, bayesTree.size()); diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index f35e99907..11b92f138 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -28,6 +28,8 @@ using namespace boost::assign; #include #include +#include "symbolicExampleGraphs.h" + using namespace gtsam; using namespace gtsam::symbol_shorthand; using namespace std; @@ -71,20 +73,13 @@ public: /* ************************************************************************* */ TEST(EliminationTree, Create) { - // create example factor graph - SymbolicFactorGraphUnordered fg; - fg.push_factor(0, 1); - fg.push_factor(0, 2); - fg.push_factor(1, 4); - fg.push_factor(2, 4); - fg.push_factor(3, 4); - - SymbolicEliminationTreeUnordered expected = EliminationTreeUnorderedTester::buildHardcodedTree(fg); + SymbolicEliminationTreeUnordered expected = + EliminationTreeUnorderedTester::buildHardcodedTree(simpleTestGraph1); // Build from factor graph OrderingUnordered order; order += 0,1,2,3,4; - SymbolicEliminationTreeUnordered actual(fg, order); + SymbolicEliminationTreeUnordered actual(simpleTestGraph1, order); CHECK(assert_equal(expected, actual)); } diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index 2a2f436a2..f41fa706d 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -68,14 +68,14 @@ TEST(SymbolicFactorGraph, eliminatePartialSequential) /* ************************************************************************* */ TEST(SymbolicFactorGraph, eliminateFullMultifrontal) { - const Index x2=0, x1=1, x3=2, x4=3; - SymbolicFactorGraphUnordered fg; - fg.push_factor(x2,x1); - fg.push_factor(x2,x3); - fg.push_factor(x3,x4); - - EXPECT(false); + OrderingUnordered ordering; ordering += 0,1,2,3; + SymbolicBayesTreeUnordered actual1 = + *simpleChain.eliminateMultifrontal(EliminateSymbolicUnordered, ordering); + EXPECT(assert_equal(simpleChainBayesTree, actual1)); + SymbolicBayesTreeUnordered actual2 = + *asiaGraph.eliminateMultifrontal(EliminateSymbolicUnordered, asiaOrdering); + EXPECT(assert_equal(asiaBayesTree, actual2)); } /* ************************************************************************* */ @@ -189,6 +189,21 @@ TEST( SymbolicFactorGraph, constructFromBayesNet ) CHECK(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST( SymbolicFactorGraph, constructFromBayesTree ) +{ + // create expected factor graph + SymbolicFactorGraphUnordered expected; + expected.push_factor(_B_, _L_, _E_, _S_); + expected.push_factor(_T_, _E_, _L_); + expected.push_factor(_X_, _E_); + + // create actual factor graph + SymbolicFactorGraphUnordered actual(asiaBayesTree); + + CHECK(assert_equal(expected, actual)); +} + /* ************************************************************************* */ TEST( SymbolicFactorGraph, push_back ) { diff --git a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp index f166d35ca..4babbecdc 100644 --- a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp @@ -24,55 +24,39 @@ using namespace boost::assign; #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include + +#include "symbolicExampleGraphs.h" using namespace gtsam; using namespace std; -typedef JunctionTree SymbolicJunctionTree; - /* ************************************************************************* * - * x1 - x2 - x3 - x4 - * x3 x4 - * x2 x1 : x3 + * 1 - 0 - 2 - 3 + * 2 3 + * 0 1 : 2 ****************************************************************************/ TEST( JunctionTree, constructor ) { - const Index x2=0, x1=1, x3=2, x4=3; - SymbolicFactorGraph fg; - fg.push_factor(x2,x1); - fg.push_factor(x2,x3); - fg.push_factor(x3,x4); + OrderingUnordered order; order += 0, 1, 2, 3; - SymbolicJunctionTree actual(fg); + SymbolicJunctionTreeUnordered actual(SymbolicEliminationTreeUnordered(simpleChain, order)); - vector frontal1; frontal1 += x3, x4; - vector frontal2; frontal2 += x2, x1; + vector frontal1; frontal1 += 3, 2; + vector frontal2; frontal2 += 1, 0; vector sep1; - vector sep2; sep2 += x3; - CHECK(assert_equal(frontal1, actual.root()->frontal)); - CHECK(assert_equal(sep1, actual.root()->separator)); - LONGS_EQUAL(1, actual.root()->size()); - CHECK(assert_equal(frontal2, actual.root()->children().front()->frontal)); - CHECK(assert_equal(sep2, actual.root()->children().front()->separator)); - LONGS_EQUAL(2, actual.root()->children().front()->size()); - CHECK(assert_equal(*fg[2], *(*actual.root())[0])); - CHECK(assert_equal(*fg[0], *(*actual.root()->children().front())[0])); - CHECK(assert_equal(*fg[1], *(*actual.root()->children().front())[1])); -} - -/* ************************************************************************* * - * x1 - x2 - x3 - x4 - * x3 x4 - * x2 x1 : x3 - ****************************************************************************/ -TEST( JunctionTree, eliminate) -{ + vector sep2; sep2 += 2; + EXPECT(assert_equal(frontal1, actual.roots().front()->keys)); + //EXPECT(assert_equal(sep1, actual.roots().front()->separator)); + LONGS_EQUAL(1, actual.roots().front()->factors.size()); + EXPECT(assert_equal(frontal2, actual.roots().front()->children.front()->keys)); + //EXPECT(assert_equal(sep2, actual.roots().front()->children.front()->separator)); + LONGS_EQUAL(2, actual.roots().front()->children.front()->factors.size()); + EXPECT(assert_equal(*simpleChain[2], *actual.roots().front()->factors[0])); + EXPECT(assert_equal(*simpleChain[0], *actual.roots().front()->children.front()->factors[0])); + EXPECT(assert_equal(*simpleChain[1], *actual.roots().front()->children.front()->factors[1])); } /* ************************************************************************* */