diff --git a/gtsam/discrete/DiscreteFactorGraph.cpp b/gtsam/discrete/DiscreteFactorGraph.cpp index 2073164c3..7d07043b2 100644 --- a/gtsam/discrete/DiscreteFactorGraph.cpp +++ b/gtsam/discrete/DiscreteFactorGraph.cpp @@ -210,6 +210,12 @@ namespace gtsam { for (auto&& factor : factors) product = (*factor) * product; gttoc(product); + // 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); + // sum out frontals, this is the factor on the separator gttic(sum); DecisionTreeFactor::shared_ptr sum = product.sum(frontalKeys); diff --git a/python/gtsam/tests/test_DiscreteFactorGraph.py b/python/gtsam/tests/test_DiscreteFactorGraph.py index 42dc807b0..6990a995e 100644 --- a/python/gtsam/tests/test_DiscreteFactorGraph.py +++ b/python/gtsam/tests/test_DiscreteFactorGraph.py @@ -216,5 +216,70 @@ class TestDiscreteFactorGraph(GtsamTestCase): self.assertEqual(vals, [desired_state]*num_obs) + def test_sumProduct_chain(self): + """ + Test for numerical underflow in EliminateDiscrete 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)] + sum_product = graph.sumProduct() + + print("This should have 9 potential assignments", sum_product.at(0)) + + print("This should have 9 potential assignments", sum_product.at(138)) + + self.assertEqual(vals, [desired_state]*num_obs) + if __name__ == "__main__": unittest.main()