diff --git a/gtsam/symbolic/tests/symbolicExampleGraphs.h b/gtsam/symbolic/tests/symbolicExampleGraphs.h new file mode 100644 index 000000000..2b5cf3434 --- /dev/null +++ b/gtsam/symbolic/tests/symbolicExampleGraphs.h @@ -0,0 +1,95 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file symbolicExampleGraphs.cpp + * @date sept 15, 2012 + * @author Frank Dellaert + * @author Michael Kaess + * @author Viorela Ila + * @author Richard Roberts + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + namespace { + + const SymbolicFactorGraphUnordered simpleTestGraph1 = boost::assign::list_of + (boost::make_shared(0,1)) + (boost::make_shared(0,2)) + (boost::make_shared(1,4)) + (boost::make_shared(2,4)) + (boost::make_shared(3,4)); + + const SymbolicBayesNetUnordered simpleTestGraph1BayesNet = boost::assign::list_of + (boost::make_shared(0,1,2)) + (boost::make_shared(1,2,4)) + (boost::make_shared(2,4)) + (boost::make_shared(3,4)) + (boost::make_shared(4)); + + const SymbolicFactorGraphUnordered simpleTestGraph2 = boost::assign::list_of + (boost::make_shared(0,1)) + (boost::make_shared(0,2)) + (boost::make_shared(1,3)) + (boost::make_shared(1,4)) + (boost::make_shared(2,3)) + (boost::make_shared(4,5)); + + /** 0 - 1 - 2 - 3 */ + const SymbolicFactorGraphUnordered simpleChain = boost::assign::list_of + (boost::make_shared(0,1)) + (boost::make_shared(1,2)) + (boost::make_shared(2,3)); + + SymbolicBayesTreeUnordered __simpleChainBayesTree() { + SymbolicBayesTreeUnordered result; + SymbolicBayesTreeCliqueUnordered::shared_ptr root = + boost::make_shared( + boost::make_shared(SymbolicConditionalUnordered::FromKeys(list_of(2)(3),2))); + } + + const SymbolicBayesTreeUnordered simpleChainBayesTree = __simpleChainBayesTree(); + + /* ************************************************************************* */ + // Keys for ASIA example from the tutorial with A and D evidence + const Key _X_=gtsam::symbol_shorthand::X(0), _T_=gtsam::symbol_shorthand::T(0), + _S_=gtsam::symbol_shorthand::S(0), _E_=gtsam::symbol_shorthand::E(0), + _L_=gtsam::symbol_shorthand::L(0), _B_=gtsam::symbol_shorthand::B(0); + + // Factor graph for Asia example + const SymbolicFactorGraphUnordered asiaGraph = boost::assign::list_of + (boost::make_shared(_T_)) + (boost::make_shared(_S_)) + (boost::make_shared(_T_, _E_, _L_)) + (boost::make_shared(_L_, _S_)) + (boost::make_shared(_S_, _B_)) + (boost::make_shared(_E_, _B_)) + (boost::make_shared(_E_, _X_)); + + const SymbolicBayesNetUnordered asiaBayesNet = boost::assign::list_of + (boost::make_shared(_T_, _E_, _L_)) + (boost::make_shared(_X_, _E_)) + (boost::make_shared(_E_, _B_, _L_)) + (boost::make_shared(_S_, _B_, _L_)) + (boost::make_shared(_L_, _B_)) + (boost::make_shared(_B_)); + + /* ************************************************************************* */ + 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 ef6766e11..9dbf809f8 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -34,7 +35,6 @@ static bool debug = false; /* ************************************************************************* */ // Conditionals for ASIA example from the tutorial with A and D evidence -static const Key _X_=X(0), _T_=T(0), _S_=S(0), _E_=E(0), _L_=L(0), _B_=B(0); static SymbolicConditionalUnordered::shared_ptr B(new SymbolicConditionalUnordered(_B_)), L(new SymbolicConditionalUnordered(_L_, _B_)), diff --git a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp index 4e3e0e117..f35e99907 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -67,27 +67,6 @@ public: } }; -/* ************************************************************************* */ -namespace { - - /* ************************************************************************* */ - // Keys for ASIA example from the tutorial with A and D evidence - const Key _X_=X(0), _T_=T(0), _S_=S(0), _E_=E(0), _L_=L(0), _B_=B(0); - - // Factor graph for Asia example - const SymbolicFactorGraphUnordered asiaGraph = list_of - (boost::make_shared(_T_)) - (boost::make_shared(_S_)) - (boost::make_shared(_T_, _E_, _L_)) - (boost::make_shared(_L_, _S_)) - (boost::make_shared(_S_, _B_)) - (boost::make_shared(_E_, _B_)) - (boost::make_shared(_E_, _X_)); - - /* ************************************************************************* */ - const OrderingUnordered asiaOrdering = list_of(_X_)(_T_)(_S_)(_E_)(_L_)(_B_); - -} /* ************************************************************************* */ TEST(EliminationTree, Create) @@ -110,78 +89,6 @@ TEST(EliminationTree, Create) CHECK(assert_equal(expected, actual)); } -/* ************************************************************************* */ -// Test to drive elimination tree development -// graph: f(0,1) f(0,2) f(1,4) f(2,4) f(3,4) -/* ************************************************************************* */ - -TEST_UNSAFE(EliminationTree, eliminate ) -{ - // create expected Chordal bayes Net - SymbolicBayesNetUnordered expected; - expected.push_back(boost::make_shared(0,1,2)); - expected.push_back(boost::make_shared(1,2,4)); - expected.push_back(boost::make_shared(2,4)); - expected.push_back(boost::make_shared(3,4)); - expected.push_back(boost::make_shared(4)); - - // Create 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); - - // eliminate - OrderingUnordered order; - order += 0,1,2,3,4; - SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg,order).eliminate(EliminateSymbolicUnordered).first; - - EXPECT(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST(EliminationTree, eliminateAsiaExample) -{ - SymbolicBayesNetUnordered expected = list_of - (boost::make_shared(_T_, _E_, _L_)) - (boost::make_shared(_X_, _E_)) - (boost::make_shared(_E_, _B_, _L_)) - (boost::make_shared(_S_, _B_, _L_)) - (boost::make_shared(_L_, _B_)) - (boost::make_shared(_B_)); - - SymbolicBayesNetUnordered actual = *asiaGraph.eliminateSequential( - EliminateSymbolicUnordered, asiaOrdering); - - EXPECT(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST(EliminationTree, disconnected_graph) { - SymbolicFactorGraphUnordered fg; - fg.push_factor(0, 1); - fg.push_factor(0, 2); - fg.push_factor(1, 2); - fg.push_factor(3, 4); - - // create expected Chordal bayes Net - SymbolicBayesNetUnordered expected; - expected.push_back(boost::make_shared(0,1,2)); - expected.push_back(boost::make_shared(1,2)); - expected.push_back(boost::make_shared(2)); - expected.push_back(boost::make_shared(3,4)); - expected.push_back(boost::make_shared(4)); - - OrderingUnordered order; - order += 0,1,2,3,4; - SymbolicBayesNetUnordered actual = *SymbolicEliminationTreeUnordered(fg, order) - .eliminate(EliminateSymbolicUnordered).first; - - EXPECT(assert_equal(expected,actual)); -} - /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp index 95fff5618..2a2f436a2 100644 --- a/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp +++ b/gtsam/symbolic/tests/testSymbolicFactorGraph.cpp @@ -19,43 +19,65 @@ #include #include -#include +#include +#include using namespace std; using namespace gtsam; using namespace boost::assign; -namespace { - const SymbolicFactorGraphUnordered simpleTestGraph = list_of - (boost::make_shared(0,1)) - (boost::make_shared(0,2)) - (boost::make_shared(1,3)) - (boost::make_shared(1,4)) - (boost::make_shared(2,3)) - (boost::make_shared(4,5)); +/* ************************************************************************* */ +TEST(SymbolicFactorGraph, eliminateFullSequential) +{ + // Test with simpleTestGraph1 + OrderingUnordered order; + order += 0,1,2,3,4; + SymbolicBayesNetUnordered actual1 = *simpleTestGraph1.eliminateSequential(EliminateSymbolicUnordered, order); + EXPECT(assert_equal(simpleTestGraph1BayesNet, actual1)); + + // Test with Asia graph + SymbolicBayesNetUnordered actual2 = *asiaGraph.eliminateSequential( + EliminateSymbolicUnordered, asiaOrdering); + EXPECT(assert_equal(asiaBayesNet, actual2)); } /* ************************************************************************* */ TEST(SymbolicFactorGraph, eliminatePartialSequential) { - SymbolicBayesNetUnordered expectedBayesNet; - expectedBayesNet.add(SymbolicConditionalUnordered::FromKeys(list_of(0)(1)(2), 1)); - expectedBayesNet.add(SymbolicConditionalUnordered::FromKeys(list_of(1)(2)(3)(4), 1)); + // Eliminate 0 and 1 + const OrderingUnordered order = list_of(0)(1); - SymbolicFactorGraphUnordered expectedSfg; - expectedSfg.push_factor(2,3); - expectedSfg.push_factor(4,5); - expectedSfg.push_factor(2,3,4); + const SymbolicBayesNetUnordered expectedBayesNet = list_of + (boost::make_shared(0,1,2)) + (boost::make_shared(1,2,3,4)); + + const SymbolicFactorGraphUnordered expectedSfg = list_of + (boost::make_shared(2,3)) + (boost::make_shared(4,5)) + (boost::make_shared(2,3,4)); SymbolicBayesNetUnordered::shared_ptr actualBayesNet; SymbolicFactorGraphUnordered::shared_ptr actualSfg; - boost::tie(actualBayesNet, actualSfg) = simpleTestGraph.eliminatePartialSequential( + boost::tie(actualBayesNet, actualSfg) = simpleTestGraph2.eliminatePartialSequential( EliminateSymbolicUnordered, OrderingUnordered(list_of(0)(1))); EXPECT(assert_equal(expectedSfg, *actualSfg)); EXPECT(assert_equal(expectedBayesNet, *actualBayesNet)); } +/* ************************************************************************* */ +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); + +} + /* ************************************************************************* */ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) { @@ -64,21 +86,45 @@ TEST(SymbolicFactorGraph, eliminatePartialMultifrontal) SymbolicConditionalUnordered::FromKeys(list_of(5)(4)(1), 2)); expectedBayesTree.insertRoot(boost::make_shared(root)); - SymbolicFactorGraphUnordered expectedFactorGraph; - expectedFactorGraph.push_factor(0,1); - expectedFactorGraph.push_factor(0,2); - expectedFactorGraph.push_factor(1,3); - expectedFactorGraph.push_factor(2,3); - expectedFactorGraph.push_factor(1); + SymbolicFactorGraphUnordered expectedFactorGraph = list_of + (boost::make_shared(0,1)) + (boost::make_shared(0,2)) + (boost::make_shared(1,3)) + (boost::make_shared(2,3)) + (boost::make_shared(1)); SymbolicBayesTreeUnordered::shared_ptr actualBayesTree; SymbolicFactorGraphUnordered::shared_ptr actualFactorGraph; - boost::tie(actualBayesTree, actualFactorGraph) = simpleTestGraph.eliminatePartialMultifrontal( + boost::tie(actualBayesTree, actualFactorGraph) = simpleTestGraph2.eliminatePartialMultifrontal( EliminateSymbolicUnordered, OrderingUnordered(list_of(4)(5))); EXPECT(assert_equal(expectedFactorGraph, *actualFactorGraph)); EXPECT(assert_equal(expectedBayesTree, *actualBayesTree)); } + +/* ************************************************************************* */ +TEST(SymbolicFactorGraph, eliminate_disconnected_graph) { + SymbolicFactorGraphUnordered fg; + fg.push_factor(0, 1); + fg.push_factor(0, 2); + fg.push_factor(1, 2); + fg.push_factor(3, 4); + + // create expected Chordal bayes Net + SymbolicBayesNetUnordered expected; + expected.push_back(boost::make_shared(0,1,2)); + expected.push_back(boost::make_shared(1,2)); + expected.push_back(boost::make_shared(2)); + expected.push_back(boost::make_shared(3,4)); + expected.push_back(boost::make_shared(4)); + + OrderingUnordered order; + order += 0,1,2,3,4; + SymbolicBayesNetUnordered actual = *fg.eliminateSequential(EliminateSymbolicUnordered, order); + + EXPECT(assert_equal(expected,actual)); +} + /* ************************************************************************* */ //TEST(SymbolicFactorGraph, eliminateFrontals) { // diff --git a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp index ba7175e24..f166d35ca 100644 --- a/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicJunctionTree.cpp @@ -73,22 +73,6 @@ TEST( JunctionTree, constructor ) ****************************************************************************/ TEST( JunctionTree, eliminate) { - 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); - - SymbolicJunctionTree jt(fg); - SymbolicBayesTree::sharedClique actual = jt.eliminate(&EliminateSymbolic); - - BayesNet bn(*SymbolicSequentialSolver(fg).eliminate()); - SymbolicBayesTree expected(bn); - -// cout << "BT from JT:\n"; -// actual->printTree(""); - - CHECK(assert_equal(*expected.root(), *actual)); } /* ************************************************************************* */