Fix a mistake in BayesTreeMarginalizationHelper:

In gatherDependentCliques(), we should only add the cliques
under those children that depend on a marginalizable variable.

gtsam_unstable/nonlinear/BayesTreeMarginalizationHelper.h

@@ -21,6 +21,7 @@
 #pragma once
 
 #include <unordered_map>
+#include <deque>
 #include <gtsam/inference/BayesTree.h>
 #include <gtsam/inference/BayesTreeCliqueBase.h>
 #include <gtsam/base/debug.h>
@@ -50,11 +51,12 @@ class GTSAM_UNSTABLE_EXPORT BayesTreeMarginalizationHelper {
    * 2. Or it has a child node depending on a marginalizable variable AND the
    *    subtree rooted at that child contains non-marginalizables.
    * 
-   * In addition, for any descendant node under the aforementioned cliques that
-   * require re-elimination, if the subtree rooted at that descendant contains
+   * In addition, for any descendant node depending on a marginalizable
+   * variable, if the subtree rooted at that descendant contains
    * non-marginalizable variables (i.e., it lies on a path from one of the
-   * aforementioned cliques to a node containing non-marginalizables at the
-   * leaf side), then it also needs to be re-eliminated.
+   * aforementioned cliques that require re-elimination to a node containing
+   * non-marginalizable variables at the leaf side), then it also needs to
+   * be re-eliminated.
    * 
    * @param[in] bayesTree The Bayes tree
    * @param[in] marginalizableKeys Keys to be marginalized
@@ -218,7 +220,7 @@ class GTSAM_UNSTABLE_EXPORT BayesTreeMarginalizationHelper {
   }
 
   /**
-   * Gather all descendant nodes that lie on a path from the root clique
+   * Gather all dependent nodes that lie on a path from the root clique
    * to a clique containing a non-marginalizable variable at the leaf side.
    *
    * @param[in] rootClique The root clique
@@ -228,22 +230,40 @@ class GTSAM_UNSTABLE_EXPORT BayesTreeMarginalizationHelper {
       const sharedClique& rootClique,
       const std::set<Key>& marginalizableKeys,
       CachedSearch* cache) {
+    std::vector<sharedClique> dependentChildren;
+    dependentChildren.reserve(rootClique->children.size());
+    for (const sharedClique& child : rootClique->children) {
+      if (hasDependency(child, marginalizableKeys)) {
+        dependentChildren.push_back(child);
+      }
+    }
+    return gatherDependentCliquesFromChildren(dependentChildren, marginalizableKeys, cache);
+  }
+
+  /**
+   * A helper function for the above gatherDependentCliques().
+   */
+  static std::set<sharedClique> gatherDependentCliquesFromChildren(
+      const std::vector<sharedClique>& dependentChildren,
+      const std::set<Key>& marginalizableKeys,
+      CachedSearch* cache) {
+    std::deque<sharedClique> descendants(
+        dependentChildren.begin(), dependentChildren.end());
     std::set<sharedClique> dependentCliques;
-    std::set<sharedClique> descendants(
-        rootClique->children. begin(), rootClique->children.end());
     while (!descendants.empty()) {
-      auto begin = descendants.begin();
-      sharedClique descendant = *begin;
-      descendants.erase(begin);
+      sharedClique descendant = descendants.front();
+      descendants.pop_front();
 
       // If the subtree rooted at this descendant contains non-marginalizables,
       // it must lie on a path from the root clique to a clique containing
       // non-marginalizables at the leaf side.
       if (!isWholeSubtreeMarginalizable(descendant, marginalizableKeys, cache)) {
         dependentCliques.insert(descendant);
       }
+
+      // Add all children of the current descendant to the set descendants.
       for (const sharedClique& child : descendant->children) {
-        descendants.insert(child);
+        descendants.push_back(child);
       }
     }
     return dependentCliques;
@@ -281,6 +301,19 @@ class GTSAM_UNSTABLE_EXPORT BayesTreeMarginalizationHelper {
       return false;
     }
   }
+
+  /**
+   * Check if the clique depends on any of the given keys.
+   */
+  static bool hasDependency(
+      const sharedClique& clique, const std::set<Key>& keys) {
+    for (Key key : keys) {
+      if (hasDependency(clique, key)) {
+        return true;
+      }
+    }
+    return false;
+  }
 };
 // BayesTreeMarginalizationHelper