From 88fb8f7c6596cf2842a46e166443ae58efea26d8 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Mon, 17 Mar 2025 15:39:56 -0400
Subject: [PATCH] remove fixed values when keys are reintroduced in newFactors

---
 gtsam/hybrid/HybridBayesNet.cpp | 3 ++-
 gtsam/hybrid/HybridSmoother.cpp | 7 +++++++
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp
index 6ce2206c0..c84ed9aa6 100644
--- a/gtsam/hybrid/HybridBayesNet.cpp
+++ b/gtsam/hybrid/HybridBayesNet.cpp
@@ -73,9 +73,10 @@ HybridBayesNet HybridBayesNet::prune(
     if (conditional->isDiscrete()) continue;
 
     // No-op if not a HybridGaussianConditional.
-    if (marginalThreshold)
+    if (marginalThreshold) {
       conditional = std::static_pointer_cast<HybridConditional>(
           conditional->restrict(fixed));
+    }
 
     // Now decide on type what to do:
     if (auto hgc = conditional->asHybrid()) {
diff --git a/gtsam/hybrid/HybridSmoother.cpp b/gtsam/hybrid/HybridSmoother.cpp
index b0fbb35c9..12de2c14f 100644
--- a/gtsam/hybrid/HybridSmoother.cpp
+++ b/gtsam/hybrid/HybridSmoother.cpp
@@ -118,6 +118,13 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
   }
 #endif
 
+  // Remove fixed values for discrete keys which are introduced in newFactors
+  for (Key key : newFactors.discreteKeySet()) {
+    if (fixedValues_.find(key) != fixedValues_.end()) {
+      fixedValues_.erase(key);
+    }
+  }
+
 #ifdef DEBUG_SMOOTHER
   // Print discrete keys in the bayesNetFragment:
   std::cout << "Discrete keys in bayesNetFragment: ";