From 93528c3d4f8ae5c87a063a3232fb2894e7899504 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Fri, 16 Sep 2022 12:19:24 -0400
Subject: [PATCH] Only eliminate variables that are in newFactors

---
 gtsam/hybrid/HybridGaussianISAM.cpp | 14 ++++++++++++--
 1 file changed, 12 insertions(+), 2 deletions(-)

diff --git a/gtsam/hybrid/HybridGaussianISAM.cpp b/gtsam/hybrid/HybridGaussianISAM.cpp
index 6946775b9..c7811992e 100644
--- a/gtsam/hybrid/HybridGaussianISAM.cpp
+++ b/gtsam/hybrid/HybridGaussianISAM.cpp
@@ -62,23 +62,29 @@ void HybridGaussianISAM::updateInternal(
   for (const sharedClique& orphan : *orphans)
     factors += boost::make_shared<BayesTreeOrphanWrapper<Node> >(orphan);
 
+  // Get all the discrete keys from the new factors
   KeySet allDiscrete;
-  for (auto& factor : factors) {
+  for (auto& factor : newFactors) {
     for (auto& k : factor->discreteKeys()) {
       allDiscrete.insert(k.first);
     }
   }
+
+  // Create KeyVector with continuous keys followed by discrete keys.
   KeyVector newKeysDiscreteLast;
+  // Insert continuous keys first.
   for (auto& k : newFactorKeys) {
     if (!allDiscrete.exists(k)) {
       newKeysDiscreteLast.push_back(k);
     }
   }
+  // Insert discrete keys at the end
   std::copy(allDiscrete.begin(), allDiscrete.end(),
             std::back_inserter(newKeysDiscreteLast));
 
   // Get an ordering where the new keys are eliminated last
-  const VariableIndex index(factors);
+  const VariableIndex index(newFactors);
+
   Ordering elimination_ordering;
   if (ordering) {
     elimination_ordering = *ordering;
@@ -89,10 +95,14 @@ void HybridGaussianISAM::updateInternal(
         true);
   }
 
+  GTSAM_PRINT(elimination_ordering);
+  std::cout << "\n\n\n\neliminateMultifrontal" << std::endl;
+  GTSAM_PRINT(factors);
   // eliminate all factors (top, added, orphans) into a new Bayes tree
   HybridBayesTree::shared_ptr bayesTree =
       factors.eliminateMultifrontal(elimination_ordering, function, index);
 
+  std::cout << "optionally prune" << std::endl;
   if (maxNrLeaves) {
     bayesTree->prune(*maxNrLeaves);
   }