Merge pull request #2061 from borglab/hybrid-fix

Remove fixed values when keys are reintroduced in `newFactors`

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()) {

gtsam/hybrid/HybridSmoother.cpp

@@ -61,6 +61,40 @@ Ordering HybridSmoother::getOrdering(const HybridGaussianFactorGraph &factors,
   return ordering;
 }
 
+/* ************************************************************************* */
+Ordering HybridSmoother::maybeComputeOrdering(
+    const HybridGaussianFactorGraph &updatedGraph,
+    const std::optional<Ordering> givenOrdering) {
+  Ordering ordering;
+  // If no ordering provided, then we compute one
+  if (!givenOrdering.has_value()) {
+    // Get the keys from the new factors
+    KeySet continuousKeysToInclude;  // Scheme 1: empty, 15sec/2000, 64sec/3000
+                                     // (69s without TF)
+    // continuousKeysToInclude = newFactors.keys();  // Scheme 2: all,
+    // 8sec/2000, 160sec/3000 continuousKeysToInclude = updatedGraph.keys();  //
+    // Scheme 3: all, stopped after 80sec/2000
+
+    // Since updatedGraph now has all the connected conditionals,
+    // we can get the correct ordering.
+    ordering = this->getOrdering(updatedGraph, continuousKeysToInclude);
+  } else {
+    ordering = *givenOrdering;
+  }
+
+  return ordering;
+}
+
+/* ************************************************************************* */
+void HybridSmoother::removeFixedValues(
+    const HybridGaussianFactorGraph &newFactors) {
+  for (Key key : newFactors.discreteKeySet()) {
+    if (fixedValues_.find(key) != fixedValues_.end()) {
+      fixedValues_.erase(key);
+    }
+  }
+}
+
 /* ************************************************************************* */
 void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
                             std::optional<size_t> maxNrLeaves,
@@ -84,22 +118,7 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
             << std::endl;
 #endif
 
-  Ordering ordering;
-  // If no ordering provided, then we compute one
-  if (!given_ordering.has_value()) {
-    // Get the keys from the new factors
-    KeySet continuousKeysToInclude;  // Scheme 1: empty, 15sec/2000, 64sec/3000
-                                     // (69s without TF)
-    // continuousKeysToInclude = newFactors.keys();  // Scheme 2: all,
-    // 8sec/2000, 160sec/3000 continuousKeysToInclude = updatedGraph.keys();  //
-    // Scheme 3: all, stopped after 80sec/2000
-
-    // Since updatedGraph now has all the connected conditionals,
-    // we can get the correct ordering.
-    ordering = this->getOrdering(updatedGraph, continuousKeysToInclude);
-  } else {
-    ordering = *given_ordering;
-  }
+  Ordering ordering = this->maybeComputeOrdering(updatedGraph, given_ordering);
 
 #if GTSAM_HYBRID_TIMING
   gttic_(HybridSmootherEliminate);
@@ -118,6 +137,9 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &newFactors,
   }
 #endif
 
+  // Remove fixed values for discrete keys which are introduced in newFactors
+  removeFixedValues(newFactors);
+
 #ifdef DEBUG_SMOOTHER
   // Print discrete keys in the bayesNetFragment:
   std::cout << "Discrete keys in bayesNetFragment: ";

gtsam/hybrid/HybridSmoother.h

@@ -122,6 +122,14 @@ class GTSAM_EXPORT HybridSmoother {
 
   /// Optimize the hybrid Bayes Net, taking into accound fixed values.
   HybridValues optimize() const;
+
+ private:
+  /// Helper to compute the ordering if ordering is not given.
+  Ordering maybeComputeOrdering(const HybridGaussianFactorGraph& updatedGraph,
+                                const std::optional<Ordering> givenOrdering);
+
+  /// Remove fixed discrete values for discrete keys introduced in `newFactors`.
+  void removeFixedValues(const HybridGaussianFactorGraph& newFactors);
 };
 
 }  // namespace gtsam