Updated and made removeTop unit tests pass

gtsam/inference/BayesTreeUnordered-inst.h

@@ -201,28 +201,6 @@ namespace gtsam {
     treeTraversal::DepthFirstForest(*this, data, boost::bind(&_pushClique<FactorType,CLIQUE>, boost::ref(graph), _1));
   }
 
-  /* ************************************************************************* */
-  template<class CLIQUE>
-  void BayesTreeUnordered<CLIQUE>::removeClique(sharedClique clique)
-  {
-    if (clique->isRoot())
-      roots_.erase(std::find(roots_.begin(), roots_.end(), clique));
-    else { // detach clique from parent
-      sharedClique parent = clique->parent_.lock();
-      typename std::vector<sharedClique>::iterator child = std::find(parent->children.begin(), parent->children.end(), clique);
-      assert(child != parent->children.end());
-      parent->children.erase(child);
-    }
-
-    // orphan my children
-    BOOST_FOREACH(sharedClique child, clique->children)
-      child->parent_ = typename Clique::weak_ptr();
-
-    BOOST_FOREACH(Key j, clique->conditional()->frontals()) {
-      nodes_.erase(j);
-    }
-  }
-
   /* ************************************************************************* */
   //template<class CLIQUE>
   //void BayesTreeUnordered<CLIQUE>::recursiveTreeBuild(const boost::shared_ptr<BayesTreeClique<IndexConditional> >& symbolic,
@@ -535,6 +513,30 @@ namespace gtsam {
     }
   }
 
+  /* ************************************************************************* */
+  template<class CLIQUE>
+  void BayesTreeUnordered<CLIQUE>::removeClique(sharedClique clique)
+  {
+    if (clique->isRoot()) {
+      std::vector<sharedClique>::iterator root = std::find(roots_.begin(), roots_.end(), clique);
+      if(root != roots_.end())
+        roots_.erase(root);
+    } else { // detach clique from parent
+      sharedClique parent = clique->parent_.lock();
+      typename std::vector<sharedClique>::iterator child = std::find(parent->children.begin(), parent->children.end(), clique);
+      assert(child != parent->children.end());
+      parent->children.erase(child);
+    }
+
+    // orphan my children
+    BOOST_FOREACH(sharedClique child, clique->children)
+      child->parent_ = typename Clique::weak_ptr();
+
+    BOOST_FOREACH(Key j, clique->conditional()->frontals()) {
+      nodes_.erase(j);
+    }
+  }
+
   /* ************************************************************************* */
   template<class CLIQUE>
   void BayesTreeUnordered<CLIQUE>::removePath(sharedClique clique, BayesNetType& bn, Cliques& orphans)
@@ -562,19 +564,17 @@ namespace gtsam {
 
   /* ************************************************************************* */
   template<class CLIQUE>
-  template<class CONTAINER>
+  void BayesTreeUnordered<CLIQUE>::removeTop(const std::vector<Key>& keys, BayesNetType& bn, Cliques& orphans)
-  void BayesTreeUnordered<CLIQUE>::removeTop(const CONTAINER& keys, BayesNetType& bn, Cliques& orphans)
   {
     // process each key of the new factor
-    BOOST_FOREACH(const Key& j, keys) {
+    BOOST_FOREACH(const Key& j, keys)
-
+    {
       // get the clique
-      if(j < nodes_.size()) {
+      // TODO: Nodes will be searched again in removeClique
-        const sharedClique& clique(nodes_[j]);
+      Nodes::const_iterator node = nodes_.find(j);
-        if(clique) {
+      if(node != nodes_.end()) {
-          // remove path from clique to root
+        // remove path from clique to root
-          this->removePath(clique, bn, orphans);
+        this->removePath(node->second, bn, orphans);
-        }
       }
     }
 

gtsam/inference/BayesTreeUnordered.h

@@ -208,8 +208,7 @@ namespace gtsam {
      * Given a list of indices, turn "contaminated" part of the tree back into a factor graph.
      * Factors and orphans are added to the in/out arguments.
      */
-    template<class CONTAINER>
+    void removeTop(const std::vector<Key>& keys, BayesNetType& bn, Cliques& orphans);
-    void removeTop(const CONTAINER& keys, BayesNetType& bn, Cliques& orphans);
 
     /**
      * Remove the requested subtree. */

gtsam/symbolic/tests/testSymbolicBayesTree.cpp

@@ -295,25 +295,19 @@ TEST( BayesTree, removeTop )
 {
   SymbolicBayesTreeUnordered bayesTree = asiaBayesTree;
 
-  bayesTree.print("asiaBayesTree: ");
-
   // create a new factor to be inserted
   //boost::shared_ptr<IndexFactor> newFactor(new IndexFactor(_S_,_B_));
 
   // Remove the contaminated part of the Bayes tree
   SymbolicBayesNetUnordered bn;
   SymbolicBayesTreeUnordered::Cliques orphans;
-  list<Key> keys; keys += _B_,_S_;
+  bayesTree.removeTop(list_of(_B_)(_S_), bn, orphans);
-  bayesTree.removeTop(keys, bn, orphans);
-  SymbolicFactorGraphUnordered factors(bn);
 
   // Check expected outcome
-  SymbolicFactorGraphUnordered expected;
+  SymbolicBayesNetUnordered expected;
-  expected.push_factor(_E_,_L_,_B_);
+  expected += SymbolicConditionalUnordered::FromKeys(list_of(_B_)(_L_)(_E_)(_S_), 4);
-//  expected.push_factor(_L_,_B_);
+  CHECK(assert_equal(expected, bn));
-//  expected.push_factor(_B_);
+
-  expected.push_factor(_S_,_L_,_B_);
-  CHECK(assert_equal(expected, factors));
   SymbolicBayesTreeUnordered::Cliques expectedOrphans;
   expectedOrphans += bayesTree[_T_], bayesTree[_X_];
   CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected));
@@ -322,8 +316,7 @@ TEST( BayesTree, removeTop )
   //boost::shared_ptr<IndexFactor> newFactor2(new IndexFactor(_B_));
   SymbolicBayesNetUnordered bn2;
   SymbolicBayesTreeUnordered::Cliques orphans2;
-  keys.clear(); keys += _B_;
+  bayesTree.removeTop(list_of(_B_), bn2, orphans2);
-  bayesTree.removeTop(keys, bn2, orphans2);
   SymbolicFactorGraphUnordered factors2(bn2);
   SymbolicFactorGraphUnordered expected2;
   CHECK(assert_equal(expected2, factors2));
@@ -344,50 +337,65 @@ TEST( BayesTree, removeTop2 )
   // Remove the contaminated part of the Bayes tree
   SymbolicBayesNetUnordered bn;
   SymbolicBayesTreeUnordered::Cliques orphans;
-  list<Key> keys; keys += _B_,_S_;
+  bayesTree.removeTop(list_of(_T_), bn, orphans);
-  bayesTree.removeTop(keys, bn, orphans);
-  SymbolicFactorGraphUnordered factors(bn);
 
   // Check expected outcome
-  SymbolicFactorGraphUnordered expected;
+  SymbolicBayesNetUnordered expected = list_of
-  expected.push_factor(_E_,_L_,_B_);
+    (SymbolicConditionalUnordered::FromKeys(list_of(_B_)(_L_)(_E_)(_S_), 4))
-//  expected.push_factor(_L_,_B_);
+    (SymbolicConditionalUnordered::FromKeys(list_of(_T_)(_E_)(_L_), 1));
-//  expected.push_factor(_B_);
+  CHECK(assert_equal(expected, bn));
-  expected.push_factor(_S_,_L_,_B_);
+
-  CHECK(assert_equal(expected, factors));
   SymbolicBayesTreeUnordered::Cliques expectedOrphans;
-  expectedOrphans += bayesTree[_T_], bayesTree[_X_];
+  expectedOrphans += bayesTree[_X_];
   CHECK(assert_container_equal(expectedOrphans|indirected, orphans|indirected));
 }
 
 /* ************************************************************************* */
 TEST( BayesTree, removeTop3 )
 {
-  const Key _x4_=5, _l5_=6;
+  SymbolicFactorGraphUnordered graph = list_of
-  // simple test case that failed after COLAMD was fixed/activated
+    (SymbolicFactorUnordered(L(5)))
-  SymbolicConditionalUnordered::shared_ptr
+    (SymbolicFactorUnordered(X(4), L(5)))
-  X(new SymbolicConditionalUnordered(_l5_)),
+    (SymbolicFactorUnordered(X(2), X(4)))
-  A(new SymbolicConditionalUnordered(_x4_, _l5_)),
+    (SymbolicFactorUnordered(X(3), X(2)));
-  B(new SymbolicConditionalUnordered(_x2_, _x4_)),
+  SymbolicBayesTreeUnordered bayesTree = *graph.eliminateMultifrontal(
-  C(new SymbolicConditionalUnordered(_x3_, _x2_));
+    OrderingUnordered(list_of (X(3)) (X(2)) (X(4)) (L(5)) ));
-
-//  Ordering newOrdering;
-//  newOrdering += _x3_, _x2_, _x1_, _l2_, _l1_, _x4_, _l5_;
-  SymbolicBayesTreeUnordered bayesTree;
-  SymbolicBayesTreeUnordered::insert(bayesTree, X);
-  SymbolicBayesTreeUnordered::insert(bayesTree, A);
-  SymbolicBayesTreeUnordered::insert(bayesTree, B);
-  SymbolicBayesTreeUnordered::insert(bayesTree, C);
 
   // remove all
-  list<Index> keys;
+  SymbolicBayesNetUnordered bn;
-  keys += _l5_, _x2_, _x3_, _x4_;
-  BayesNet<SymbolicConditionalUnordered> bn;
   SymbolicBayesTreeUnordered::Cliques orphans;
-  bayesTree.removeTop(keys, bn, orphans);
+  bayesTree.removeTop(list_of(L(5))(X(4))(X(2))(X(3)), bn, orphans);
-  SymbolicFactorGraph factors(bn);
 
-  CHECK(orphans.size() == 0);
+  SymbolicBayesNetUnordered expectedBn = list_of
+    (SymbolicConditionalUnordered::FromKeys(list_of(L(5))(X(4)), 2))
+    (SymbolicConditionalUnordered(X(2), X(4)))
+    (SymbolicConditionalUnordered(X(3), X(2)));
+  EXPECT(assert_equal(expectedBn, bn));
+  EXPECT(orphans.empty());
+}
+
+/* ************************************************************************* */
+TEST( BayesTree, removeTop4 )
+{
+  SymbolicFactorGraphUnordered graph = list_of
+    (SymbolicFactorUnordered(L(5)))
+    (SymbolicFactorUnordered(X(4), L(5)))
+    (SymbolicFactorUnordered(X(2), X(4)))
+    (SymbolicFactorUnordered(X(3), X(2)));
+  SymbolicBayesTreeUnordered bayesTree = *graph.eliminateMultifrontal(
+    OrderingUnordered(list_of (X(3)) (X(2)) (X(4)) (L(5)) ));
+
+  // remove all
+  SymbolicBayesNetUnordered bn;
+  SymbolicBayesTreeUnordered::Cliques orphans;
+  bayesTree.removeTop(list_of(X(2))(L(5))(X(4))(X(3)), bn, orphans);
+
+  SymbolicBayesNetUnordered expectedBn = list_of
+    (SymbolicConditionalUnordered::FromKeys(list_of(L(5))(X(4)), 2))
+    (SymbolicConditionalUnordered(X(2), X(4)))
+    (SymbolicConditionalUnordered(X(3), X(2)));
+  EXPECT(assert_equal(expectedBn, bn));
+  EXPECT(orphans.empty());
 }
 
 /////* ************************************************************************* */