diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h index 1c696d3ef..23148cea8 100644 --- a/gtsam/inference/BayesTree-inl.h +++ b/gtsam/inference/BayesTree-inl.h @@ -152,20 +152,29 @@ namespace gtsam { /* ************************************************************************* */ template typename BayesTree::sharedClique - BayesTree::addClique(const sharedConditional& conditional, sharedClique parent_clique) { + BayesTree::addClique(const sharedConditional& conditional, const sharedClique& parent_clique) { sharedClique new_clique(new Clique(conditional)); - nodes_.resize(std::max(conditional->lastFrontalKey()+1, nodes_.size())); - BOOST_FOREACH(Index key, conditional->frontals()) - nodes_[key] = new_clique; - if (parent_clique != NULL) { - new_clique->parent_ = parent_clique; - parent_clique->children_.push_back(new_clique); - } - new_clique->assertInvariants(); + addClique(new_clique, parent_clique); return new_clique; } - /* ************************************************************************* */ + /* ************************************************************************* */ + template + void BayesTree::addClique(const sharedClique& clique, const sharedClique& parent_clique) { + nodes_.resize(std::max((*clique)->lastFrontalKey()+1, nodes_.size())); + BOOST_FOREACH(Index key, (*clique)->frontals()) + nodes_[key] = clique; + if (parent_clique != NULL) { + clique->parent_ = parent_clique; + parent_clique->children_.push_back(clique); + } else { + assert(!root_); + root_ = clique; + } + clique->assertInvariants(); + } + + /* ************************************************************************* */ template typename BayesTree::sharedClique BayesTree::addClique( const sharedConditional& conditional, list& child_cliques) { diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h index 650341a5c..1fae472fc 100644 --- a/gtsam/inference/BayesTree.h +++ b/gtsam/inference/BayesTree.h @@ -100,8 +100,6 @@ namespace gtsam { /** Gather data on a single clique */ void getCliqueData(CliqueData& stats, sharedClique clique) const; - protected: - /** Root clique */ sharedClique root_; @@ -109,12 +107,13 @@ namespace gtsam { void removeClique(sharedClique clique); /** add a clique (top down) */ - sharedClique addClique(const sharedConditional& conditional, - sharedClique parent_clique = sharedClique()); + sharedClique addClique(const sharedConditional& conditional, const sharedClique& parent_clique = sharedClique()); + + /** add a clique (top down) */ + void addClique(const sharedClique& clique, const sharedClique& parent_clique = sharedClique()); /** add a clique (bottom up) */ - sharedClique addClique(const sharedConditional& conditional, - std::list& child_cliques); + sharedClique addClique(const sharedConditional& conditional, std::list& child_cliques); /** * Add a conditional to the front of a clique, i.e. a conditional whose @@ -179,20 +178,19 @@ namespace gtsam { bool equals(const BayesTree& other, double tol = 1e-9) const; void cloneTo(shared_ptr& newTree) const { - cloneTo(newTree, root()); + cloneTo(newTree, root(), sharedClique()); } private: /** deep copy from another tree */ - void cloneTo(shared_ptr& newTree, const sharedClique& root) const { - if(root) { - sharedClique newClique = root->clone(); - newTree->insert(newClique); - BOOST_FOREACH(const sharedClique& childClique, root->children()) { - cloneTo(newTree, childClique); - } + void cloneTo(shared_ptr& newTree, const sharedClique& subtree, const sharedClique& parent) const { + sharedClique newClique(subtree->clone()); + newTree->addClique(newClique, parent); + BOOST_FOREACH(const sharedClique& childClique, subtree->children()) { + cloneTo(newTree, childClique, newClique); } } + public: /**