diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt
index 77d415d86..8c93a84ef 100644
--- a/gtsam/CMakeLists.txt
+++ b/gtsam/CMakeLists.txt
@@ -50,11 +50,10 @@ install(FILES ${gtsam_core_headers} DESTINATION include/gtsam)
 # assemble core libaries
 foreach(subdir ${gtsam_subdirs})
     # Build convenience libraries
-    file(GLOB subdir_cpp_srcs "${subdir}/*.cpp")
-    file(GLOB subdir_headers "${subdir}/*.h")
-    list(REMOVE_ITEM subdir_cpp_srcs ${excluded_sources})
-    list(REMOVE_ITEM subdir_headers ${excluded_headers})
-    set(subdir_srcs ${subdir_cpp_srcs} ${subdir_headers}) # Include header files so they show up in Visual Studio
+    file(GLOB_RECURSE subdir_srcs "${subdir}/*.cpp" "${subdir}/*.h") # Include header files so they show up in Visual Studio
+    list(REMOVE_ITEM subdir_srcs ${excluded_sources})
+	file(GLOB subdir_test_files "${subdir}/tests/*")
+	list(REMOVE_ITEM subdir_srcs ${subdir_test_files}) # Remove test files from sources compiled into library
     gtsam_assign_source_folders("${subdir_srcs}") # Create MSVC structure
     set(${subdir}_srcs ${subdir_srcs})
 
diff --git a/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt
index a77c5cece..c5dd4fd56 100644
--- a/gtsam/base/CMakeLists.txt
+++ b/gtsam/base/CMakeLists.txt
@@ -1,5 +1,5 @@
 # Install headers
-file(GLOB base_headers "*.h")
+file(GLOB base_headers "*.h" "treeTraversal/*.h")
 install(FILES ${base_headers} DESTINATION include/gtsam/base)
 
 # Files to exclude from compilation of tests and timing scripts
diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h
index 9b3229d8e..e1db89e1b 100644
--- a/gtsam/base/treeTraversal-inst.h
+++ b/gtsam/base/treeTraversal-inst.h
@@ -16,6 +16,9 @@
 */
 #pragma once
 
+#include <gtsam/base/treeTraversal/parallelTraversalTasks.h>
+#include <gtsam/base/treeTraversal/statistics.h>
+
 #include <gtsam/base/FastList.h>
 #include <gtsam/base/FastVector.h>
 #include <gtsam/inference/Key.h>
@@ -28,13 +31,6 @@
 #include <boost/foreach.hpp>
 #include <boost/bind.hpp>
 
-#ifdef GTSAM_USE_TBB
-#  include <tbb/tbb.h>
-#  undef max // TBB seems to include windows.h and we don't want these macros
-#  undef min
-#  undef ERROR
-#endif
-
 namespace gtsam {
 
   /** Internal functions used for traversing trees */
@@ -59,151 +55,6 @@ namespace gtsam {
         void operator()(const boost::shared_ptr<NODE>& node, const DATA& data) {}
       };
 
-
-#ifdef GTSAM_USE_TBB
-
-      /* ************************************************************************* */
-      template<typename NODE, typename DATA, typename VISITOR_POST>
-      class PostOrderTask : public tbb::task
-      {
-      public:
-        const boost::shared_ptr<NODE>& treeNode;
-        boost::shared_ptr<DATA> myData;
-        VISITOR_POST& visitorPost;
-
-        PostOrderTask(const boost::shared_ptr<NODE>& treeNode, const boost::shared_ptr<DATA>& myData, VISITOR_POST& visitorPost) :
-          treeNode(treeNode), myData(myData), visitorPost(visitorPost) {}
-
-        tbb::task* execute()
-        {
-          // Run the post-order visitor
-          (void) visitorPost(treeNode, *myData);
-
-          return NULL;
-        }
-      };
-
-      /* ************************************************************************* */
-      template<typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      class PreOrderTask : public tbb::task
-      {
-      public:
-        const boost::shared_ptr<NODE>& treeNode;
-        boost::shared_ptr<DATA> myData;
-        VISITOR_PRE& visitorPre;
-        VISITOR_POST& visitorPost;
-        int problemSizeThreshold;
-        bool makeNewTasks;
-
-        PreOrderTask(const boost::shared_ptr<NODE>& treeNode, const boost::shared_ptr<DATA>& myData,
-          VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold,
-          bool makeNewTasks = true) :
-          treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
-          problemSizeThreshold(problemSizeThreshold), makeNewTasks(makeNewTasks) {}
-
-        tbb::task* execute()
-        {
-          if (makeNewTasks)
-          {
-            if(!treeNode->children.empty())
-            {
-              // Allocate post-order task as a continuation
-              PostOrderTask<NODE, DATA, VISITOR_POST>& postOrderTask =
-                *new(allocate_continuation()) PostOrderTask<NODE, DATA, VISITOR_POST>(treeNode, myData, visitorPost);
-
-              bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold);
-
-              tbb::task_list childTasks;
-              BOOST_FOREACH(const boost::shared_ptr<NODE>& child, treeNode->children)
-              {
-                // Process child in a subtask.  Important:  Run visitorPre before calling
-                // allocate_child so that if visitorPre throws an exception, we will not have
-                // allocated an extra child, this causes a TBB error.
-                boost::shared_ptr<DATA> childData = boost::allocate_shared<DATA>(tbb::scalable_allocator<DATA>(), visitorPre(child, *myData));
-                childTasks.push_back(*new(postOrderTask.allocate_child())
-                  PreOrderTask(child, childData, visitorPre, visitorPost,
-                  problemSizeThreshold, overThreshold));
-              }
-
-              // If we have child tasks, start subtasks and wait for them to complete
-              postOrderTask.set_ref_count((int) treeNode->children.size());
-              spawn(childTasks);
-            }
-            else
-            {
-              // Run the post-order visitor in this task if we have no children
-              (void) visitorPost(treeNode, *myData);
-            }
-          }
-          else
-          {
-            // Process this node and its children in this task
-            processNodeRecursively(treeNode, *myData);
-          }
-
-          // Return NULL
-          return NULL;
-        }
-
-        void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData)
-        {
-          BOOST_FOREACH(const boost::shared_ptr<NODE>& child, node->children)
-          {
-            DATA childData = visitorPre(child, myData);
-            processNodeRecursively(child, childData);
-          }
-
-          // Run the post-order visitor
-          (void) visitorPost(node, myData);
-        }
-      };
-
-      /* ************************************************************************* */
-      template<typename ROOTS, typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      class RootTask : public tbb::task
-      {
-      public:
-        const ROOTS& roots;
-        DATA& myData;
-        VISITOR_PRE& visitorPre;
-        VISITOR_POST& visitorPost;
-        int problemSizeThreshold;
-        RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
-          int problemSizeThreshold) :
-          roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
-            problemSizeThreshold(problemSizeThreshold) {}
-
-        tbb::task* execute()
-        {
-          typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
-          // Create data and tasks for our children
-          tbb::task_list tasks;
-          BOOST_FOREACH(const boost::shared_ptr<NODE>& root, roots)
-          {
-            boost::shared_ptr<DATA> rootData = boost::allocate_shared<DATA>(tbb::scalable_allocator<DATA>(), visitorPre(root, myData));
-            tasks.push_back(*new(allocate_child())
-              PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold));
-          }
-          // Set TBB ref count
-          set_ref_count(1 + (int)roots.size());
-          // Spawn tasks
-          spawn_and_wait_for_all(tasks);
-          // Return NULL
-          return NULL;
-        }
-      };
-
-      template<typename NODE, typename ROOTS, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
-      RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST>&
-        CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
-        int problemSizeThreshold)
-      {
-        typedef RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST> RootTask;
-        return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold);
-      }
-
-#endif
-
     }
 
     /** Traverse a forest depth-first with pre-order and post-order visits.
@@ -306,7 +157,7 @@ namespace gtsam {
       typedef typename FOREST::Node Node;
       typedef boost::shared_ptr<Node> sharedNode;
 
-      tbb::task::spawn_root_and_wait(CreateRootTask<Node>(
+      tbb::task::spawn_root_and_wait(internal::CreateRootTask<Node>(
         forest.roots(), rootData, visitorPre, visitorPost, problemSizeThreshold));
 #else
       DepthFirstForest(forest, rootData, visitorPre, visitorPost);
diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h
new file mode 100644
index 000000000..7e294643a
--- /dev/null
+++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h
@@ -0,0 +1,205 @@
+/* ----------------------------------------------------------------------------
+
+* GTSAM Copyright 2010, Georgia Tech Research Corporation,
+* Atlanta, Georgia 30332-0415
+* All Rights Reserved
+* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+* See LICENSE for the license information
+
+* -------------------------------------------------------------------------- */
+
+/**
+* @file    parallelTraversalTasks.h
+* @author  Richard Roberts
+* @date    April 9, 2013
+*/
+#pragma once
+
+#include <gtsam/global_includes.h>
+
+#include <boost/shared_ptr.hpp>
+#include <boost/make_shared.hpp>
+#include <boost/foreach.hpp>
+
+#ifdef GTSAM_USE_TBB
+#  include <tbb/tbb.h>
+#  undef max // TBB seems to include windows.h and we don't want these macros
+#  undef min
+#  undef ERROR
+
+namespace gtsam {
+
+  /** Internal functions used for traversing trees */
+  namespace treeTraversal {
+
+    namespace internal {
+
+      /* ************************************************************************* */
+      template<typename NODE, typename DATA, typename VISITOR_POST>
+      class PostOrderTask : public tbb::task
+      {
+      public:
+        const boost::shared_ptr<NODE>& treeNode;
+        boost::shared_ptr<DATA> myData;
+        VISITOR_POST& visitorPost;
+
+        PostOrderTask(const boost::shared_ptr<NODE>& treeNode, const boost::shared_ptr<DATA>& myData, VISITOR_POST& visitorPost) :
+          treeNode(treeNode), myData(myData), visitorPost(visitorPost) {}
+
+        tbb::task* execute()
+        {
+          // Run the post-order visitor
+          (void) visitorPost(treeNode, *myData);
+
+          return NULL;
+        }
+      };
+
+      /* ************************************************************************* */
+      template<typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
+      class PreOrderTask : public tbb::task
+      {
+      public:
+        const boost::shared_ptr<NODE>& treeNode;
+        boost::shared_ptr<DATA> myData;
+        VISITOR_PRE& visitorPre;
+        VISITOR_POST& visitorPost;
+        int problemSizeThreshold;
+        bool makeNewTasks;
+
+        bool isPostOrderPhase;
+
+        PreOrderTask(const boost::shared_ptr<NODE>& treeNode, const boost::shared_ptr<DATA>& myData,
+          VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold,
+          bool makeNewTasks = true) :
+          treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
+          problemSizeThreshold(problemSizeThreshold), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {}
+
+        tbb::task* execute()
+        {
+          if(isPostOrderPhase)
+          {
+            // Run the post-order visitor since this task was recycled to run the post-order visitor
+            (void) visitorPost(treeNode, *myData);
+            return NULL;
+          }
+          else
+          {
+            if(makeNewTasks)
+            {
+              if(!treeNode->children.empty())
+              {
+                // Allocate post-order task as a continuation
+                isPostOrderPhase = true;
+                recycle_as_continuation();
+                //PostOrderTask<NODE, DATA, VISITOR_POST>& postOrderTask =
+                //  *new(allocate_continuation()) PostOrderTask<NODE, DATA, VISITOR_POST>(treeNode, myData, visitorPost);
+
+                bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold);
+
+                tbb::task* firstChild = 0;
+                tbb::task_list childTasks;
+                BOOST_FOREACH(const boost::shared_ptr<NODE>& child, treeNode->children)
+                {
+                  // Process child in a subtask.  Important:  Run visitorPre before calling
+                  // allocate_child so that if visitorPre throws an exception, we will not have
+                  // allocated an extra child, this causes a TBB error.
+                  boost::shared_ptr<DATA> childData = boost::allocate_shared<DATA>(tbb::scalable_allocator<DATA>(), visitorPre(child, *myData));
+                  //childTasks.push_back(*new(postOrderTask.allocate_child())
+                  //  PreOrderTask(child, childData, visitorPre, visitorPost,
+                  //  problemSizeThreshold, overThreshold));
+                  tbb::task* childTask = new(allocate_child())
+                    PreOrderTask(child, childData, visitorPre, visitorPost,
+                    problemSizeThreshold, overThreshold);
+                  if(firstChild)
+                    childTasks.push_back(*childTask);
+                  else
+                    firstChild = childTask;
+                }
+
+                // If we have child tasks, start subtasks and wait for them to complete
+                //postOrderTask.set_ref_count((int) treeNode->children.size());
+                set_ref_count(treeNode->children.size());
+                spawn(childTasks);
+                return firstChild;
+              }
+              else
+              {
+                // Run the post-order visitor in this task if we have no children
+                (void) visitorPost(treeNode, *myData);
+                return NULL;
+              }
+            }
+            else
+            {
+              // Process this node and its children in this task
+              processNodeRecursively(treeNode, *myData);
+              return NULL;
+            }
+          }
+        }
+
+        void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData)
+        {
+          BOOST_FOREACH(const boost::shared_ptr<NODE>& child, node->children)
+          {
+            DATA childData = visitorPre(child, myData);
+            processNodeRecursively(child, childData);
+          }
+
+          // Run the post-order visitor
+          (void) visitorPost(node, myData);
+        }
+      };
+
+      /* ************************************************************************* */
+      template<typename ROOTS, typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
+      class RootTask : public tbb::task
+      {
+      public:
+        const ROOTS& roots;
+        DATA& myData;
+        VISITOR_PRE& visitorPre;
+        VISITOR_POST& visitorPost;
+        int problemSizeThreshold;
+        RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
+          int problemSizeThreshold) :
+          roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
+          problemSizeThreshold(problemSizeThreshold) {}
+
+        tbb::task* execute()
+        {
+          typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
+          // Create data and tasks for our children
+          tbb::task_list tasks;
+          BOOST_FOREACH(const boost::shared_ptr<NODE>& root, roots)
+          {
+            boost::shared_ptr<DATA> rootData = boost::allocate_shared<DATA>(tbb::scalable_allocator<DATA>(), visitorPre(root, myData));
+            tasks.push_back(*new(allocate_child())
+              PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold));
+          }
+          // Set TBB ref count
+          set_ref_count(1 + (int) roots.size());
+          // Spawn tasks
+          spawn_and_wait_for_all(tasks);
+          // Return NULL
+          return NULL;
+        }
+      };
+
+      template<typename NODE, typename ROOTS, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
+      RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST>&
+        CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
+      {
+          typedef RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST> RootTask;
+          return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold);
+        }
+
+    }
+
+  }
+
+}
+
+#endif
diff --git a/gtsam/base/treeTraversal/statistics.h b/gtsam/base/treeTraversal/statistics.h
new file mode 100644
index 000000000..805c69758
--- /dev/null
+++ b/gtsam/base/treeTraversal/statistics.h
@@ -0,0 +1,95 @@
+/* ----------------------------------------------------------------------------
+
+* GTSAM Copyright 2010, Georgia Tech Research Corporation,
+* Atlanta, Georgia 30332-0415
+* All Rights Reserved
+* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+* See LICENSE for the license information
+
+* -------------------------------------------------------------------------- */
+
+/**
+* @file    statistics.h
+* @brief   Tools for gathering statistics about a forest to aid tuning parallel traversal
+* @author  Richard Roberts
+* @date    April 9, 2013
+*/
+#pragma once
+
+#include <gtsam/global_includes.h>
+#include <gtsam/base/FastMap.h>
+
+#include <ostream>
+
+namespace gtsam {
+
+  namespace treeTraversal {
+
+    /* ************************************************************************* */
+    /// Struct to store gathered statistics about a forest
+    struct ForestStatistics
+    {
+      typedef FastMap<int, ValueWithDefault<int, 0> > Histogram;
+      Histogram problemSizeHistogram;
+      Histogram numberOfChildrenHistogram;
+      Histogram problemSizeOfSecondLargestChildHistogram;
+
+      static void Write(std::ostream& outStream, const Histogram& histogram)
+      {
+        if (!histogram.empty())
+        {
+          Histogram::const_iterator endIt = histogram.end();
+          -- endIt;
+          const int largest = endIt->first;
+          for (int bin = 0; bin <= largest; ++bin)
+          {
+            Histogram::const_iterator item = histogram.find(bin);
+            const int count = (item == histogram.end() ? 0 : *item->second);
+            outStream << bin << " " << count << "\n";
+          }
+        }
+      }
+    };
+
+    /* ************************************************************************* */
+    namespace internal {
+      template<class NODE>
+      ForestStatistics* statisticsVisitor(const boost::shared_ptr<NODE>& node, ForestStatistics* stats)
+      {
+        (*stats->problemSizeHistogram[node->problemSize()]) ++;
+        (*stats->numberOfChildrenHistogram[(int)node->children.size()]) ++;
+        if (node->children.size() > 1)
+        {
+          int largestProblemSize = 0;
+          int secondLargestProblemSize = 0;
+          BOOST_FOREACH(const boost::shared_ptr<NODE>& child, node->children)
+          {
+            if (child->problemSize() > largestProblemSize)
+            {
+              secondLargestProblemSize = largestProblemSize;
+              largestProblemSize = child->problemSize();
+            }
+            else if (child->problemSize() > secondLargestProblemSize)
+            {
+              secondLargestProblemSize = child->problemSize();
+            }
+          }
+          (*stats->problemSizeOfSecondLargestChildHistogram[secondLargestProblemSize]) ++;
+        }
+        return stats;
+      }
+    }
+
+    /* ************************************************************************* */
+    template<class FOREST>
+    ForestStatistics GatherStatistics(const FOREST& forest)
+    {
+      ForestStatistics stats;
+      ForestStatistics* statsPtr = &stats;
+      DepthFirstForest(forest, statsPtr, internal::statisticsVisitor<typename FOREST::Node>);
+      return stats;
+    }
+
+  }
+}