check for potentiall pruning

gtsam/hybrid/HybridGaussianFactor.cpp

@@ -151,12 +151,26 @@ GaussianFactorGraphTree HybridGaussianFactor::asGaussianFactorGraphTree()
   return {factors_, wrap};
 }
 
+/* *******************************************************************************/
+double HybridGaussianFactor::potentiallyPrunedComponentError(
+    const sharedFactor &gf, const VectorValues &values) const {
+  // Check if valid pointer
+  if (gf) {
+    return gf->error(values);
+  } else {
+    // If not valid, pointer, it means this component was pruned,
+    // so we return maximum error.
+    // This way the negative exponential will give
+    // a probability value close to 0.0.
+    return std::numeric_limits<double>::max();
+  }
+}
 /* *******************************************************************************/
 AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
     const VectorValues &continuousValues) const {
   // functor to convert from sharedFactor to double error value.
-  auto errorFunc = [&continuousValues](const sharedFactor &gf) {
-    return gf->error(continuousValues);
+  auto errorFunc = [this, &continuousValues](const sharedFactor &gf) {
+    return this->potentiallyPrunedComponentError(gf, continuousValues);
   };
   DecisionTree<Key, double> error_tree(factors_, errorFunc);
   return error_tree;
@@ -164,8 +178,9 @@ AlgebraicDecisionTree<Key> HybridGaussianFactor::errorTree(
 
 /* *******************************************************************************/
 double HybridGaussianFactor::error(const HybridValues &values) const {
+  // Directly index to get the component, no need to build the whole tree.
   const sharedFactor gf = factors_(values.discrete());
-  return gf->error(values.continuous());
+  return potentiallyPrunedComponentError(gf, values.continuous());
 }
 
 }  // namespace gtsam

gtsam/hybrid/HybridGaussianFactor.h

@@ -169,6 +169,10 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor {
   /// @}
 
  private:
+  /// Helper method to compute the error of a component.
+  double potentiallyPrunedComponentError(
+      const sharedFactor &gf, const VectorValues &continuousValues) const;
+
 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
   /** Serialization function */
   friend class boost::serialization::access;