fixes based on previous PR

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 =

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

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 (
+from gtsam import (DiscreteConditional, DiscreteKeys, GaussianConditional,
-    DecisionTreeFactor,
+                   GaussianMixture, GaussianMixtureFactor,
-    DiscreteConditional,
+                   HybridGaussianFactorGraph, JacobianFactor, Ordering,
-    DiscreteKeys,
+                   noiseModel)
-    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):
+    def tiny(num_measurements: int = 1) -> gtsam.HybridBayesNet:
-        """Create a tiny two variable hybrid model."""
+        """
+        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 = (M(0), 2)
-        mode = (modeKey, 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(
+            conditional0 = GaussianConditional.FromMeanAndStddev(Z(i),
-                Z(i), I, X(0), [0], sigma=0.5
+                                                                 I,
-            )
+                                                                 X(0), [0],
-            conditional1 = GaussianConditional.FromMeanAndStddev(
+                                                                 sigma=0.5)
-                Z(i), I, X(0), [0], sigma=3
+            conditional1 = GaussianConditional.FromMeanAndStddev(Z(i),
-            )
+                                                                 I,
-            bayesNet.emplaceMixture(
+                                                                 X(0), [0],
-                [Z(i)], [X(0)], keys, [conditional0, conditional1]
+                                                                 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):
+    def test_ratio(self):
-        """Test a tiny two variable hybrid model, with 2 measurements."""
+        """
-        # Create the Bayes net and sample from it.
+        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.