diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 01ecfe5ac..a6fe955eb 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -542,10 +542,15 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::errorTree( // Iterate over each factor. for (auto &factor : factors_) { if (auto f = std::dynamic_pointer_cast(factor)) { + // Check for HybridFactor, and call errorTree error_tree = error_tree + f->errorTree(continuousValues); - } else if (auto f = std::dynamic_pointer_cast(factor)) { - error_tree = - error_tree + AlgebraicDecisionTree(f->error(continuousValues)); + } else if (auto f = std::dynamic_pointer_cast(factor)) { + // Skip discrete factors + continue; + } else { + // Everything else is a continuous only factor + HybridValues hv(continuousValues, DiscreteValues()); + error_tree = error_tree + AlgebraicDecisionTree(factor->error(hv)); } } return error_tree;