Show that ratio is different for different modes.

python/gtsam/tests/test_HybridFactorGraph.py

@@ -142,12 +142,21 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
 
         self.assertEqual(fg.size(), 3)
 
+    @staticmethod
+    def calculate_ratio(bayesNet, fg, sample):
+        """Calculate ratio  between Bayes net probability and the factor graph."""
+        continuous = gtsam.VectorValues()
+        continuous.insert(X(0), sample.at(X(0)))
+        return bayesNet.evaluate(sample) / fg.probPrime(
+            continuous, sample.discrete()
+        )
+
     def test_tiny2(self):
         """Test a tiny two variable hybrid model, with 2 measurements."""
         # Create the Bayes net and sample from it.
         bayesNet = self.tiny(num_measurements=2)
         sample = bayesNet.sample()
-        # print(sample)
+        print(sample)
 
         # Create a factor graph from the Bayes net with sampled measurements.
         fg = HybridGaussianFactorGraph()
@@ -160,17 +169,26 @@ class TestHybridGaussianFactorGraph(GtsamTestCase):
         fg.push_back(bayesNet.atGaussian(2))
         fg.push_back(bayesNet.atDiscrete(3))
 
+        print(fg)
         self.assertEqual(fg.size(), 4)
-        # 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
-        )
-        print(expected_ratio)
 
-        # TODO(dellaert): Change the mode to 0 and calculate the ratio again.
+        # Calculate ratio between Bayes net probability and the factor graph:
+        expected_ratio = self.calculate_ratio(bayesNet, fg, sample)
+        print(f"expected_ratio: {expected_ratio}\n")
+
+        # Create measurements from the sample.
+        measurements = gtsam.VectorValues()
+        for i in range(2):
+            measurements.insert(Z(i), sample.at(Z(i)))
+
+        # Check with a number of other samples.
+        for i in range(10):
+            other = bayesNet.sample()
+            other.update(measurements)
+            print(other)
+            ratio = self.calculate_ratio(bayesNet, fg, other)
+            print(f"Ratio: {ratio}\n")
+            self.assertAlmostEqual(ratio, expected_ratio)
 
 
 if __name__ == "__main__":