From 229e2d91954221d930ef5bdedc73efbc751b7220 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Mon, 18 Mar 2013 19:27:59 +0000
Subject: [PATCH] Added BayesTree::removeSubtree

---
 gtsam/inference/BayesTree-inl.h | 37 +++++++++++++++++++++++++++++++++
 gtsam/inference/BayesTree.h     |  4 ++++
 2 files changed, 41 insertions(+)

diff --git a/gtsam/inference/BayesTree-inl.h b/gtsam/inference/BayesTree-inl.h
index f862f86a4..1efaf593c 100644
--- a/gtsam/inference/BayesTree-inl.h
+++ b/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 {
diff --git a/gtsam/inference/BayesTree.h b/gtsam/inference/BayesTree.h
index ebc62a5e4..6199d66a6 100644
--- a/gtsam/inference/BayesTree.h
+++ b/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