Fix bug

borglab · Jan 29, 2025 · 2702854 · 2702854
1 parent d4d95e2
commit 2702854
Showing 1 changed file with 10 additions and 3 deletions.
diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -214,14 +214,21 @@ namespace gtsam {
   std::pair<DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr>  //
   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;