diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 2e47793ef..fd78986df 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -138,8 +138,6 @@ void HybridBayesNet::updateDiscreteConditionals( HybridConditional::shared_ptr conditional = this->at(i); if (conditional->isDiscrete()) { auto discrete = conditional->asDiscrete(); - KeyVector frontals(discrete->frontals().begin(), - discrete->frontals().end()); // Apply prunerFunc to the underlying AlgebraicDecisionTree auto discreteTree = diff --git a/gtsam/hybrid/HybridValues.h b/gtsam/hybrid/HybridValues.h index 80c942a83..adeee5963 100644 --- a/gtsam/hybrid/HybridValues.h +++ b/gtsam/hybrid/HybridValues.h @@ -104,7 +104,7 @@ class GTSAM_EXPORT HybridValues { * @param j The index with which the value will be associated. */ void insert(Key j, const Vector& value) { continuous_.insert(j, value); } - // TODO(Shangjie)- update() and insert_or_assign() , similar to Values.h + // TODO(Shangjie)- insert_or_assign() , similar to Values.h /** * Read/write access to the discrete value with key \c j, throws diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py index 2ebc87971..40016c8bf 100644 --- a/python/gtsam/tests/test_HybridFactorGraph.py +++ b/python/gtsam/tests/test_HybridFactorGraph.py @@ -11,30 +11,20 @@ Author: Fan Jiang # pylint: disable=invalid-name, no-name-in-module, no-member import unittest -import math import numpy as np from gtsam.symbol_shorthand import C, M, X, Z from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam import ( - DecisionTreeFactor, - DiscreteConditional, - DiscreteKeys, - GaussianConditional, - GaussianMixture, - GaussianMixtureFactor, - HybridGaussianFactorGraph, - JacobianFactor, - Ordering, - noiseModel, -) +from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional, + GaussianMixture, GaussianMixtureFactor, + HybridGaussianFactorGraph, JacobianFactor, Ordering, + noiseModel) class TestHybridGaussianFactorGraph(GtsamTestCase): """Unit tests for HybridGaussianFactorGraph.""" - def test_create(self): """Test construction of hybrid factor graph.""" model = noiseModel.Unit.Create(3) @@ -52,8 +42,8 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): hfg.push_back(gmf) hbn = hfg.eliminateSequential( - Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)]) - ) + Ordering.ColamdConstrainedLastHybridGaussianFactorGraph( + hfg, [C(0)])) self.assertEqual(hbn.size(), 2) @@ -84,36 +74,39 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): hfg.push_back(dtf) hbn = hfg.eliminateSequential( - Ordering.ColamdConstrainedLastHybridGaussianFactorGraph(hfg, [C(0)]) - ) + Ordering.ColamdConstrainedLastHybridGaussianFactorGraph( + hfg, [C(0)])) hv = hbn.optimize() self.assertEqual(hv.atDiscrete(C(0)), 1) @staticmethod - def tiny(num_measurements: int = 1): - """Create a tiny two variable hybrid model.""" + def tiny(num_measurements: int = 1) -> gtsam.HybridBayesNet: + """ + Create a tiny two variable hybrid model which represents + the generative probability P(z, x, n) = P(z | x, n)P(x)P(n). + """ # Create hybrid Bayes net. bayesNet = gtsam.HybridBayesNet() # Create mode key: 0 is low-noise, 1 is high-noise. - modeKey = M(0) - mode = (modeKey, 2) + mode = (M(0), 2) # Create Gaussian mixture Z(0) = X(0) + noise for each measurement. I = np.eye(1) keys = DiscreteKeys() keys.push_back(mode) for i in range(num_measurements): - conditional0 = GaussianConditional.FromMeanAndStddev( - Z(i), I, X(0), [0], sigma=0.5 - ) - conditional1 = GaussianConditional.FromMeanAndStddev( - Z(i), I, X(0), [0], sigma=3 - ) - bayesNet.emplaceMixture( - [Z(i)], [X(0)], keys, [conditional0, conditional1] - ) + conditional0 = GaussianConditional.FromMeanAndStddev(Z(i), + I, + X(0), [0], + sigma=0.5) + conditional1 = GaussianConditional.FromMeanAndStddev(Z(i), + I, + X(0), [0], + sigma=3) + bayesNet.emplaceMixture([Z(i)], [X(0)], keys, + [conditional0, conditional1]) # Create prior on X(0). prior_on_x0 = GaussianConditional.FromMeanAndStddev(X(0), [5.0], 5.0) @@ -142,14 +135,22 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): self.assertEqual(fg.size(), 3) - def test_tiny2(self): - """Test a tiny two variable hybrid model, with 2 measurements.""" - # Create the Bayes net and sample from it. + def test_ratio(self): + """ + Given a tiny two variable hybrid model, with 2 measurements, + test the ratio of the bayes net model representing P(z, x, n)=P(z|x, n)P(x)P(n) + and the factor graph P(x, n | z)=P(x | n, z)P(n|z), + both of which represent the same posterior. + """ + # Create the Bayes net representing the generative model P(z, x, n)=P(z|x, n)P(x)P(n) bayesNet = self.tiny(num_measurements=2) - sample = bayesNet.sample() + # Sample from the Bayes net. + sample: gtsam.HybridValues = bayesNet.sample() # print(sample) # Create a factor graph from the Bayes net with sampled measurements. + # The factor graph is `P(x)P(n) ϕ(x, n; z1) ϕ(x, n; z2)` + # and thus represents the same joint probability as the Bayes net. fg = HybridGaussianFactorGraph() for i in range(2): conditional = bayesNet.atMixture(i) @@ -161,13 +162,14 @@ class TestHybridGaussianFactorGraph(GtsamTestCase): fg.push_back(bayesNet.atDiscrete(3)) self.assertEqual(fg.size(), 4) - # Calculate ratio between Bayes net probability and the factor graph: + + # Calculate ratio between Bayes net probability and the factor graph: continuousValues = gtsam.VectorValues() continuousValues.insert(X(0), sample.at(X(0))) discreteValues = sample.discrete() expected_ratio = bayesNet.evaluate(sample) / fg.probPrime( - continuousValues, discreteValues - ) + continuousValues, discreteValues) + #TODO(Varun) This should be 1. Adding the normalizing factor should fix fg.probPrime print(expected_ratio) # TODO(dellaert): Change the mode to 0 and calculate the ratio again.