diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 6c7b6456e..2ed499a4c 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -121,6 +121,11 @@ namespace gtsam { for (auto&& factor : factors) product = (*factor) * product; gttoc(product); + // Normalize the product factor to prevent underflow + Ordering ordering = Ordering::Colamd(DiscreteFactorGraph{product}); + DecisionTreeFactor::shared_ptr normalization = product.sum(ordering); + product = product / (*normalization); + // max out frontals, this is the factor on the separator gttic(max); DecisionTreeFactor::shared_ptr max = product.max(frontalKeys);