From 3c06f0755139ec93e4b473b988c35fea8bc1adec Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 29 Oct 2024 01:56:03 -0400
Subject: [PATCH] check if NonlinearFactor is valid before linearizing

---
 gtsam/hybrid/HybridNonlinearFactor.cpp | 5 +++++
 1 file changed, 5 insertions(+)

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<HybridGaussianFactor> HybridNonlinearFactor::linearize(
       [continuousValues](
           const std::pair<sharedFactor, double>& f) -> GaussianFactorValuePair {
     auto [factor, val] = f;
+    // Check if valid factor. If not, return null and infinite error.
+    if (!factor) {
+      return {nullptr, std::numeric_limits<double>::infinity()};
+    }
+
     if (auto gaussian = std::dynamic_pointer_cast<noiseModel::Gaussian>(
             factor->noiseModel())) {
       return {factor->linearize(continuousValues),