From 148d99f1d19158b1cb0e9e72095465003e855964 Mon Sep 17 00:00:00 2001
From: Kevin <kdoherty@mit.edu>
Date: Tue, 7 Feb 2023 10:15:57 -0500
Subject: [PATCH 1/3] Add normalization to max-product, avoiding underflow.

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 5 +++++
 1 file changed, 5 insertions(+)

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);

From 571b0f5e90020747ca108d03686580777606725a Mon Sep 17 00:00:00 2001
From: Kevin <kdoherty@mit.edu>
Date: Tue, 7 Feb 2023 10:43:16 -0500
Subject: [PATCH 2/3] Add toy problem testing max-product underflow.

---
 .../gtsam/tests/test_DiscreteFactorGraph.py   | 62 ++++++++++++++++++-
 1 file changed, 61 insertions(+), 1 deletion(-)

diff --git a/python/gtsam/tests/test_DiscreteFactorGraph.py b/python/gtsam/tests/test_DiscreteFactorGraph.py
index ef85fc753..42dc807b0 100644
--- a/python/gtsam/tests/test_DiscreteFactorGraph.py
+++ b/python/gtsam/tests/test_DiscreteFactorGraph.py
@@ -13,7 +13,8 @@ Author: Frank Dellaert
 
 import unittest
 
-from gtsam import DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering
+import numpy as np
+from gtsam import DiscreteConditional, DiscreteFactorGraph, DiscreteKeys, DiscreteValues, Ordering, Symbol
 from gtsam.utils.test_case import GtsamTestCase
 
 OrderingType = Ordering.OrderingType
@@ -155,6 +156,65 @@ class TestDiscreteFactorGraph(GtsamTestCase):
             bayesNet = graph.sumProduct(ordering_type)
             self.assertEqual(bayesNet(mpe), mpeProbability)
 
+    def test_MPE_chain(self):
+        """
+        Test for numerical underflow in EliminateMPE on long chains.
+        Adapted from the toy problem of @pcl15423
+        Ref: https://github.com/borglab/gtsam/issues/1448
+        """
+        num_states = 3
+        num_obs = 200
+        desired_state = 1
+        states = list(range(num_states))
+
+        # Helper function to mimic the behavior of gtbook.Variables discrete_series function
+        def make_key(character, index, cardinality):
+            symbol = Symbol(character, index)
+            key = symbol.key()
+            return (key, cardinality)
+
+        X = {index: make_key("X", index, len(states)) for index in range(num_obs)}
+        Z = {index: make_key("Z", index, num_obs + 1) for index in range(num_obs)}
+        graph = DiscreteFactorGraph()
+
+        # Mostly identity transition matrix
+        transitions = np.eye(num_states)
+
+        # Needed otherwise mpe is always state 0?
+        transitions += 0.1/(num_states)
+
+        transition_cpt = []
+        for i in range(0, num_states):
+            transition_row = "/".join([str(x) for x in transitions[i]])
+            transition_cpt.append(transition_row)
+        transition_cpt = " ".join(transition_cpt)
+
+        for i in reversed(range(1, num_obs)):
+            transition_conditional = DiscreteConditional(X[i], [X[i-1]], transition_cpt)
+            graph.push_back(transition_conditional)
+
+        # Contrived example such that the desired state gives measurements [0, num_obs) with equal probability
+        #   but all other states always give measurement num_obs
+        obs = np.zeros((num_states, num_obs+1))
+        obs[:,-1] = 1
+        obs[desired_state,0: -1] = 1
+        obs[desired_state,-1] = 0
+        obs_cpt_list = []
+        for i in range(0, num_states):
+            obs_row = "/".join([str(z) for z in obs[i]])
+            obs_cpt_list.append(obs_row)
+        obs_cpt = " ".join(obs_cpt_list)
+
+        # Contrived example where each measurement is its own index
+        for i in range(0, num_obs):
+            obs_conditional = DiscreteConditional(Z[i], [X[i]], obs_cpt)
+            factor = obs_conditional.likelihood(i)
+            graph.push_back(factor)
+
+        mpe = graph.optimize()
+        vals = [mpe[X[i][0]] for i in range(num_obs)]
+
+        self.assertEqual(vals, [desired_state]*num_obs)
 
 if __name__ == "__main__":
     unittest.main()

From 9d0be8f2b2692f2ae4a81c2c1acd3dfa26f227a9 Mon Sep 17 00:00:00 2001
From: Kevin <kdoherty@mit.edu>
Date: Wed, 8 Feb 2023 08:55:03 -0500
Subject: [PATCH 3/3] Use simpler normalization for max-product.

---
 gtsam/discrete/DiscreteFactorGraph.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp
index 2ed499a4c..2073164c3 100644
--- a/gtsam/discrete/DiscreteFactorGraph.cpp
+++ b/gtsam/discrete/DiscreteFactorGraph.cpp
@@ -121,9 +121,10 @@ 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);
+    // Sum all the potentials by pretending all keys are frontal:
+    auto normalization = product.sum(product.size());
+
+    // Normalize the product factor to prevent underflow.
     product = product / (*normalization);
 
     // max out frontals, this is the factor on the separator