diff --git a/cpp/BayesTree-inl.h b/cpp/BayesTree-inl.h index d3b56806d..3b8c41322 100644 --- a/cpp/BayesTree-inl.h +++ b/cpp/BayesTree-inl.h @@ -197,7 +197,7 @@ namespace gtsam { BOOST_FOREACH(sharedClique child, clique->children_) child->parent_.reset(); - BOOST_FOREACH(std::string key, clique->ordering()) + BOOST_FOREACH(string key, clique->ordering()) nodes_.erase(key); } @@ -311,7 +311,7 @@ namespace gtsam { template template FactorGraph - BayesTree::joint(const std::string& key1, const std::string& key2) const { + BayesTree::joint(const string& key1, const string& key2) const { // get clique C1 and C2 sharedClique C1 = (*this)[key1], C2 = (*this)[key2]; @@ -334,7 +334,7 @@ namespace gtsam { template template BayesNet - BayesTree::jointBayesNet(const std::string& key1, const std::string& key2) const { + BayesTree::jointBayesNet(const string& key1, const string& key2) const { // calculate marginal as a factor graph FactorGraph fg = this->joint(key1,key2); @@ -344,32 +344,33 @@ namespace gtsam { ordering += key1, key2; return eliminate(fg,ordering); } -#if 0 + /* ************************************************************************* */ template template - std::pair, std::list > + pair, list::sharedClique> > BayesTree::removePath(sharedClique clique) { - // base case is NULL, return empty factor graph - if (clique==NULL) { - list orphans; - return make_pair(), orphans>; + FactorGraph factors; + list orphans; + + // base case is NULL, if so we do nothing and return empties above + if (clique!=NULL) { + + // remove path above me + boost::tie(factors,orphans) = removePath(clique->parent_); + + // add children to list of orphans + orphans.insert(orphans.begin(), clique->children_.begin(), clique->children_.end()); + + // remove me and add my factors + removeClique(clique); + factors.push_back(*clique); } - // remove path above me - std::pair, std::list > factors_orphans = removePath(clique->parent_); - - // add children to list of orphans - factors_orphans.second.insert(factors_orphans.second.begin(), clique->children_.begin(), clique->children_.end()); - - // remove me and add my factors - removeClique(clique); - factors.push_back(*clique); - - return factors_orphans; + return make_pair(factors,orphans); } -#endif + /* ************************************************************************* */ } diff --git a/cpp/BayesTree.h b/cpp/BayesTree.h index 29c29ce3c..9b22829e6 100644 --- a/cpp/BayesTree.h +++ b/cpp/BayesTree.h @@ -154,11 +154,11 @@ namespace gtsam { /** return joint on two variables as a BayesNet */ template BayesNet jointBayesNet(const std::string& key1, const std::string& key2) const; -#if 0 + /** Remove path from clique to root and return that path as factors plus a list of orphaned subtree roots */ template std::pair, std::list > removePath(sharedClique clique); -#endif + }; // BayesTree } /// namespace gtsam diff --git a/cpp/testBayesTree.cpp b/cpp/testBayesTree.cpp index bfc12275d..2d4e7adaf 100644 --- a/cpp/testBayesTree.cpp +++ b/cpp/testBayesTree.cpp @@ -331,18 +331,18 @@ TEST( BayesTree, removePath ) expected.push_factor("A","B"); expected.push_factor("A"); expected.push_factor("A","C"); -#if 0 - std::pair, std::list > actual = - bayesTree.removePath(bayesTree["C"]); - CHECK(assert_equal(expected, actual.first)); + + FactorGraph factors; + list orphans; + boost::tie(factors,orphans) = bayesTree.removePath(bayesTree["C"]); + CHECK(assert_equal((FactorGraph)expected, factors)); // remove E: factor graph with EB; E|B removed from second orphan tree SymbolicFactorGraph expected3; expected3.push_factor("B","E"); - actual = bayesTree.removePath(bayesTree["E"]); - CHECK(assert_equal(expected3, actual.first)); -#endif + boost::tie(factors,orphans) = bayesTree.removePath(bayesTree["E"]); + CHECK(assert_equal((FactorGraph)expected3, factors)); } /* ************************************************************************* */