Merge branch 'jtree_mania' into feature/CeresDefaults-2

gtsam/base/treeTraversal-inst.h

@@ -46,13 +46,15 @@ namespace gtsam {
   DATA& parentData;
   typename FastList<DATA>::iterator dataPointer;
   TraversalNode(const boost::shared_ptr<NODE>& _treeNode, DATA& _parentData) :
-          expanded(false), treeNode(_treeNode), parentData(_parentData) {}
+      expanded(false), treeNode(_treeNode), parentData(_parentData) {
+  }
 };
 
 // Do nothing - default argument for post-visitor for tree traversal
 struct no_op {
   template<typename NODE, typename DATA>
-        void operator()(const boost::shared_ptr<NODE>& node, const DATA& data) {}
+  void operator()(const boost::shared_ptr<NODE>& node, const DATA& data) {
+  }
 };
 
 }
@@ -71,9 +73,10 @@ namespace gtsam {
  *         call to \c visitorPre (the \c DATA object may be modified by visiting the children).
  *  @param rootData The data to pass by reference to \c visitorPre when it is called on each
  *         root node. */
-    template<class FOREST, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
+template<class FOREST, typename DATA, typename VISITOR_PRE,
-    void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost)
+    typename VISITOR_POST>
-    {
+void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre,
+    VISITOR_POST& visitorPost) {
   // Typedefs
   typedef typename FOREST::Node Node;
   typedef boost::shared_ptr<Node> sharedNode;
@@ -92,8 +95,7 @@ namespace gtsam {
   }
 
   // Traverse
-      while(!stack.empty())
+  while (!stack.empty()) {
-      {
     // Get next node
     TraversalNode& node = stack.front();
 
@@ -106,7 +108,8 @@ namespace gtsam {
     } else {
       // If not already visited, visit the node and add its children (use reverse iterators so
       // children are processed in the order they appear)
-          node.dataPointer = dataList.insert(dataList.end(), visitorPre(node.treeNode, node.parentData));
+      node.dataPointer = dataList.insert(dataList.end(),
+          visitorPre(node.treeNode, node.parentData));
       typename Stack::iterator insertLocation = stack.begin();
       BOOST_FOREACH(const sharedNode& child, node.treeNode->children)
         stack.insert(insertLocation, TraversalNode(child, *node.dataPointer));
@@ -128,8 +131,7 @@ namespace gtsam {
  *  @param rootData The data to pass by reference to \c visitorPre when it is called on each
  *         root node. */
 template<class FOREST, typename DATA, typename VISITOR_PRE>
-    void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre)
+void DepthFirstForest(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre) {
-    {
   no_op visitorPost;
   DepthFirstForest(forest, rootData, visitorPre, visitorPost);
 }
@@ -148,30 +150,31 @@ namespace gtsam {
  *         call to \c visitorPre (the \c DATA object may be modified by visiting the children).
  *  @param rootData The data to pass by reference to \c visitorPre when it is called on each
  *         root node. */
-    template<class FOREST, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
+template<class FOREST, typename DATA, typename VISITOR_PRE,
-    void DepthFirstForestParallel(FOREST& forest, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
+    typename VISITOR_POST>
-      int problemSizeThreshold = 10)
+void DepthFirstForestParallel(FOREST& forest, DATA& rootData,
-    {
+    VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
+    int problemSizeThreshold = 10) {
 #ifdef GTSAM_USE_TBB
   // Typedefs
   typedef typename FOREST::Node Node;
   typedef boost::shared_ptr<Node> sharedNode;
 
-      tbb::task::spawn_root_and_wait(internal::CreateRootTask<Node>(
+  tbb::task::spawn_root_and_wait(
-        forest.roots(), rootData, visitorPre, visitorPost, problemSizeThreshold));
+      internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
+          visitorPost, problemSizeThreshold));
 #else
   DepthFirstForest(forest, rootData, visitorPre, visitorPost);
 #endif
 }
 
-
 /* ************************************************************************* */
 /** Traversal function for CloneForest */
 namespace {
 template<typename NODE>
-      boost::shared_ptr<NODE>
+boost::shared_ptr<NODE> CloneForestVisitorPre(
-        CloneForestVisitorPre(const boost::shared_ptr<NODE>& node, const boost::shared_ptr<NODE>& parentPointer)
+    const boost::shared_ptr<NODE>& node,
-      {
+    const boost::shared_ptr<NODE>& parentPointer) {
   // Clone the current node and add it to its cloned parent
   boost::shared_ptr<NODE> clone = boost::make_shared<NODE>(*node);
   clone->children.clear();
@@ -186,24 +189,25 @@ namespace gtsam {
  *         return a collection of shared pointers to \c FOREST::Node.
  *  @return The new collection of roots. */
 template<class FOREST>
-    FastVector<boost::shared_ptr<typename FOREST::Node> > CloneForest(const FOREST& forest)
+FastVector<boost::shared_ptr<typename FOREST::Node> > CloneForest(
-    {
+    const FOREST& forest) {
   typedef typename FOREST::Node Node;
   boost::shared_ptr<Node> rootContainer = boost::make_shared<Node>();
   DepthFirstForest(forest, rootContainer, CloneForestVisitorPre<Node>);
-      return FastVector<boost::shared_ptr<Node> >(rootContainer->children.begin(), rootContainer->children.end());
+  return FastVector<boost::shared_ptr<Node> >(rootContainer->children.begin(),
+      rootContainer->children.end());
 }
 
-
 /* ************************************************************************* */
 /** Traversal function for PrintForest */
 namespace {
-      struct PrintForestVisitorPre
+struct PrintForestVisitorPre {
-      {
   const KeyFormatter& formatter;
-        PrintForestVisitorPre(const KeyFormatter& formatter) : formatter(formatter) {}
+  PrintForestVisitorPre(const KeyFormatter& formatter) :
-        template<typename NODE> std::string operator()(const boost::shared_ptr<NODE>& node, const std::string& parentString)
+      formatter(formatter) {
-        {
+  }
+  template<typename NODE> std::string operator()(
+      const boost::shared_ptr<NODE>& node, const std::string& parentString) {
     // Print the current node
     node->print(parentString + "-", formatter);
     // Increment the indentation
@@ -215,7 +219,8 @@ namespace gtsam {
 /** Print a tree, prefixing each line with \c str, and formatting keys using \c keyFormatter.
  *  To print each node, this function calls the \c print function of the tree nodes. */
 template<class FOREST>
-    void PrintForest(const FOREST& forest, std::string str, const KeyFormatter& keyFormatter) {
+void PrintForest(const FOREST& forest, std::string str,
+    const KeyFormatter& keyFormatter) {
   PrintForestVisitorPre visitor(keyFormatter);
   DepthFirstForest(forest, str, visitor);
 }

gtsam/inference/ClusterTree-inst.h

@@ -7,36 +7,39 @@
  * @brief Collects factorgraph fragments defined on variable clusters, arranged in a tree
  */
 
-#include <gtsam/base/timing.h>
-#include <gtsam/base/treeTraversal-inst.h>
 #include <gtsam/inference/ClusterTree.h>
 #include <gtsam/inference/BayesTree.h>
 #include <gtsam/inference/Ordering.h>
+#include <gtsam/base/timing.h>
+#include <gtsam/base/treeTraversal-inst.h>
 
 #include <boost/foreach.hpp>
 #include <boost/bind.hpp>
 
-namespace gtsam
+namespace gtsam {
-{
+
-  namespace
-  {
 /* ************************************************************************* */
 // Elimination traversal data - stores a pointer to the parent data and collects the factors
 // resulting from elimination of the children.  Also sets up BayesTree cliques with parent and
 // child pointers.
 template<class CLUSTERTREE>
 struct EliminationData {
+  // Typedefs
+  typedef typename CLUSTERTREE::sharedFactor sharedFactor;
+  typedef typename CLUSTERTREE::FactorType FactorType;
+  typedef typename CLUSTERTREE::FactorGraphType FactorGraphType;
+  typedef typename CLUSTERTREE::ConditionalType ConditionalType;
+  typedef typename CLUSTERTREE::BayesTreeType::Node BTNode;
+
   EliminationData* const parentData;
   size_t myIndexInParent;
-      FastVector<typename CLUSTERTREE::sharedFactor> childFactors;
+  FastVector<sharedFactor> childFactors;
-      boost::shared_ptr<typename CLUSTERTREE::BayesTreeType::Node> bayesTreeNode;
+  boost::shared_ptr<BTNode> bayesTreeNode;
   EliminationData(EliminationData* _parentData, size_t nChildren) :
-        parentData(_parentData),
+      parentData(_parentData), bayesTreeNode(boost::make_shared<BTNode>()) {
-        bayesTreeNode(boost::make_shared<typename CLUSTERTREE::BayesTreeType::Node>())
-      {
     if (parentData) {
       myIndexInParent = parentData->childFactors.size();
-          parentData->childFactors.push_back(typename CLUSTERTREE::sharedFactor());
+      parentData->childFactors.push_back(sharedFactor());
     } else {
       myIndexInParent = 0;
     }
@@ -47,39 +50,30 @@ namespace gtsam
       parentData->bayesTreeNode->children.push_back(bayesTreeNode);
     }
   }
-    };
 
-    /* ************************************************************************* */
+  // Elimination pre-order visitor - creates the EliminationData structure for the visited node.
-    // Elimination pre-order visitor - just creates the EliminationData structure for the visited
+  static EliminationData EliminationPreOrderVisitor(
-    // node.
+      const typename CLUSTERTREE::sharedNode& node,
-    template<class CLUSTERTREE>
+      EliminationData& parentData) {
-    EliminationData<CLUSTERTREE> eliminationPreOrderVisitor(
+    assert(node);
-      const typename CLUSTERTREE::sharedNode& node, EliminationData<CLUSTERTREE>& parentData)
+    EliminationData myData(&parentData, node->children.size());
-    {
-      EliminationData<CLUSTERTREE> myData(&parentData, node->children.size());
     myData.bayesTreeNode->problemSize_ = node->problemSize();
     return myData;
   }
 
-    /* ************************************************************************* */
   // Elimination post-order visitor - combine the child factors with our own factors, add the
   // resulting conditional to the BayesTree, and add the remaining factor to the parent.
-    template<class CLUSTERTREE>
+  struct EliminationPostOrderVisitor {
-    struct EliminationPostOrderVisitor
-    {
     const typename CLUSTERTREE::Eliminate& eliminationFunction;
     typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex;
-      EliminationPostOrderVisitor(const typename CLUSTERTREE::Eliminate& eliminationFunction,
+    EliminationPostOrderVisitor(
+        const typename CLUSTERTREE::Eliminate& eliminationFunction,
         typename CLUSTERTREE::BayesTreeType::Nodes& nodesIndex) :
-      eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {}
+        eliminationFunction(eliminationFunction), nodesIndex(nodesIndex) {
-      void operator()(const typename CLUSTERTREE::sharedNode& node, EliminationData<CLUSTERTREE>& myData)
+    }
-      {
+    void operator()(const typename CLUSTERTREE::sharedNode& node,
-        // Typedefs
+        EliminationData& myData) {
-        typedef typename CLUSTERTREE::sharedFactor sharedFactor;
+      assert(node);
-        typedef typename CLUSTERTREE::FactorType FactorType;
-        typedef typename CLUSTERTREE::FactorGraphType FactorGraphType;
-        typedef typename CLUSTERTREE::ConditionalType ConditionalType;
-        typedef typename CLUSTERTREE::BayesTreeType::Node BTNode;
 
       // Gather factors
       FactorGraphType gatheredFactors;
@@ -88,17 +82,17 @@ namespace gtsam
       gatheredFactors += myData.childFactors;
 
       // Check for Bayes tree orphan subtrees, and add them to our children
-        BOOST_FOREACH(const sharedFactor& f, node->factors)
+      BOOST_FOREACH(const sharedFactor& f, node->factors) {
-        {
+        if (const BayesTreeOrphanWrapper<BTNode>* asSubtree =
-          if(const BayesTreeOrphanWrapper<BTNode>* asSubtree = dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get()))
+            dynamic_cast<const BayesTreeOrphanWrapper<BTNode>*>(f.get())) {
-          {
           myData.bayesTreeNode->children.push_back(asSubtree->clique);
           asSubtree->clique->parent_ = myData.bayesTreeNode;
         }
       }
 
       // Do dense elimination step
-        std::pair<boost::shared_ptr<ConditionalType>, boost::shared_ptr<FactorType> > eliminationResult =
+      std::pair<boost::shared_ptr<ConditionalType>,
+          boost::shared_ptr<FactorType> > eliminationResult =
           eliminationFunction(gatheredFactors, node->orderedFrontalKeys);
 
       // Store conditional in BayesTree clique, and in the case of ISAM2Clique also store the remaining factor
@@ -112,32 +106,82 @@ namespace gtsam
 
       // Store remaining factor in parent's gathered factors
       if (!eliminationResult.second->empty())
-          myData.parentData->childFactors[myData.myIndexInParent] = eliminationResult.second;
+        myData.parentData->childFactors[myData.myIndexInParent] =
+            eliminationResult.second;
     }
   };
-  }
+};
 
 /* ************************************************************************* */
 template<class BAYESTREE, class GRAPH>
-  void ClusterTree<BAYESTREE,GRAPH>::Cluster::print(
+void ClusterTree<BAYESTREE, GRAPH>::Cluster::print(const std::string& s,
-    const std::string& s, const KeyFormatter& keyFormatter) const
+    const KeyFormatter& keyFormatter) const {
-  {
   std::cout << s << " (" << problemSize_ << ")";
   PrintKeyVector(orderedFrontalKeys);
 }
 
 /* ************************************************************************* */
 template<class BAYESTREE, class GRAPH>
-  void ClusterTree<BAYESTREE,GRAPH>::print(
+void ClusterTree<BAYESTREE, GRAPH>::Cluster::mergeChildren(
-    const std::string& s, const KeyFormatter& keyFormatter) const
+    const std::vector<bool>& merge) {
-  {
+  gttic(Cluster::mergeChildren);
+
+  // Count how many keys, factors and children we'll end up with
+  size_t nrKeys = orderedFrontalKeys.size();
+  size_t nrFactors = factors.size();
+  size_t nrNewChildren = 0;
+  // Loop over children
+  size_t i = 0;
+  BOOST_FOREACH(const sharedNode& child, children) {
+    if (merge[i]) {
+      nrKeys += child->orderedFrontalKeys.size();
+      nrFactors += child->factors.size();
+      nrNewChildren += child->children.size();
+    } else {
+      nrNewChildren += 1; // we keep the child
+    }
+    ++i;
+  }
+
+  // now reserve space, and really merge
+  orderedFrontalKeys.reserve(nrKeys);
+  factors.reserve(nrFactors);
+  typename Node::Children newChildren;
+  newChildren.reserve(nrNewChildren);
+  i = 0;
+  BOOST_FOREACH(const sharedNode& child, children) {
+    if (merge[i]) {
+      // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after..
+      orderedFrontalKeys.insert(orderedFrontalKeys.end(),
+          child->orderedFrontalKeys.rbegin(), child->orderedFrontalKeys.rend());
+      // Merge keys, factors, and children.
+      factors.insert(factors.end(), child->factors.begin(),
+          child->factors.end());
+      newChildren.insert(newChildren.end(), child->children.begin(),
+          child->children.end());
+      // Increment problem size
+      problemSize_ = std::max(problemSize_, child->problemSize_);
+      // Increment number of frontal variables
+    } else {
+      newChildren.push_back(child); // we keep the child
+    }
+    ++i;
+  }
+  children = newChildren;
+  std::reverse(orderedFrontalKeys.begin(), orderedFrontalKeys.end());
+}
+
+/* ************************************************************************* */
+template<class BAYESTREE, class GRAPH>
+void ClusterTree<BAYESTREE, GRAPH>::print(const std::string& s,
+    const KeyFormatter& keyFormatter) const {
   treeTraversal::PrintForest(*this, s, keyFormatter);
 }
 
 /* ************************************************************************* */
 template<class BAYESTREE, class GRAPH>
-  ClusterTree<BAYESTREE,GRAPH>& ClusterTree<BAYESTREE,GRAPH>::operator=(const This& other)
+ClusterTree<BAYESTREE, GRAPH>& ClusterTree<BAYESTREE, GRAPH>::operator=(
-  {
+    const This& other) {
   // Start by duplicating the tree.
   roots_ = treeTraversal::CloneForest(other);
 
@@ -150,35 +194,40 @@ namespace gtsam
 
 /* ************************************************************************* */
 template<class BAYESTREE, class GRAPH>
-  std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> >
+std::pair<boost::shared_ptr<BAYESTREE>, boost::shared_ptr<GRAPH> > ClusterTree<
-  ClusterTree<BAYESTREE,GRAPH>::eliminate(const Eliminate& function) const
+    BAYESTREE, GRAPH>::eliminate(const Eliminate& function) const {
-  {
   gttic(ClusterTree_eliminate);
   // Do elimination (depth-first traversal).  The rootsContainer stores a 'dummy' BayesTree node
   // that contains all of the roots as its children.  rootsContainer also stores the remaining
   // uneliminated factors passed up from the roots.
   boost::shared_ptr<BayesTreeType> result = boost::make_shared<BayesTreeType>();
-    EliminationData<This> rootsContainer(0, roots_.size());
+  typedef EliminationData<This> Data;
-    EliminationPostOrderVisitor<This> visitorPost(function, result->nodes_);
+  Data rootsContainer(0, roots_.size());
+  typename Data::EliminationPostOrderVisitor visitorPost(function,
+      result->nodes_);
   {
     TbbOpenMPMixedScope threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP
     treeTraversal::DepthFirstForestParallel(*this, rootsContainer,
-        eliminationPreOrderVisitor<This>, visitorPost, 10);
+        Data::EliminationPreOrderVisitor, visitorPost, 10);
   }
 
   // Create BayesTree from roots stored in the dummy BayesTree node.
-    result->roots_.insert(result->roots_.end(), rootsContainer.bayesTreeNode->children.begin(), rootsContainer.bayesTreeNode->children.end());
+  result->roots_.insert(result->roots_.end(),
+      rootsContainer.bayesTreeNode->children.begin(),
+      rootsContainer.bayesTreeNode->children.end());
 
   // Add remaining factors that were not involved with eliminated variables
-    boost::shared_ptr<FactorGraphType> allRemainingFactors = boost::make_shared<FactorGraphType>();
+  boost::shared_ptr<FactorGraphType> remaining = boost::make_shared<
-    allRemainingFactors->reserve(remainingFactors_.size() + rootsContainer.childFactors.size());
+      FactorGraphType>();
-    allRemainingFactors->push_back(remainingFactors_.begin(), remainingFactors_.end());
+  remaining->reserve(
-    BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors)
+      remainingFactors_.size() + rootsContainer.childFactors.size());
+  remaining->push_back(remainingFactors_.begin(), remainingFactors_.end());
+  BOOST_FOREACH(const sharedFactor& factor, rootsContainer.childFactors) {
     if (factor)
-        allRemainingFactors->push_back(factor);
+      remaining->push_back(factor);
-
+  }
   // Return result
-    return std::make_pair(result, allRemainingFactors);
+  return std::make_pair(result, remaining);
 }
 
 }

gtsam/inference/ClusterTree.h

@@ -13,8 +13,7 @@
 #include <gtsam/base/FastVector.h>
 #include <gtsam/inference/Ordering.h>
 
-namespace gtsam
+namespace gtsam {
-{
 
 /**
  * A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman:
@@ -23,8 +22,7 @@ namespace gtsam
  * \nosubgrouping
  */
 template<class BAYESTREE, class GRAPH>
-  class ClusterTree
+class ClusterTree {
-  {
 public:
   typedef GRAPH FactorGraphType; ///< The factor graph type
   typedef typename GRAPH::FactorType FactorType; ///< The type of factors
@@ -41,8 +39,10 @@ namespace gtsam
     typedef FastVector<sharedFactor> Factors;
     typedef FastVector<boost::shared_ptr<Cluster> > Children;
 
-      Cluster() {}
+    Cluster() {
-      Cluster(Key key, const Factors& factors) : factors(factors) {
+    }
+    Cluster(Key key, const Factors& factors) :
+        factors(factors) {
       orderedFrontalKeys.push_back(key);
     }
 
@@ -51,10 +51,16 @@ namespace gtsam
     Children children; ///< sub-trees
     int problemSize_;
 
-      int problemSize() const { return problemSize_; }
+    int problemSize() const {
+      return problemSize_;
+    }
 
-      /** print this node */
+    /// print this node
-      void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+    void print(const std::string& s = "", const KeyFormatter& keyFormatter =
+        DefaultKeyFormatter) const;
+
+    /// Merge all children for which bit is set into this node
+    void mergeChildren(const std::vector<bool>& merge);
   };
 
   typedef boost::shared_ptr<Cluster> sharedCluster; ///< Shared pointer to Cluster
@@ -124,7 +130,5 @@ namespace gtsam
 
 };
 
-
 }
 
-

gtsam/inference/JunctionTree-inst.h

@@ -37,8 +37,13 @@ struct ConstructorTraversalData {
   FastVector<SymbolicConditional::shared_ptr> childSymbolicConditionals;
   FastVector<SymbolicFactor::shared_ptr> childSymbolicFactors;
 
-  ConstructorTraversalData(ConstructorTraversalData* _parentData)
+  // Small inner class to store symbolic factors
-      : parentData(_parentData) {}
+  class SymbolicFactors: public FactorGraph<Factor> {
+  };
+
+  ConstructorTraversalData(ConstructorTraversalData* _parentData) :
+      parentData(_parentData) {
+  }
 
   // Pre-order visitor function
   static ConstructorTraversalData ConstructorTraversalVisitorPre(
@@ -64,13 +69,12 @@ struct ConstructorTraversalData {
     // our number of symbolic elimination parents is exactly 1 less than
     // our child's symbolic elimination parents - this condition indicates that
     // eliminating the current node did not introduce any parents beyond those
-    // already in the child.
+    // already in the child->
 
     // Do symbolic elimination for this node
-    class : public FactorGraph<Factor> {}
+    SymbolicFactors symbolicFactors;
-    symbolicFactors;
+    symbolicFactors.reserve(
-    symbolicFactors.reserve(ETreeNode->factors.size() +
+        ETreeNode->factors.size() + myData.childSymbolicFactors.size());
-                            myData.childSymbolicFactors.size());
     // Add ETree node factors
     symbolicFactors += ETreeNode->factors;
     // Add symbolic factors passed up from children
@@ -78,54 +82,41 @@ struct ConstructorTraversalData {
 
     Ordering keyAsOrdering;
     keyAsOrdering.push_back(ETreeNode->key);
-    std::pair<SymbolicConditional::shared_ptr, SymbolicFactor::shared_ptr>
+    SymbolicConditional::shared_ptr myConditional;
-        symbolicElimResult =
+    SymbolicFactor::shared_ptr mySeparatorFactor;
-            internal::EliminateSymbolic(symbolicFactors, keyAsOrdering);
+    boost::tie(myConditional, mySeparatorFactor) = internal::EliminateSymbolic(
+        symbolicFactors, keyAsOrdering);
 
     // Store symbolic elimination results in the parent
-    myData.parentData->childSymbolicConditionals.push_back(
+    myData.parentData->childSymbolicConditionals.push_back(myConditional);
-        symbolicElimResult.first);
+    myData.parentData->childSymbolicFactors.push_back(mySeparatorFactor);
-    myData.parentData->childSymbolicFactors.push_back(
+
-        symbolicElimResult.second);
     sharedNode node = myData.myJTNode;
+    const FastVector<SymbolicConditional::shared_ptr>& childConditionals =
+        myData.childSymbolicConditionals;
+    node->problemSize_ = (int) (myConditional->size() * symbolicFactors.size());
 
     // Merge our children if they are in our clique - if our conditional has
     // exactly one fewer parent than our child's conditional.
-    size_t myNrFrontals = 1;
+    const size_t myNrParents = myConditional->nrParents();
-    const size_t myNrParents = symbolicElimResult.first->nrParents();
+    const size_t nrChildren = node->children.size();
-    size_t nrMergedChildren = 0;
+    assert(childConditionals.size() == nrChildren);
-    assert(node->children.size() == myData.childSymbolicConditionals.size());
+
-    // Loop over children
+    // decide which children to merge, as index into children
-    int combinedProblemSize =
+    std::vector<bool> merge(nrChildren, false);
-        (int)(symbolicElimResult.first->size() * symbolicFactors.size());
+    size_t myNrFrontals = 1, i = 0;
-    gttic(merge_children);
+    BOOST_FOREACH(const sharedNode& child, node->children) {
-    for (size_t i = 0; i < myData.childSymbolicConditionals.size(); ++i) {
       // Check if we should merge the i^th child
-      if (myNrParents + myNrFrontals ==
+      if (myNrParents + myNrFrontals == childConditionals[i]->nrParents()) {
-          myData.childSymbolicConditionals[i]->nrParents()) {
-        // Get a reference to the i, adjusting the index to account for children
-        // previously merged and removed from the i list.
-        const Node& child = *node->children[i - nrMergedChildren];
-        // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after..
-        node->orderedFrontalKeys.insert(node->orderedFrontalKeys.end(),
-                                        child.orderedFrontalKeys.rbegin(),
-                                        child.orderedFrontalKeys.rend());
-        // Merge keys, factors, and children.
-        node->factors.insert(node->factors.end(), child.factors.begin(), child.factors.end());
-        node->children.insert(node->children.end(), child.children.begin(), child.children.end());
-        // Increment problem size
-        combinedProblemSize = std::max(combinedProblemSize, child.problemSize_);
         // Increment number of frontal variables
-        myNrFrontals += child.orderedFrontalKeys.size();
+        myNrFrontals += child->orderedFrontalKeys.size();
-        // Remove i from list.
+        merge[i] = true;
-        node->children.erase(node->children.begin() + (i - nrMergedChildren));
-        // Increment number of merged children
-        ++nrMergedChildren;
       }
+      ++i;
     }
-    std::reverse(node->orderedFrontalKeys.begin(), node->orderedFrontalKeys.end());
+
-    gttoc(merge_children);
+    // now really merge
-    node->problemSize_ = combinedProblemSize;
+    node->mergeChildren(merge);
   }
 };
 
@@ -147,15 +138,17 @@ JunctionTree<BAYESTREE, GRAPH>::JunctionTree(
   typedef typename EliminationTree<ETREE_BAYESNET, ETREE_GRAPH>::Node ETreeNode;
   typedef ConstructorTraversalData<BAYESTREE, GRAPH, ETreeNode> Data;
   Data rootData(0);
-  rootData.myJTNode =
+  rootData.myJTNode = boost::make_shared<typename Base::Node>(); // Make a dummy node to gather
-      boost::make_shared<typename Base::Node>();  // Make a dummy node to gather
                                                                  // the junction tree roots
   treeTraversal::DepthFirstForest(eliminationTree, rootData,
       Data::ConstructorTraversalVisitorPre,
       Data::ConstructorTraversalVisitorPostAlg2);
 
   // Assign roots from the dummy node
-  Base::roots_ = rootData.myJTNode->children;
+  typedef typename JunctionTree<BAYESTREE, GRAPH>::Node Node;
+  const typename Node::Children& children = rootData.myJTNode->children;
+  Base::roots_.reserve(children.size());
+  Base::roots_.insert(Base::roots_.begin(), children.begin(), children.end());
 
   // Transfer remaining factors from elimination tree
   Base::remainingFactors_ = eliminationTree.remainingFactors();