258 lines
8.1 KiB
Python
258 lines
8.1 KiB
Python
"""
|
|
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
|
|
Atlanta, Georgia 30332-0415
|
|
All Rights Reserved
|
|
|
|
See LICENSE for the license information
|
|
|
|
Unit tests for Discrete Factor Graphs.
|
|
Author: Frank Dellaert
|
|
"""
|
|
|
|
# pylint: disable=no-name-in-module, invalid-name
|
|
|
|
import unittest
|
|
|
|
import numpy as np
|
|
from gtsam.utils.test_case import GtsamTestCase
|
|
from dfg_utils import make_key, generate_transition_cpt, generate_observation_cpt
|
|
|
|
from gtsam import (
|
|
DecisionTreeFactor,
|
|
DiscreteConditional,
|
|
DiscreteFactorGraph,
|
|
DiscreteKeys,
|
|
DiscreteValues,
|
|
Ordering,
|
|
)
|
|
|
|
OrderingType = Ordering.OrderingType
|
|
|
|
|
|
class TestDiscreteFactorGraph(GtsamTestCase):
|
|
"""Tests for Discrete Factor Graphs."""
|
|
|
|
def test_evaluation(self):
|
|
"""Test constructing and evaluating a discrete factor graph."""
|
|
|
|
# Three keys
|
|
P1 = (0, 2)
|
|
P2 = (1, 2)
|
|
P3 = (2, 3)
|
|
|
|
# Create the DiscreteFactorGraph
|
|
graph = DiscreteFactorGraph()
|
|
|
|
# Add two unary factors (priors)
|
|
graph.add(P1, [0.9, 0.3])
|
|
graph.add(P2, "0.9 0.6")
|
|
|
|
# Add a binary factor
|
|
graph.add([P1, P2], "4 1 10 4")
|
|
|
|
# Instantiate Values
|
|
assignment = DiscreteValues()
|
|
assignment[0] = 1
|
|
assignment[1] = 1
|
|
|
|
# Check if graph evaluation works ( 0.3*0.6*4 )
|
|
self.assertAlmostEqual(0.72, graph(assignment))
|
|
|
|
# Create a new test with third node and adding unary and ternary factor
|
|
graph.add(P3, "0.9 0.2 0.5")
|
|
keys = DiscreteKeys()
|
|
keys.push_back(P1)
|
|
keys.push_back(P2)
|
|
keys.push_back(P3)
|
|
graph.add(keys, "1 2 3 4 5 6 7 8 9 10 11 12")
|
|
|
|
# Below assignment selects the 8th index in the ternary factor table
|
|
assignment[0] = 1
|
|
assignment[1] = 0
|
|
assignment[2] = 1
|
|
|
|
# Check if graph evaluation works (0.3*0.9*1*0.2*8)
|
|
self.assertAlmostEqual(4.32, graph(assignment))
|
|
|
|
# Below assignment selects the 3rd index in the ternary factor table
|
|
assignment[0] = 0
|
|
assignment[1] = 1
|
|
assignment[2] = 0
|
|
|
|
# Check if graph evaluation works (0.9*0.6*1*0.9*4)
|
|
self.assertAlmostEqual(1.944, graph(assignment))
|
|
|
|
# Check if graph product works
|
|
product = graph.product()
|
|
self.assertAlmostEqual(1.944, product(assignment))
|
|
|
|
def test_optimize(self):
|
|
"""Test constructing and optizing a discrete factor graph."""
|
|
|
|
# Three keys
|
|
C = (0, 2)
|
|
B = (1, 2)
|
|
A = (2, 2)
|
|
|
|
# A simple factor graph (A)-fAC-(C)-fBC-(B)
|
|
# with smoothness priors
|
|
graph = DiscreteFactorGraph()
|
|
graph.add([A, C], "3 1 1 3")
|
|
graph.add([C, B], "3 1 1 3")
|
|
|
|
# Test optimization
|
|
expectedValues = DiscreteValues()
|
|
expectedValues[0] = 0
|
|
expectedValues[1] = 0
|
|
expectedValues[2] = 0
|
|
actualValues = graph.optimize()
|
|
self.assertEqual(list(actualValues.items()), list(expectedValues.items()))
|
|
|
|
def test_MPE(self):
|
|
"""Test maximum probable explanation (MPE): same as optimize."""
|
|
|
|
# Declare a bunch of keys
|
|
C, A, B = (0, 2), (1, 2), (2, 2)
|
|
|
|
# Create Factor graph
|
|
graph = DiscreteFactorGraph()
|
|
graph.add([C, A], "0.2 0.8 0.3 0.7")
|
|
graph.add([C, B], "0.1 0.9 0.4 0.6")
|
|
|
|
# We know MPE
|
|
mpe = DiscreteValues()
|
|
mpe[0] = 0
|
|
mpe[1] = 1
|
|
mpe[2] = 1
|
|
|
|
# Use maxProduct
|
|
dag = graph.maxProduct(OrderingType.COLAMD)
|
|
actualMPE = dag.argmax()
|
|
self.assertEqual(list(actualMPE.items()), list(mpe.items()))
|
|
|
|
# All in one
|
|
actualMPE2 = graph.optimize()
|
|
self.assertEqual(list(actualMPE2.items()), list(mpe.items()))
|
|
|
|
def test_sumProduct(self):
|
|
"""Test sumProduct."""
|
|
|
|
# Declare a bunch of keys
|
|
C, A, B = (0, 2), (1, 2), (2, 2)
|
|
|
|
# Create Factor graph
|
|
graph = DiscreteFactorGraph()
|
|
graph.add([C, A], "0.2 0.8 0.3 0.7")
|
|
graph.add([C, B], "0.1 0.9 0.4 0.6")
|
|
|
|
# We know MPE
|
|
mpe = DiscreteValues()
|
|
mpe[0] = 0
|
|
mpe[1] = 1
|
|
mpe[2] = 1
|
|
|
|
# Use default sumProduct
|
|
bayesNet = graph.sumProduct()
|
|
mpeProbability = bayesNet(mpe)
|
|
self.assertAlmostEqual(mpeProbability, 0.36) # regression
|
|
|
|
# Use sumProduct
|
|
for ordering_type in [
|
|
OrderingType.COLAMD,
|
|
OrderingType.METIS,
|
|
OrderingType.NATURAL,
|
|
OrderingType.CUSTOM,
|
|
]:
|
|
bayesNet = graph.sumProduct(ordering_type)
|
|
self.assertEqual(bayesNet(mpe), mpeProbability)
|
|
|
|
|
|
class TestChains(GtsamTestCase):
|
|
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))
|
|
|
|
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()
|
|
|
|
transition_cpt = generate_transition_cpt(num_states)
|
|
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_cpt = generate_observation_cpt(num_states, num_obs, desired_state)
|
|
# Contrived example where each measurement is its own index
|
|
for i in range(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)
|
|
|
|
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
|
|
chain_length = 400
|
|
states = list(range(num_states))
|
|
|
|
X = {index: make_key("X", index, len(states)) for index in range(chain_length)}
|
|
graph = DiscreteFactorGraph()
|
|
|
|
# Construct test transition matrix
|
|
transitions = np.diag([1.0, 0.5, 0.1])
|
|
transitions += 0.1/(num_states)
|
|
|
|
# Ensure that the transition matrix is Markov (columns sum to 1)
|
|
transitions /= np.sum(transitions, axis=0)
|
|
|
|
# The stationary distribution is the eigenvector corresponding to eigenvalue 1
|
|
eigvals, eigvecs = np.linalg.eig(transitions)
|
|
stationary_idx = np.where(np.isclose(eigvals, 1.0))
|
|
stationary_dist = eigvecs[:, stationary_idx]
|
|
|
|
# Ensure that the stationary distribution is positive and normalized
|
|
stationary_dist /= np.sum(stationary_dist)
|
|
expected = DecisionTreeFactor(X[chain_length - 1], stationary_dist.ravel())
|
|
|
|
# The transition matrix parsed by DiscreteConditional is a row-wise CPT
|
|
transition_cpt = generate_transition_cpt(num_states, transitions.T)
|
|
|
|
for i in reversed(range(1, chain_length)):
|
|
transition_conditional = DiscreteConditional(
|
|
X[i], [X[i - 1]], transition_cpt
|
|
)
|
|
graph.push_back(transition_conditional)
|
|
|
|
# Run sum product using natural ordering so the resulting Bayes net has the form:
|
|
# X_0 <- X_1 <- ... <- X_n
|
|
sum_product = graph.sumProduct(OrderingType.NATURAL)
|
|
|
|
# Get the DiscreteConditional representing the marginal on the last factor
|
|
last_marginal = sum_product.at(chain_length - 1)
|
|
|
|
# Ensure marginal probabilities are close to the stationary distribution
|
|
self.gtsamAssertEquals(expected, last_marginal)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|