new removeModes method

gtsam/hybrid/HybridConditional.cpp

@@ -169,4 +169,41 @@ double HybridConditional::evaluate(const HybridValues &values) const {
   return std::exp(logProbability(values));
 }
 
+/* ************************************************************************ */
+void HybridConditional::removeModes(const DiscreteValues &given) {
+  if (this->isDiscrete()) {
+    auto d = this->asDiscrete();
+
+    AlgebraicDecisionTree<Key> tree(*d);
+    for (auto [key, value] : given) {
+      tree = tree.choose(key, value);
+    }
+
+    // Get the leftover DiscreteKeys
+    DiscreteKeys dkeys;
+    for (DiscreteKey dkey : d->discreteKeys()) {
+      if (given.count(dkey.first) == 0) {
+        dkeys.emplace_back(dkey);
+      }
+    }
+    inner_ = std::make_shared<DiscreteConditional>(dkeys.size(), dkeys, tree);
+
+  } else if (this->isHybrid()) {
+    auto d = this->asHybrid();
+    HybridGaussianFactor::FactorValuePairs tree = d->factors();
+    for (auto [key, value] : given) {
+      tree = tree.choose(key, value);
+    }
+
+    // Get the leftover DiscreteKeys
+    DiscreteKeys dkeys;
+    for (DiscreteKey dkey : d->discreteKeys()) {
+      if (given.count(dkey.first) == 0) {
+        dkeys.emplace_back(dkey);
+      }
+    }
+    inner_ = std::make_shared<HybridGaussianConditional>(dkeys, tree);
+  }
+}
+
 }  // namespace gtsam

gtsam/hybrid/HybridConditional.h

@@ -215,6 +215,15 @@ class GTSAM_EXPORT HybridConditional
     return true;
   }
 
+  /**
+   * @brief Remove the modes whose assignments are given to us.
+   *
+   * Imperative method so we can update nodes in the Bayes net or Bayes tree.
+   *
+   * @param given The discrete modes whose assignments we know.
+   */
+  void removeModes(const DiscreteValues& given);
+
   /// @}
 
  private: