diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index a895be67d..afefe83de 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -372,6 +372,38 @@ namespace gtsam { } /* ************************************************************************* */ + template + template + pair, typename BayesTree::Cliques> + BayesTree::removeTop(const boost::shared_ptr& newFactor) { + + FactorGraph factors; + Cliques orphans; + + // process each key of the new factor + BOOST_FOREACH(string key, newFactor->keys()) + // only add if key is not yet in the factor graph + if (!factors.involves(key)) { + + // get the clique + sharedClique clique = (*this)[key]; + + // remove path above this clique + FactorGraph factors1; Cliques orphans1; + boost::tie(factors1,orphans1) = removePath(clique->parent_); + + // add to global factors and orphans + factors.push_back(factors1); + orphans.splice (orphans.begin(), orphans1); + } + + // now add the new factor + factors.push_back(newFactor); + + return make_pair(factors,orphans); + } + + /* ************************************************************************* */ } /// namespace gtsam diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index cd6db13cd..4eb911990 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -162,10 +162,21 @@ namespace gtsam { template BayesNet jointBayesNet(const std::string& key1, const std::string& key2) const; - /** Remove path from clique to root and return that path as factors plus a list of orphaned subtree roots */ + /** + * Remove path from clique to root and return that path as factors + * plus a list of orphaned subtree roots. Used in removeTop below. + */ template std::pair, Cliques> removePath(sharedClique clique); + /** + * Given a set of factors, turn "contaminated" part of the tree back into a factor graph + * and return it along with the new factors plus a list of orphaned subtree roots. + * This is used for incrementally updating a BayesTree given new measurements (factors). + */ + template + std::pair, Cliques> removeTop(const boost::shared_ptr& newFactor); + }; // BayesTree } /// namespace gtsam diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index 9b580eda0..eff4f74c1 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -345,6 +345,46 @@ TEST( BayesTree, removePath ) CHECK(assert_equal((FactorGraph)expected3, factors)); } +/* ************************************************************************* */ +TEST( BayesTree, removeTop ) +{ + // Conditionals for ASIA example from the tutorial with A and D evidence + SymbolicConditional::shared_ptr + B(new SymbolicConditional("B")), + L(new SymbolicConditional("L", "B")), + E(new SymbolicConditional("E", "B", "L")), + S(new SymbolicConditional("S", "L", "B")), + T(new SymbolicConditional("T", "E", "L")), + X(new SymbolicConditional("X", "E")); + + // Create using insert + SymbolicBayesTree bayesTree; + bayesTree.insert(B); + bayesTree.insert(L); + bayesTree.insert(E); + bayesTree.insert(S); + bayesTree.insert(T); + bayesTree.insert(X); + + // create a new factor to be inserted + list keys; + keys += "B","S"; + boost::shared_ptr newFactor(new SymbolicFactor(keys)); + + // Remove the contaminated part of the Bayes tree + FactorGraph factors; + SymbolicBayesTree::Cliques orphans; + boost::tie(factors,orphans) = bayesTree.removeTop(newFactor); + + // Check expected outcome + SymbolicFactorGraph expected; + expected.push_factor("B","L","E"); + expected.push_factor("B","L"); + expected.push_factor("B"); + expected.push_factor("B","S"); + CHECK(assert_equal((FactorGraph)expected, factors)); +} + /* ************************************************************************* */ int main() { TestResult tr;