From 07f0101db7401be00917c1f2c099cd6326e3369c Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sun, 21 Aug 2022 12:48:33 -0400
Subject: [PATCH] check subset rather than equality for GaussianISAM pruning

---
 gtsam/hybrid/HybridGaussianISAM.cpp | 18 +++++++++++++++++-
 1 file changed, 17 insertions(+), 1 deletion(-)

diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp
index afcda8c20..23a95c021 100644
--- a/gtsam/hybrid/HybridGaussianISAM.cpp
+++ b/gtsam/hybrid/HybridGaussianISAM.cpp
@@ -105,6 +105,22 @@ void HybridGaussianISAM::update(const HybridGaussianFactorGraph& newFactors,
   this->updateInternal(newFactors, &orphans, ordering, function);
 }
 
+/* ************************************************************************* */
+/**
+ * @brief Check if `b` is a subset of `a`.
+ * Non-const since they need to be sorted.
+ *
+ * @param a KeyVector
+ * @param b KeyVector
+ * @return True if the keys of b is a subset of a, else false.
+ */
+bool IsSubset(KeyVector a, KeyVector b) {
+  std::sort(a.begin(), a.end());
+  std::sort(b.begin(), b.end());
+  return std::includes(a.begin(), a.end(), b.begin(), b.end());
+}
+
+/* ************************************************************************* */
 void HybridGaussianISAM::prune(const Key& root, const size_t maxNrLeaves) {
   auto decisionTree = boost::dynamic_pointer_cast<DecisionTreeFactor>(
       this->clique(root)->conditional()->inner());
@@ -131,7 +147,7 @@ void HybridGaussianISAM::prune(const Key& root, const size_t maxNrLeaves) {
       parents.push_back(parent);
     }
 
-    if (parents == decisionTree->keys()) {
+    if (IsSubset(parents, decisionTree->keys())) {
       auto gaussianMixture = boost::dynamic_pointer_cast<GaussianMixture>(
           clique.second->conditional()->inner());