diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index 48c327156..6f52df917 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -185,6 +185,11 @@ std::shared_ptr HybridNonlinearFactor::linearize( [continuousValues]( const std::pair& f) -> GaussianFactorValuePair { auto [factor, val] = f; + // Check if valid factor. If not, return null and infinite error. + if (!factor) { + return {nullptr, std::numeric_limits::infinity()}; + } + if (auto gaussian = std::dynamic_pointer_cast( factor->noiseModel())) { return {factor->linearize(continuousValues),