fixes based on previous PR
							parent
							
								
									3ab91b27d5
								
							
						
					
					
						commit
						5ea63be8c5
					
				|  | @ -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 = | ||||
|  |  | |||
|  | @ -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 | ||||
|  |  | |||
|  | @ -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. | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue