diff --git a/gtsam/hybrid/HybridNonlinearISAM.cpp b/gtsam/hybrid/HybridNonlinearISAM.cpp index 29e467d86..3b4856dfb 100644 --- a/gtsam/hybrid/HybridNonlinearISAM.cpp +++ b/gtsam/hybrid/HybridNonlinearISAM.cpp @@ -15,6 +15,7 @@ * @author Varun Agrawal */ +#include #include #include #include @@ -65,7 +66,14 @@ void HybridNonlinearISAM::reorderRelinearize() { // Obtain the new linearization point const Values newLinPoint = estimate(); - auto discreteProbs = *(isam_.roots().at(0)->conditional()->asDiscrete()); + DiscreteConditional::shared_ptr discreteProbabilities; + + auto discreteRoot = isam_.roots().at(0)->conditional(); + if (discreteRoot->asDiscrete()) { + discreteProbabilities = discreteRoot->asDiscrete(); + } else { + discreteProbabilities = discreteRoot->asDiscrete(); + } isam_.clear(); @@ -73,7 +81,7 @@ void HybridNonlinearISAM::reorderRelinearize() { HybridNonlinearFactorGraph pruned_factors; for (auto&& factor : factors_) { if (auto nf = std::dynamic_pointer_cast(factor)) { - pruned_factors.push_back(nf->prune(discreteProbs)); + pruned_factors.push_back(nf->prune(*discreteProbabilities)); } else { pruned_factors.push_back(factor); }