diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 5cb2c34a9..411f0bba7 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -555,20 +555,6 @@ namespace gtsam { } } - /* ************************************************************************* */ - template - void BayesTree::deleteCachedShorcuts(const sharedClique& subtree) { - // Check if subtree exists - if (subtree) { - //Delete CachedShortcut for this clique - subtree->resetCachedShortcut(); - // Recursive call over all child cliques - BOOST_FOREACH(sharedClique& childClique, subtree->children()) { - deleteCachedShorcuts(childClique); - } - } - } - /* ************************************************************************* */ template template @@ -591,7 +577,7 @@ namespace gtsam { // Delete cachedShorcuts for each orphan subtree //TODO: Consider Improving BOOST_FOREACH(sharedClique& orphan, orphans) - deleteCachedShorcuts(orphan); + orphan->deleteCachedShorcuts(); } /* ************************************************************************* */ diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 14b01ab03..8c7ba1a39 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -280,11 +280,6 @@ namespace gtsam { sharedClique insert(const sharedConditional& clique, std::list& children, bool isRootClique = false); - /** - * This deletes the cached shortcuts of all cliques in a subtree. This is - * performed when the bayes tree is modified. - */ - void deleteCachedShorcuts(const sharedClique& subtree); private: diff --git a/gtsam/inference/BayesTreeCliqueBase-inl.h b/gtsam/inference/BayesTreeCliqueBase-inl.h index c5851a5e4..7e0ba6a7b 100644 --- a/gtsam/inference/BayesTreeCliqueBase-inl.h +++ b/gtsam/inference/BayesTreeCliqueBase-inl.h @@ -265,4 +265,15 @@ namespace gtsam { return *solver.jointFactorGraph(keys12vector, function); } + /* ************************************************************************* */ + template + void BayesTreeCliqueBase::deleteCachedShorcuts() { + //Delete CachedShortcut for this clique + this->resetCachedShortcut(); + // Recursive call over all child cliques + BOOST_FOREACH(derived_ptr& child, children_) { + child->deleteCachedShorcuts(); + } + } + } diff --git a/gtsam/inference/BayesTreeCliqueBase.h b/gtsam/inference/BayesTreeCliqueBase.h index 675215904..e1a7395da 100644 --- a/gtsam/inference/BayesTreeCliqueBase.h +++ b/gtsam/inference/BayesTreeCliqueBase.h @@ -161,6 +161,15 @@ namespace gtsam { /** return the joint P(C1,C2), where C1==this. TODO: not a method? */ FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; + /** + * This deletes the cached shortcuts of all cliques (subtree) below this clique. + * This is performed when the bayes tree is modified. + */ + void deleteCachedShorcuts(); + + /** return cached shortcut of the clique */ + const boost::optional > cachedShortcut() const { return cachedShortcut_; } + friend class BayesTree; protected: diff --git a/gtsam/inference/tests/testBayesTree.cpp b/gtsam/inference/tests/testBayesTree.cpp index f64e56df6..b99364092 100644 --- a/gtsam/inference/tests/testBayesTree.cpp +++ b/gtsam/inference/tests/testBayesTree.cpp @@ -238,6 +238,76 @@ TEST( BayesTree, removePath3 ) CHECK(assert_equal(expectedOrphans, orphans)); } +void getAllCliques(const SymbolicBayesTree::sharedClique& subtree, SymbolicBayesTree::Cliques& cliques) { + // Check if subtree exists + if (subtree) { + cliques.push_back(subtree); + // Recursive call over all child cliques + BOOST_FOREACH(SymbolicBayesTree::sharedClique& childClique, subtree->children()) { + getAllCliques(childClique,cliques); + } + } +} + +/* ************************************************************************* */ +TEST( BayesTree, shortcutCheck ) +{ + const Index _A_=6, _B_=5, _C_=4, _D_=3, _E_=2, _F_=1, _G_=0; + IndexConditional::shared_ptr + A(new IndexConditional(_A_)), + B(new IndexConditional(_B_, _A_)), + C(new IndexConditional(_C_, _A_)), + D(new IndexConditional(_D_, _C_)), + E(new IndexConditional(_E_, _B_)), + F(new IndexConditional(_F_, _E_)), + G(new IndexConditional(_G_, _F_)); + SymbolicBayesTree bayesTree; +// Ordering ord; ord += _A_,_B_,_C_,_D_,_E_,_F_; + SymbolicBayesTree::insert(bayesTree, A); + SymbolicBayesTree::insert(bayesTree, B); + SymbolicBayesTree::insert(bayesTree, C); + SymbolicBayesTree::insert(bayesTree, D); + SymbolicBayesTree::insert(bayesTree, E); + SymbolicBayesTree::insert(bayesTree, F); + SymbolicBayesTree::insert(bayesTree, G); + + //bayesTree.print("BayesTree"); + //bayesTree.saveGraph("BT1.dot"); + + SymbolicBayesTree::sharedClique rootClique= bayesTree.root(); + //rootClique->printTree(); + SymbolicBayesTree::Cliques allCliques; + getAllCliques(rootClique,allCliques); + + BayesNet bn; + BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { + //clique->print("Clique#"); + bn = clique->shortcut(rootClique, &EliminateSymbolic); + //bn.print("Shortcut:\n"); + //cout << endl; + } + + // Check if all the cached shortcuts are cleared + rootClique->deleteCachedShorcuts(); + BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { + bool notCleared = clique->cachedShortcut(); + CHECK( notCleared == false); + } + +// BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { +// clique->print("Clique#"); +// if(clique->cachedShortcut()){ +// bn = clique->cachedShortcut().get(); +// bn.print("Shortcut:\n"); +// } +// else +// cout << "Not Initialized" << endl; +// cout << endl; +// } +} + + + /* ************************************************************************* */ TEST( BayesTree, removeTop ) {