From 3118f6bb9f0aecdce3a38cd54a58eb25806857db Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Fri, 26 Jul 2013 01:10:16 +0000
Subject: [PATCH] JunctionTree keeps track of "problem size" at each node

---
 gtsam/inference/JunctionTreeUnordered-inst.h | 9 +++++++--
 gtsam/inference/JunctionTreeUnordered.h      | 3 +++
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/gtsam/inference/JunctionTreeUnordered-inst.h b/gtsam/inference/JunctionTreeUnordered-inst.h
index 1702634e3..b392d210c 100644
--- a/gtsam/inference/JunctionTreeUnordered-inst.h
+++ b/gtsam/inference/JunctionTreeUnordered-inst.h
@@ -96,6 +96,7 @@ namespace gtsam {
       size_t nrMergedChildren = 0;
       assert(myData.myJTNode->children.size() == myData.childSymbolicConditionals.size());
       // Loop over children
+      int combinedProblemSize = symbolicElimResult.first->size();
       for(size_t child = 0; child < myData.childSymbolicConditionals.size(); ++child) {
         // Check if we should merge the child
         if(myNrParents + 1 == myData.childSymbolicConditionals[child]->nrParents()) {
@@ -111,8 +112,11 @@ namespace gtsam {
           myData.myJTNode->children.erase(myData.myJTNode->children.begin() + child - nrMergedChildren);
           // Increment number of merged children
           ++ nrMergedChildren;
+          // Increment problem size
+          combinedProblemSize = std::max(combinedProblemSize, childToMerge.problemSize_);
         }
       }
+      myData.myJTNode->problemSize_ = combinedProblemSize;
     }
 
     /* ************************************************************************* */
@@ -263,8 +267,9 @@ namespace gtsam {
     // that contains all of the roots as its children.  rootsContainer also stores the remaining
     // uneliminated factors passed up from the roots.
     EliminationData<This> rootsContainer(0, roots_.size());
-    treeTraversal::DepthFirstForest(*this, rootsContainer, eliminationPreOrderVisitor<This>,
-      boost::bind(eliminationPostOrderVisitor<This>, _1, _2, function));
+    //tbb::task_scheduler_init init(1);
+    treeTraversal::DepthFirstForestParallel(*this, rootsContainer, eliminationPreOrderVisitor<This>,
+      boost::bind(eliminationPostOrderVisitor<This>, _1, _2, function), -10000);
 
     // Create BayesTree from roots stored in the dummy BayesTree node.
     boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
diff --git a/gtsam/inference/JunctionTreeUnordered.h b/gtsam/inference/JunctionTreeUnordered.h
index 015b605dc..ade95d335 100644
--- a/gtsam/inference/JunctionTreeUnordered.h
+++ b/gtsam/inference/JunctionTreeUnordered.h
@@ -68,6 +68,9 @@ namespace gtsam {
       Keys keys; ///< Frontal keys of this node
       Factors factors; ///< Factors associated with this node
       Children children; ///< sub-trees
+      int problemSize_;
+
+      int problemSize() const { return problemSize_; }
 
       /** print this node */
       void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;