diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index 2391beee6..02bd8ce47 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -171,6 +171,37 @@ namespace gtsam { return marginalize(*bn,keys12); } + /* ************************************************************************* */ + template + typename BayesTree::sharedClique BayesTree::addClique + (const sharedConditional& conditional, sharedClique parent_clique) { + sharedClique new_clique(new Clique(conditional)); + nodes_.insert(make_pair(conditional->key(), new_clique)); + if (parent_clique != NULL) { + new_clique->parent_ = parent_clique; + parent_clique->children_.push_back(new_clique); + } + return new_clique; + } + + /* ************************************************************************* */ + template + void BayesTree::removeClique(sharedClique clique) { + if (!clique->isRoot()) + clique->parent_->children_.remove(clique); + else { + // we remove the root clique: have to make another clique the root + if (clique->children_.empty()) + root_.reset(); + else + root_ = *(clique->children_.begin()); + } + BOOST_FOREACH(sharedClique child, clique->children_) + child->parent_.reset(); + BOOST_FOREACH(std::string key, clique->ordering()) + nodes_.erase(key); + } + /* ************************************************************************* */ template BayesTree::BayesTree() { @@ -319,21 +350,20 @@ namespace gtsam { template template FactorGraph - BayesTree::removePath(const string& key) { - sharedClique clique = (*this)[key]; + BayesTree::removePath(sharedClique clique) { -#if 0 - cout << "removing:" << endl; - clique->print(); - cout << "from" << endl; - typedef std::pair sometype; - BOOST_FOREACH(sometype clique, nodes_) { - clique.second->print(); + //if (clique==NULL) return; + + bool verbose = false; + if (verbose) { + clique->print("removing"); + cout << "before" << endl; + BOOST_FOREACH(typename Nodes::value_type clique, nodes_) + clique.second->print(); } -#endif // convert clique to factor - FactorGraph factors(*clique); + FactorGraph factors(*clique);// = removePath(clique->parent_); while (!(clique->isRoot())) { sharedClique old_clique = clique; @@ -343,6 +373,12 @@ namespace gtsam { } removeClique(clique); + if (verbose) { + cout << "after" << endl; + BOOST_FOREACH(typename Nodes::value_type clique, nodes_) + clique.second->print(); + } + return factors; } diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index 6dcc70eb2..422896f07 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -51,7 +51,7 @@ namespace gtsam { Ordering keys() const; /** print this node */ - void print(const std::string& s = "Bayes tree node") const; + void print(const std::string& s = "") const; /** The size *includes* the separator */ size_t size() const { @@ -94,34 +94,10 @@ namespace gtsam { /** add a clique */ sharedClique addClique(const sharedConditional& conditional, - sharedClique parent_clique = sharedClique()) { - sharedClique new_clique(new Clique(conditional)); - nodes_.insert(make_pair(conditional->key(), new_clique)); - if (parent_clique != NULL) { - new_clique->parent_ = parent_clique; - parent_clique->children_.push_back(new_clique); - } - return new_clique; - } + sharedClique parent_clique = sharedClique()); /** remove a clique: warning, can result in a forest */ - void removeClique(sharedClique clique) { - if (clique->parent_ != NULL) { - clique->parent_->children_.remove(clique); - } else { - // we remove the root clique: have to make another clique the root - if (clique->children_.empty()) { - root_.reset(); - } else { - root_ = *(clique->children_.begin()); - } - } - BOOST_FOREACH(sharedClique child, clique->children_) { - child->parent_.reset(); - } - std::string key = *(clique->keys().begin()); - nodes_.erase(key); - } + void removeClique(sharedClique clique); public: @@ -179,11 +155,10 @@ namespace gtsam { template BayesNet jointBayesNet(const std::string& key1, const std::string& key2) const; - /** IMPERATIVE! Return the factor containing all nodes contaminated by key; also, - * removes those entries from the Bayes Tree, resulting in a forest of orphans - */ + /** Remove path from clique to root and return that path as factors */ template - FactorGraph removePath(const std::string& key); + FactorGraph removePath(sharedClique clique); + }; // BayesTree } /// namespace gtsam diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index 10089bc11..342ad0b5a 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -332,21 +332,15 @@ TEST( BayesTree, removePath ) expected.push_factor("A","B"); expected.push_factor("A"); - SymbolicFactorGraph actual = bayesTree.removePath("C"); + SymbolicFactorGraph actual = bayesTree.removePath(bayesTree["C"]); CHECK(assert_equal(expected, actual)); - // remove A, nothing should happen (already removed) - SymbolicFactorGraph expected2; // empty factor - - actual = bayesTree.removePath("A"); -// CHECK(assert_equal(expected2, actual)); - // remove E: factor graph with EB; E|B removed from second orphan tree SymbolicFactorGraph expected3; - expected3.push_factor("C","A"); + expected3.push_factor("B","E"); - actual = bayesTree.removePath("E"); -// CHECK(assert_equal(expected3, actual)); + actual = bayesTree.removePath(bayesTree["E"]); + CHECK(assert_equal(expected3, actual)); } /* ************************************************************************* */