From fa353840b3d0dd16574cf5dadce5a11d6da4ff2c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 18 Sep 2024 15:08:09 -0400 Subject: [PATCH] update HybridGaussianConditional to use single discrete parent --- gtsam/hybrid/HybridGaussianConditional.cpp | 6 +++--- gtsam/hybrid/HybridGaussianConditional.h | 7 ++++--- gtsam/hybrid/hybrid.i | 2 +- gtsam/hybrid/tests/TinyHybridExample.h | 4 +--- gtsam/hybrid/tests/testHybridBayesNet.cpp | 21 ++++++++------------ python/gtsam/tests/test_HybridBayesNet.py | 4 +--- python/gtsam/tests/test_HybridFactorGraph.py | 4 +--- 7 files changed, 19 insertions(+), 29 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianConditional.cpp b/gtsam/hybrid/HybridGaussianConditional.cpp index a8ed7eed13..4c2739c865 100644 --- 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 &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 index 9cd13cbec8..240160744b 100644 --- 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 &conditionals); /// @} diff --git a/gtsam/hybrid/hybrid.i b/gtsam/hybrid/hybrid.i index 485d108487..37bcefe811 100644 --- 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& conditionals); gtsam::HybridGaussianFactor* likelihood( diff --git a/gtsam/hybrid/tests/TinyHybridExample.h b/gtsam/hybrid/tests/TinyHybridExample.h index b2b944f91e..b3bb14c8f8 100644 --- 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 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( - 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 index 462ce8625a..bbe2d3df51 100644 --- 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( - KeyVector{X(1)}, KeyVector{}, discreteParents, - HybridGaussianConditional::Conditionals( - discreteParents, std::vector{conditional0, conditional1})); + KeyVector{X(1)}, KeyVector{}, Asia, + std::vector{conditional0, conditional1}); bayesNet.emplace_shared(Asia, "99/1"); // Create values at which to evaluate. @@ -170,11 +168,9 @@ TEST(HybridBayesNet, Error) { conditional1 = std::make_shared( X(1), Vector1::Constant(2), I_1x1, model1); - DiscreteKeys discreteParents{Asia}; auto gm = std::make_shared( - 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>(X(0), X(1), 0, noise_model); auto one_motion = std::make_shared>(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(KeyVector{X(0), X(1)}, discreteKeys, - factors); + nfg.emplace_shared( + KeyVector{X(0), X(1)}, DiscreteKey(M(0), 2), + std::vector{{zero_motion, 0.0}, + {one_motion, 0.0}}); DiscreteKey mode(M(0), 2); nfg.emplace_shared(mode, "1/1"); diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py index 6e50c2bfe0..2f7dd4a987 100644 --- 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 index ce71d75ed8..5eafc05aed 100644 --- 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).