Moved merging to ClusterTree

gtsam/inference/ClusterTree.h

@@ -58,6 +58,61 @@ public:
     /** print this node */
     void print(const std::string& s = "", const KeyFormatter& keyFormatter =
         DefaultKeyFormatter) const;
+
+    void mergeChildren(const std::vector<bool>& merge) {
+      gttic(merge_children);
+      size_t nrChildren = children.size();
+
+      // Count how many keys, factors and children we'll end up with
+      size_t nrKeys = this->orderedFrontalKeys.size();
+      size_t nrFactors = this->factors.size();
+      size_t nrNewChildren = 0;
+      // Loop over children
+      for (size_t i = 0; i < nrChildren; ++i) {
+        if (merge[i]) {
+          // Get a reference to the i, adjusting the index to account for children
+          // previously merged and removed from the i list.
+          sharedNode child = this->children[i];
+          nrKeys += child->orderedFrontalKeys.size();
+          nrFactors += child->factors.size();
+          nrNewChildren += child->children.size();
+        } else {
+          nrNewChildren += 1; // we keep the child
+        }
+      }
+
+      // now reserve space, and really merge
+      this->orderedFrontalKeys.reserve(nrKeys);
+      this->factors.reserve(nrFactors);
+      typename Node::Children newChildren;
+      newChildren.reserve(nrNewChildren);
+      // Loop over newChildren
+      for (size_t i = 0; i < nrChildren; ++i) {
+        // Check if we should merge the i^th child
+        sharedNode child = this->children[i];
+        if (merge[i]) {
+          // Get a reference to the i, adjusting the index to account for newChildren
+          // previously merged and removed from the i list.
+          // Merge keys. For efficiency, we add keys in reverse order at end, calling reverse after..
+          this->orderedFrontalKeys.insert(this->orderedFrontalKeys.end(),
+              child->orderedFrontalKeys.rbegin(),
+              child->orderedFrontalKeys.rend());
+          // Merge keys, factors, and children.
+          this->factors.insert(this->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
+        }
+      }
+      this->children = newChildren;
+      std::reverse(this->orderedFrontalKeys.begin(),
+          this->orderedFrontalKeys.end());
+    }
   };
 
   typedef boost::shared_ptr<Cluster> sharedCluster; ///< Shared pointer to Cluster

gtsam/inference/JunctionTree-inst.h

@@ -94,6 +94,7 @@ struct ConstructorTraversalData {
     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.
@@ -105,7 +106,6 @@ struct ConstructorTraversalData {
 
     // decide which children to merge, as index into children
     std::vector<bool> merge(nrChildren, false);
-    {
     size_t myNrFrontals = 1;
     for (size_t i = 0; i < nrChildren; ++i) {
       // Check if we should merge the i^th child
@@ -116,65 +116,11 @@ struct ConstructorTraversalData {
         merge[i] = true;
       }
     }
-    }
 
-    // Count how many keys, factors and children we'll end up with
+    // now really merge
-    size_t nrKeys = node->orderedFrontalKeys.size();
+    node->mergeChildren(merge);
-    size_t nrFactors = node->factors.size();
-    size_t nrNewChildren = 0;
-    // Loop over children
-    for (size_t i = 0; i < nrChildren; ++i) {
-      if (merge[i]) {
-        // Get a reference to the i, adjusting the index to account for children
-        // previously merged and removed from the i list.
-        sharedNode child = node->children[i];
-        nrKeys += child->orderedFrontalKeys.size();
-        nrFactors += child->factors.size();
-        nrNewChildren += child->children.size();
-      } else {
-        nrNewChildren += 1; // we keep the child
   }
-    }
+};
-
-    // now reserve space, and really merge
-    node->orderedFrontalKeys.reserve(nrKeys);
-    node->factors.reserve(nrFactors);
-    typename Node::Children newChildren;
-    newChildren.reserve(nrNewChildren);
-    int combinedProblemSize = (int) (myConditional->size()
-        * symbolicFactors.size());
-    // Loop over newChildren
-    for (size_t i = 0; i < nrChildren; ++i) {
-      // Check if we should merge the i^th child
-      sharedNode child = node->children[i];
-      if (merge[i]) {
-        // Get a reference to the i, adjusting the index to account for newChildren
-        // previously merged and removed from the i list.
-        // 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());
-        newChildren.insert(newChildren.end(), child->children.begin(),
-            child->children.end());
-        // Increment problem size
-        combinedProblemSize = std::max(combinedProblemSize,
-            child->problemSize_);
-        // Increment number of frontal variables
-      } else {
-        newChildren.push_back(child); // we keep the child
-      }
-    }
-    node->children = newChildren;
-    std::reverse(node->orderedFrontalKeys.begin(),
-        node->orderedFrontalKeys.end());
-    gttoc(merge_children);
-    node->problemSize_ = combinedProblemSize;
-  }
-}
-;
 
 /* ************************************************************************* */
 template<class BAYESTREE, class GRAPH>