update HybridGaussianConditional to use single discrete parent

borglab · Sep 18, 2024 · fa35384 · fa35384
1 parent c1ebdb2
commit fa35384
Show file tree

Hide file tree

Showing 7 changed files with 19 additions and 29 deletions.
diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp
@@ -58,11 +58,11 @@ HybridGaussianConditional::conditionals() const {
 /* *******************************************************************************/
 HybridGaussianConditional::HybridGaussianConditional(
     const KeyVector &continuousFrontals, const KeyVector &continuousParents,
-    const DiscreteKeys &discreteParents,
+    const DiscreteKey &discreteParent,
     const std::vector<GaussianConditional::shared_ptr> &conditionals)
     : HybridGaussianConditional(continuousFrontals, continuousParents,
-                                discreteParents,
-                                Conditionals(discreteParents, conditionals)) {}
+                                DiscreteKeys{discreteParent},
+                                Conditionals({discreteParent}, conditionals)) {}
 
 /* *******************************************************************************/
 // TODO(dellaert): This is copy/paste: HybridGaussianConditional should be

diff --git a/gtsam/hybrid/HybridGaussianConditional.h b/gtsam/hybrid/HybridGaussianConditional.h
@@ -112,12 +112,13 @@ class GTSAM_EXPORT HybridGaussianConditional
    *
    * @param continuousFrontals The continuous frontal variables
    * @param continuousParents The continuous parent variables
-   * @param discreteParents Discrete parents variables
-   * @param conditionals Vector of conditionals
+   * @param discreteParent Single discrete parent variable
+   * @param conditionals Vector of conditionals with the same size as the
+   * cardinality of the discrete parent.
    */
   HybridGaussianConditional(
       const KeyVector &continuousFrontals, const KeyVector &continuousParents,
-      const DiscreteKeys &discreteParents,
+      const DiscreteKey &discreteParent,
       const std::vector<GaussianConditional::shared_ptr> &conditionals);
 
   /// @}

diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i
@@ -95,7 +95,7 @@ class HybridGaussianConditional : gtsam::HybridFactor {
   HybridGaussianConditional(
       const gtsam::KeyVector& continuousFrontals,
       const gtsam::KeyVector& continuousParents,
-      const gtsam::DiscreteKeys& discreteParents,
+      const gtsam::DiscreteKey& discreteParent,
       const std::vector<gtsam::GaussianConditional::shared_ptr>& conditionals);
 
   gtsam::HybridGaussianFactor* likelihood(

diff --git a/gtsam/hybrid/tests/TinyHybridExample.h b/gtsam/hybrid/tests/TinyHybridExample.h
@@ -43,13 +43,11 @@ inline HybridBayesNet createHybridBayesNet(size_t num_measurements = 1,
   // Create Gaussian mixture z_i = x0 + noise for each measurement.
   for (size_t i = 0; i < num_measurements; i++) {
     const auto mode_i = manyModes ? DiscreteKey{M(i), 2} : mode;
-    DiscreteKeys modes{mode_i};
     std::vector<GaussianConditional::shared_ptr> conditionals{
         GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 0.5),
         GaussianConditional::sharedMeanAndStddev(Z(i), I_1x1, X(0), Z_1x1, 3)};
     bayesNet.emplace_shared<HybridGaussianConditional>(
-        KeyVector{Z(i)}, KeyVector{X(0)}, DiscreteKeys{mode_i},
-        HybridGaussianConditional::Conditionals(modes, conditionals));
+        KeyVector{Z(i)}, KeyVector{X(0)}, mode_i, conditionals);
   }
 
   // Create prior on X(0).

diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp
@@ -107,11 +107,9 @@ TEST(HybridBayesNet, evaluateHybrid) {
   // Create hybrid Bayes net.
   HybridBayesNet bayesNet;
   bayesNet.push_back(continuousConditional);
-  DiscreteKeys discreteParents{Asia};
   bayesNet.emplace_shared<HybridGaussianConditional>(
-      KeyVector{X(1)}, KeyVector{}, discreteParents,
-      HybridGaussianConditional::Conditionals(
-          discreteParents, std::vector{conditional0, conditional1}));
+      KeyVector{X(1)}, KeyVector{}, Asia,
+      std::vector{conditional0, conditional1});
   bayesNet.emplace_shared<DiscreteConditional>(Asia, "99/1");
 
   // Create values at which to evaluate.
@@ -170,11 +168,9 @@ TEST(HybridBayesNet, Error) {
              conditional1 = std::make_shared<GaussianConditional>(
                  X(1), Vector1::Constant(2), I_1x1, model1);
 
-  DiscreteKeys discreteParents{Asia};
   auto gm = std::make_shared<HybridGaussianConditional>(
-      KeyVector{X(1)}, KeyVector{}, discreteParents,
-      HybridGaussianConditional::Conditionals(
-          discreteParents, std::vector{conditional0, conditional1}));
+      KeyVector{X(1)}, KeyVector{}, Asia,
+      std::vector{conditional0, conditional1});
   // Create hybrid Bayes net.
   HybridBayesNet bayesNet;
   bayesNet.push_back(continuousConditional);
@@ -393,11 +389,10 @@ TEST(HybridBayesNet, Sampling) {
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
   auto one_motion =
       std::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
-  DiscreteKeys discreteKeys{DiscreteKey(M(0), 2)};
-  HybridNonlinearFactor::Factors factors(
-      discreteKeys, {{zero_motion, 0.0}, {one_motion, 0.0}});
-  nfg.emplace_shared<HybridNonlinearFactor>(KeyVector{X(0), X(1)}, discreteKeys,
-                                            factors);
+  nfg.emplace_shared<HybridNonlinearFactor>(
+      KeyVector{X(0), X(1)}, DiscreteKey(M(0), 2),
+      std::vector<NonlinearFactorValuePair>{{zero_motion, 0.0},
+                                            {one_motion, 0.0}});
 
   DiscreteKey mode(M(0), 2);
   nfg.emplace_shared<DiscreteDistribution>(mode, "1/1");

diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py
@@ -43,14 +43,12 @@ def test_evaluate(self):
         # Create the conditionals
         conditional0 = GaussianConditional(X(1), [5], I_1x1, model0)
         conditional1 = GaussianConditional(X(1), [2], I_1x1, model1)
-        discrete_keys = DiscreteKeys()
-        discrete_keys.push_back(Asia)
 
         # Create hybrid Bayes net.
         bayesNet = HybridBayesNet()
         bayesNet.push_back(conditional)
         bayesNet.push_back(
-            HybridGaussianConditional([X(1)], [], discrete_keys,
+            HybridGaussianConditional([X(1)], [], Asia,
                                       [conditional0, conditional1]))
         bayesNet.push_back(DiscreteConditional(Asia, "99/1"))
 

diff --git a/python/gtsam/tests/test_HybridFactorGraph.py b/python/gtsam/tests/test_HybridFactorGraph.py
@@ -101,10 +101,8 @@ def tiny(num_measurements: int = 1,
                                                                  I_1x1,
                                                                  X(0), [0],
                                                                  sigma=3)
-            discreteParents = DiscreteKeys()
-            discreteParents.push_back(mode)
             bayesNet.push_back(
-                HybridGaussianConditional([Z(i)], [X(0)], discreteParents,
+                HybridGaussianConditional([Z(i)], [X(0)], mode,
                                           [conditional0, conditional1]))
 
         # Create prior on X(0).