add deadModeThreshold argument to HybridBayesNet::prune

gtsam/hybrid/HybridBayesNet.cpp

@@ -47,8 +47,8 @@ bool HybridBayesNet::equals(const This &bn, double tol) const {
 // TODO(Frank): This can be quite expensive *unless* the factors have already
 // been pruned before. Another, possibly faster approach is branch and bound
 // search to find the K-best leaves and then create a single pruned conditional.
-HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves,
-                                     bool removeDeadModes) const {
+HybridBayesNet HybridBayesNet::prune(size_t maxNrLeaves, bool removeDeadModes,
+                                     double deadModeThreshold) const {
   // Collect all the discrete conditionals. Could be small if already pruned.
   const DiscreteBayesNet marginal = discreteMarginal();
 

gtsam/hybrid/HybridBayesNet.h

@@ -219,9 +219,13 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet<HybridConditional> {
    * @param maxNrLeaves Continuous values at which to compute the error.
    * @param removeDeadModes Flag to enable removal of modes which only have a
    * single possible assignment.
+   * @param deadModeThreshold The threshold to check the mode marginals against.
+   * If greater than this threshold, the mode gets assigned that value and is
+   * considered "dead" for hybrid elimination.
    * @return A pruned HybridBayesNet
    */
-  HybridBayesNet prune(size_t maxNrLeaves, bool removeDeadModes = false) const;
+  HybridBayesNet prune(size_t maxNrLeaves, bool removeDeadModes = false,
+                       double deadModeThreshold = 0.99) const;
 
   /**
    * @brief Error method using HybridValues which returns specific error for

gtsam/hybrid/HybridSmoother.cpp

@@ -74,6 +74,11 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph,
     ordering = *given_ordering;
   }
 
+  // graph.print("Original GRAPH");
+  // GTSAM_PRINT(updatedGraph);
+  // GTSAM_PRINT(hybridBayesNet_);
+  // GTSAM_PRINT(ordering);
+
   // Eliminate.
   HybridBayesNet bayesNetFragment = *updatedGraph.eliminateSequential(ordering);
 
@@ -81,7 +86,8 @@ void HybridSmoother::update(const HybridGaussianFactorGraph &graph,
   if (maxNrLeaves) {
     // `pruneBayesNet` sets the leaves with 0 in discreteFactor to nullptr in
     // all the conditionals with the same keys in bayesNetFragment.
-    bayesNetFragment = bayesNetFragment.prune(*maxNrLeaves, removeDeadModes_);
+    bayesNetFragment = bayesNetFragment.prune(*maxNrLeaves, removeDeadModes_,
+                                              deadModeThreshold_);
   }
 
   // Add the partial bayes net to the posterior bayes net.