diff --git a/gtsam/hybrid/HybridBayesTree.cpp b/gtsam/hybrid/HybridBayesTree.cpp index c74930a8e..fbf892235 100644 --- a/gtsam/hybrid/HybridBayesTree.cpp +++ b/gtsam/hybrid/HybridBayesTree.cpp @@ -202,6 +202,11 @@ VectorValues HybridBayesTree::optimize(const DiscreteValues& assignment) const { /* ************************************************************************* */ void HybridBayesTree::prune(const size_t maxNrLeaves) { + if (!this->roots_.at(0)->conditional()->asDiscrete()) { + // Root of the BayesTree is not a discrete clique, so we do nothing. + return; + } + auto prunedDiscreteProbs = this->roots_.at(0)->conditional()->asDiscrete(); diff --git a/gtsam/hybrid/HybridNonlinearFactor.cpp b/gtsam/hybrid/HybridNonlinearFactor.cpp index 376bc66f1..fa22051e5 100644 --- a/gtsam/hybrid/HybridNonlinearFactor.cpp +++ b/gtsam/hybrid/HybridNonlinearFactor.cpp @@ -99,7 +99,8 @@ AlgebraicDecisionTree HybridNonlinearFactor::errorTree( auto errorFunc = [continuousValues](const std::pair& f) { auto [factor, val] = f; - return factor->error(continuousValues) + val; + return factor ? factor->error(continuousValues) + val + : std::numeric_limits::infinity(); }; return {factors_, errorFunc}; }