From 71dd480b9595700e8cb6f803f351a44eafc5a673 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Thu, 6 Jun 2013 15:36:58 +0000 Subject: [PATCH] Fixed bug in JunctionTree construction with merging the wrong children - cleaned up some unit tests and added a sequential elimination ASIA test. --- gtsam/inference/BayesNetUnordered.h | 10 +++- gtsam/inference/JunctionTreeUnordered-inst.h | 13 +++--- gtsam/inference/OrderingUnordered.h | 7 +++ gtsam/inference/inference-inst.h | 4 +- gtsam/symbolic/SymbolicBayesNetUnordered.h | 4 ++ .../symbolic/tests/testSymbolicBayesTree.cpp | 4 +- .../tests/testSymbolicEliminationTree.cpp | 46 +++++++++++++++++++ 7 files changed, 78 insertions(+), 10 deletions(-) diff --git a/gtsam/inference/BayesNetUnordered.h b/gtsam/inference/BayesNetUnordered.h index 0552576dc..f10ace87a 100644 --- a/gtsam/inference/BayesNetUnordered.h +++ b/gtsam/inference/BayesNetUnordered.h @@ -33,21 +33,27 @@ namespace gtsam { template class BayesNetUnordered : public FactorGraphUnordered { - public: + private: typedef FactorGraphUnordered Base; - typedef typename boost::shared_ptr sharedConditional; ///< A shared pointer to a conditional public: + typedef typename boost::shared_ptr sharedConditional; ///< A shared pointer to a conditional + protected: /// @name Standard Constructors /// @{ /** Default constructor as an empty BayesNet */ BayesNetUnordered() {}; + /** Construct from iterator over conditionals */ + template + BayesNetUnordered(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {} + /// @} + public: /// @name Testable /// @{ diff --git a/gtsam/inference/JunctionTreeUnordered-inst.h b/gtsam/inference/JunctionTreeUnordered-inst.h index b41663fc3..f74fb15fe 100644 --- a/gtsam/inference/JunctionTreeUnordered-inst.h +++ b/gtsam/inference/JunctionTreeUnordered-inst.h @@ -62,7 +62,7 @@ namespace gtsam { // Post-order visitor function template void ConstructorTraversalVisitorPost( - const boost::shared_ptr& node, + const boost::shared_ptr& ETreeNode, const ConstructorTraversalData& myData) { // In this post-order visitor, we combine the symbolic elimination results from the @@ -74,21 +74,20 @@ namespace gtsam { // Do symbolic elimination for this node std::vector symbolicFactors; - symbolicFactors.reserve(node->factors.size() + myData.childSymbolicFactors.size()); + symbolicFactors.reserve(ETreeNode->factors.size() + myData.childSymbolicFactors.size()); // Add symbolic versions of the ETree node factors - BOOST_FOREACH(const typename GRAPH::sharedFactor& factor, node->factors) { + BOOST_FOREACH(const typename GRAPH::sharedFactor& factor, ETreeNode->factors) { symbolicFactors.push_back(boost::make_shared( SymbolicFactorUnordered::FromKeys(*factor))); } // Add symbolic factors passed up from children symbolicFactors.insert(symbolicFactors.end(), myData.childSymbolicFactors.begin(), myData.childSymbolicFactors.end()); - std::vector keyAsVector(1); keyAsVector[0] = node->key; + std::vector keyAsVector(1); keyAsVector[0] = ETreeNode->key; std::pair symbolicElimResult = EliminateSymbolicUnordered(symbolicFactors, keyAsVector); // Store symbolic elimination results in the parent myData.parentData->childSymbolicConditionals.push_back(symbolicElimResult.first); - if(!symbolicElimResult.second->empty()) - myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second); + myData.parentData->childSymbolicFactors.push_back(symbolicElimResult.second); // Merge our children if they are in our clique - if our conditional has exactly one fewer // parent than our child's conditional. @@ -109,6 +108,8 @@ namespace gtsam { myData.myJTNode->children.insert(myData.myJTNode->children.end(), childToMerge.children.begin(), childToMerge.children.end()); // Remove child from list. myData.myJTNode->children.erase(myData.myJTNode->children.begin() + child - nrMergedChildren); + // Increment number of merged children + ++ nrMergedChildren; } } } diff --git a/gtsam/inference/OrderingUnordered.h b/gtsam/inference/OrderingUnordered.h index aa50c2f01..ab1f4dd25 100644 --- a/gtsam/inference/OrderingUnordered.h +++ b/gtsam/inference/OrderingUnordered.h @@ -25,10 +25,17 @@ namespace gtsam { class OrderingUnordered : public std::vector { + protected: + typedef std::vector Base; + public: /// Create an empty ordering GTSAM_EXPORT OrderingUnordered() {} + /// Create an ordering using iterators over keys + template + OrderingUnordered(ITERATOR firstKey, ITERATOR lastKey) : Base(firstKey, lastKey) {} + /// Compute an ordering using COLAMD directly from a factor graph - this internally builds a /// VariableIndex so if you already have a VariableIndex, it is faster to use COLAMD(const /// VariableIndexUnordered&) diff --git a/gtsam/inference/inference-inst.h b/gtsam/inference/inference-inst.h index 51ec5366d..eb6ef9de7 100644 --- a/gtsam/inference/inference-inst.h +++ b/gtsam/inference/inference-inst.h @@ -54,7 +54,9 @@ namespace gtsam { RESULT& result, const typename TREE::Eliminate& eliminationFunction) { // Call eliminate on the node and add the result to the parent's gathered factors - myData.parentData->childFactors.push_back(node->eliminate(result, eliminationFunction, myData.childFactors)); + typename TREE::sharedFactor childFactor = node->eliminate(result, eliminationFunction, myData.childFactors); + if(!childFactor->empty()) + myData.parentData->childFactors.push_back(childFactor); } } diff --git a/gtsam/symbolic/SymbolicBayesNetUnordered.h b/gtsam/symbolic/SymbolicBayesNetUnordered.h index 57cf1f9f0..7eb137e9c 100644 --- a/gtsam/symbolic/SymbolicBayesNetUnordered.h +++ b/gtsam/symbolic/SymbolicBayesNetUnordered.h @@ -41,6 +41,10 @@ namespace gtsam { /** Construct empty factor graph */ SymbolicBayesNetUnordered() {} + + /** Construct from iterator over conditionals */ + template + SymbolicBayesNetUnordered(ITERATOR firstConditional, ITERATOR lastConditional) : Base(firstConditional, lastConditional) {} /// @} /// @name Standard Interface diff --git a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp index c55851a9e..ef6766e11 100644 --- a/gtsam/symbolic/tests/testSymbolicBayesTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicBayesTree.cpp @@ -18,6 +18,7 @@ */ #include +#include #include #include @@ -27,12 +28,13 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +using namespace gtsam::symbol_shorthand; static bool debug = false; /* ************************************************************************* */ // Conditionals for ASIA example from the tutorial with A and D evidence -static const Key _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; +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 b06d69b69..bc3c9a902 100644 --- a/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp +++ b/gtsam/symbolic/tests/testSymbolicEliminationTree.cpp @@ -20,13 +20,16 @@ #include #include +#include using namespace boost::assign; #include #include #include +#include using namespace gtsam; +using namespace gtsam::symbol_shorthand; using namespace std; class EliminationTreeUnorderedTester { @@ -64,6 +67,32 @@ public: } }; +/* ************************************************************************* */ +namespace { + + /* ************************************************************************* */ + // Conditionals 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); + + // Bayes Tree for Asia example + SymbolicFactorGraphUnordered createAsiaGraph() { + 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_); + return asiaGraph; + } + + /* ************************************************************************* */ + OrderingUnordered asiaOrdering = list_of(_X_)(_T_)(_S_)(_E_)(_L_)(_B_); + +} + +/* ************************************************************************* */ TEST(EliminationTree, Create) { // create example factor graph @@ -115,6 +144,23 @@ TEST_UNSAFE(EliminationTree, eliminate ) 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 = *createAsiaGraph().eliminateSequential( + EliminateSymbolicUnordered, asiaOrdering); + + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ TEST(EliminationTree, disconnected_graph) { SymbolicFactorGraphUnordered fg;