Added BayesTree::removeSubtree

gtsam/inference/BayesTree-inl.h

@@ -756,6 +756,43 @@ namespace gtsam {
       orphan->deleteCachedShortcuts();
   }
 
+  /* ************************************************************************* */
+  template<class CONDITIONAL, class CLIQUE>
+  typename BayesTree<CONDITIONAL,CLIQUE>::Cliques BayesTree<CONDITIONAL,CLIQUE>::removeSubtree(
+    const sharedClique& subtree)
+  {
+    // Result clique list
+    Cliques cliques;
+    cliques.push_back(subtree);
+
+    // Remove the first clique from its parents
+    if(!subtree->isRoot())
+      subtree->parent()->children().remove(subtree);
+    else
+      root_.reset();
+
+    // Add all subtree cliques and erase the children and parent of each
+    for(Cliques::iterator clique = cliques.begin(); clique != cliques.end(); ++clique)
+    {
+      // Add children
+      BOOST_FOREACH(const sharedClique& child, (*clique)->children()) {
+        cliques.push_back(child); }
+
+      // Delete cached shortcuts
+      (*clique)->deleteCachedShortcutsNonRecursive();
+
+      // Remove this node from the nodes index
+      BOOST_FOREACH(Index j, (*clique)->conditional()->frontals()) {
+        nodes_[j].reset(); }
+
+      // Erase the parent and children pointers
+      (*clique)->parent_.reset();
+      (*clique)->children_.clear();
+    }
+
+    return cliques;
+  }
+
   /* ************************************************************************* */
   template<class CONDITIONAL, class CLIQUE>
   void BayesTree<CONDITIONAL,CLIQUE>::cloneTo(This& newTree) const {

gtsam/inference/BayesTree.h

@@ -252,6 +252,10 @@ namespace gtsam {
     template<class CONTAINER>
     void removeTop(const CONTAINER& indices, BayesNet<CONDITIONAL>& bn, Cliques& orphans);
 
+    /**
+     * Remove the requested subtree. */
+    Cliques removeSubtree(const sharedClique& subtree);
+
     /**
      * Hang a new subtree off of the existing tree.  This finds the appropriate
      * parent clique for the subtree (which may be the root), and updates the