check for valid factor in HybridNonlinearFactor::errorTree

2025-01-26 11:14:16 -05:00 · 2025-01-26 11:14:16 -05:00 · 655f57ef0d
parent 3302ad46c8
commit 655f57ef0d
1 changed files with 2 additions and 1 deletions
--- a/gtsam/hybrid/HybridNonlinearFactor.cpp
+++ b/gtsam/hybrid/HybridNonlinearFactor.cpp
@ -99,7 +99,8 @@ AlgebraicDecisionTree<Key> HybridNonlinearFactor::errorTree(
  auto errorFunc =
      [continuousValues](const std::pair<sharedFactor, double>& f) {
        auto [factor, val] = f;
-        return factor->error(continuousValues) + val;
+        return factor ? factor->error(continuousValues) + val
+                      : std::numeric_limits<double>::infinity();
      };
  return {factors_, errorFunc};
 }