diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index 46481e3058..f3a699094b 100644 --- a/gtsam/hybrid/HybridGaussianConditional.cpp +++ b/gtsam/hybrid/HybridGaussianConditional.cpp @@ -37,14 +37,9 @@ HybridGaussianConditional::ConstructorHelper::ConstructorHelper( [&](const GaussianConditional::shared_ptr &c) -> GaussianFactorValuePair { double value = 0.0; if (c) { - KeyVector cf(c->frontals().begin(), c->frontals().end()); - KeyVector cp(c->parents().begin(), c->parents().end()); if (frontals.empty()) { - frontals = cf; - parents = cp; - } else if (cf != frontals || cp != parents) { - throw std::invalid_argument( - "All conditionals must have the same frontals and parents"); + frontals = KeyVector(c->frontals().begin(), c->frontals().end()); + parents = KeyVector(c->parents().begin(), c->parents().end()); } value = c->negLogConstant(); negLogConstant = std::min(negLogConstant, value); diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index 279ac4069d..938a79b19d 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -76,11 +76,9 @@ HybridGaussianFactor::ConstructorHelper::ConstructorHelper( : discreteKeys({discreteKey}) { // Extract continuous keys from the first non-null factor for (const auto &factor : factors) { - if (!factor) continue; // Skip null factors - if (continuousKeys.empty()) { + if (factor && continuousKeys.empty()) { continuousKeys = factor->keys(); - } else if (factor->keys() != continuousKeys) { - throw std::invalid_argument("All factors must have the same keys"); + break; } } @@ -95,11 +93,9 @@ HybridGaussianFactor::ConstructorHelper::ConstructorHelper( : discreteKeys({discreteKey}) { // Extract continuous keys from the first non-null factor for (const auto &pair : factorPairs) { - if (!pair.first) continue; // Skip null factors - if (continuousKeys.empty()) { + if (pair.first && continuousKeys.empty()) { continuousKeys = pair.first->keys(); - } else if (pair.first->keys() != continuousKeys) { - throw std::invalid_argument("All factors must have the same keys"); + break; } } @@ -113,11 +109,8 @@ HybridGaussianFactor::ConstructorHelper::ConstructorHelper( : discreteKeys(discreteKeys) { // Extract continuous keys from the first non-null factor factorPairs.visit([&](const GaussianFactorValuePair &pair) { - if (!pair.first) return; // Skip null factors - if (continuousKeys.empty()) { + if (pair.first && continuousKeys.empty()) { continuousKeys = pair.first->keys(); - } else if (pair.first->keys() != continuousKeys) { - throw std::invalid_argument("All factors must have the same keys"); } });