diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 1fb353423..321ec7147 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -214,14 +214,21 @@ namespace gtsam { std::pair // EliminateDiscrete(const DiscreteFactorGraph& factors, const Ordering& frontalKeys) { - DiscreteFactor::shared_ptr product = factors.scaledProduct(); + gttic(product); + DiscreteFactor::shared_ptr product = factors.product(); + gttoc(product); // sum out frontals, this is the factor on the separator gttic(sum); DiscreteFactor::shared_ptr sum = product->sum(frontalKeys); - // Normalize/scale to prevent underflow. - sum = sum->scale(); gttoc(sum); + + // Normalize/scale to prevent underflow. + gttic(scale); + DiscreteFactor::shared_ptr denominator = sum->max(sum->size()); + product = product->operator/(denominator); + sum = sum->operator/(denominator); + gttoc(scale); // Ordering keys for the conditional so that frontalKeys are really in front Ordering orderedKeys;