2 GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
3 Atlanta, Georgia 30332-0415
6 See LICENSE for the license information
8 Unit tests for Hybrid Factor Graphs.
9 Author: Fan Jiang, Varun Agrawal, Frank Dellaert
20 from gtsam
import (DiscreteConditional, GaussianConditional,
21 HybridBayesNet, HybridGaussianConditional,
22 HybridGaussianFactor, HybridGaussianFactorGraph,
23 HybridValues, JacobianFactor, noiseModel)
25 DEBUG_MARGINALS =
False
29 """Unit tests for HybridGaussianFactorGraph."""
32 """Test construction of hybrid factor graph."""
33 model = noiseModel.Unit.Create(3)
45 hbn = hfg.eliminateSequential()
47 self.assertEqual(hbn.size(), 2)
49 hybridCond = hbn.at(0).inner()
50 self.assertIsInstance(hybridCond, HybridGaussianConditional)
51 self.assertEqual(
len(hybridCond.keys()), 2)
53 discrete_conditional = hbn.at(hbn.size() - 1).inner()
54 self.assertIsInstance(discrete_conditional, DiscreteConditional)
57 """Test construction of hybrid factor graph."""
58 model = noiseModel.Unit.Create(3)
73 hbn = hfg.eliminateSequential()
76 self.assertEqual(hv.atDiscrete(
C(0)), 1)
79 def tiny(num_measurements: int = 1,
80 prior_mean: float = 5.0,
81 prior_sigma: float = 0.5) -> HybridBayesNet:
83 Create a tiny two variable hybrid model which represents
84 the generative probability P(Z, x0, mode) = P(Z|x0, mode)P(x0)P(mode).
85 num_measurements: number of measurements in Z = {z0, z1...}
95 for i
in range(num_measurements):
96 conditional0 = GaussianConditional.FromMeanAndStddev(
Z(i),
100 conditional1 = GaussianConditional.FromMeanAndStddev(
Z(i),
108 prior_on_x0 = GaussianConditional.FromMeanAndStddev(
109 X(0), [prior_mean], prior_sigma)
110 bayesNet.push_back(prior_on_x0)
118 """Test evaluate with two different prior noise models."""
121 bayesNet1 = self.
tiny(prior_sigma=0.5, num_measurements=1)
122 bayesNet2 = self.
tiny(prior_sigma=5.0, num_measurements=1)
125 expected_ratio = np.sqrt(2 * np.pi * 5.0**2) / np.sqrt(
128 mean0.insert(
X(0), [5.0])
129 mean0.insert(
Z(0), [5.0])
130 mean0.insert(
M(0), 0)
131 self.assertAlmostEqual(bayesNet1.evaluate(mean0) /
132 bayesNet2.evaluate(mean0),
136 mean1.insert(
X(0), [5.0])
137 mean1.insert(
Z(0), [5.0])
138 mean1.insert(
M(0), 1)
139 self.assertAlmostEqual(bayesNet1.evaluate(mean1) /
140 bayesNet2.evaluate(mean1),
146 """Create measurements from a sample, grabbing Z(i) for indices."""
149 measurements.insert(
Z(i), sample.at(
Z(i)))
155 proposal_density: HybridBayesNet,
157 """Do importance sampling to estimate discrete marginal P(mode)."""
159 marginals = np.zeros((2, ))
163 proposed = proposal_density.sample()
164 target_proposed = target(proposed)
165 weight = target_proposed / proposal_density.evaluate(proposed)
166 marginals[proposed.atDiscrete(
M(0))] += weight
169 marginals /= marginals.sum()
173 """Test a tiny two variable hybrid model."""
176 bayesNet = self.
tiny(prior_sigma=prior_sigma)
180 values.insert(
X(0), [5.0])
181 values.insert(
M(0), 0)
183 measurements.insert(
Z(0), [5.0])
184 values.insert(measurements)
186 def unnormalized_posterior(x):
187 """Posterior is proportional to joint, centered at 5.0 as well."""
188 x.insert(measurements)
189 return bayesNet.evaluate(x)
192 posterior_information = 1 / (prior_sigma**2) + 1 / (0.5**2)
193 posterior_sigma = posterior_information**(-0.5)
194 proposal_density = self.
tiny(num_measurements=0,
196 prior_sigma=posterior_sigma)
200 proposal_density=proposal_density)
202 print(f
"True mode: {values.atDiscrete(M(0))}")
203 print(f
"P(mode=0; Z) = {marginals[0]}")
204 print(f
"P(mode=1; Z) = {marginals[1]}")
207 self.assertAlmostEqual(marginals[0], 0.74, delta=0.01)
208 self.assertAlmostEqual(marginals[1], 0.26, delta=0.01)
211 fg = bayesNet.toFactorGraph(measurements)
212 self.assertEqual(fg.size(), 3)
216 values.insert_or_assign(
M(0), mode)
217 self.assertAlmostEqual(
218 bayesNet.evaluate(values) / np.exp(-fg.error(values)),
220 self.assertAlmostEqual(bayesNet.error(values), fg.error(values))
223 posterior = fg.eliminateSequential()
225 def true_posterior(x):
226 """Posterior from elimination."""
227 x.insert(measurements)
228 return posterior.evaluate(x)
232 proposal_density=proposal_density)
234 print(f
"True mode: {values.atDiscrete(M(0))}")
235 print(f
"P(mode=0; z0) = {marginals[0]}")
236 print(f
"P(mode=1; z0) = {marginals[1]}")
239 self.assertAlmostEqual(marginals[0], 0.74, delta=0.01)
240 self.assertAlmostEqual(marginals[1], 0.26, delta=0.01)
244 fg: HybridGaussianFactorGraph, sample: HybridValues):
245 """Calculate ratio between Bayes net and factor graph."""
246 return bayesNet.evaluate(sample) / fg.probPrime(sample)
if \
247 fg.probPrime(sample) > 0
else 0
251 Given a tiny two variable hybrid model, with 2 measurements, test the
252 ratio of the bayes net model representing P(z,x,n)=P(z|x, n)P(x)P(n)
253 and the factor graph P(x, n | z)=P(x | n, z)P(n|z),
254 both of which represent the same posterior.
258 bayesNet = self.
tiny(prior_sigma=prior_sigma, num_measurements=2)
262 values.insert(
X(0), [5.0])
263 values.insert(
M(0), 0)
265 measurements.insert(
Z(0), [4.0])
266 measurements.insert(
Z(1), [6.0])
267 values.insert(measurements)
269 def unnormalized_posterior(x):
270 """Posterior is proportional to joint, centered at 5.0 as well."""
271 x.insert(measurements)
272 return bayesNet.evaluate(x)
275 posterior_information = 1 / (prior_sigma**2) + 2.0 / (3.0**2)
276 posterior_sigma = posterior_information**(-0.5)
277 proposal_density = self.
tiny(num_measurements=0,
279 prior_sigma=posterior_sigma)
283 proposal_density=proposal_density)
285 print(f
"True mode: {values.atDiscrete(M(0))}")
286 print(f
"P(mode=0; Z) = {marginals[0]}")
287 print(f
"P(mode=1; Z) = {marginals[1]}")
290 self.assertAlmostEqual(marginals[0], 0.23, delta=0.01)
291 self.assertAlmostEqual(marginals[1], 0.77, delta=0.01)
294 fg = bayesNet.toFactorGraph(measurements)
295 self.assertEqual(fg.size(), 4)
303 samples = bayesNet.sample()
304 samples.update(measurements)
308 self.assertAlmostEqual(ratio, expected_ratio)
311 posterior = fg.eliminateSequential()
319 samples = posterior.sample()
320 samples.insert(measurements)
324 self.assertAlmostEqual(ratio, expected_ratio)
327 if __name__ ==
"__main__":