From 06887b702ab9f03852e7126b223135dd071b88c0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 25 Sep 2024 16:10:24 -0700 Subject: [PATCH 1/4] Moved mixture model tests to its own file --- gtsam/hybrid/tests/testGaussianMixture.cpp | 239 ++++++++++++++++++ .../hybrid/tests/testHybridGaussianFactor.cpp | 187 -------------- 2 files changed, 239 insertions(+), 187 deletions(-) create mode 100644 gtsam/hybrid/tests/testGaussianMixture.cpp diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp new file mode 100644 index 0000000000..7f2bd99f47 --- /dev/null +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -0,0 +1,239 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testHybridGaussianFactor.cpp + * @brief Unit tests for HybridGaussianFactor + * @author Varun Agrawal + * @author Fan Jiang + * @author Frank Dellaert + * @date December 2021 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Include for test suite +#include + +#include + +using namespace std; +using namespace gtsam; +using symbol_shorthand::M; +using symbol_shorthand::X; +using symbol_shorthand::Z; + +namespace test_gmm { + +/** + * Function to compute P(m=1|z). For P(m=0|z), swap mus and sigmas. + * If sigma0 == sigma1, it simplifies to a sigmoid function. + * + * Follows equation 7.108 since it is more generic. + */ +double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, + double z) { + double x1 = ((z - mu0) / sigma0), x2 = ((z - mu1) / sigma1); + double d = sigma1 / sigma0; + double e = d * std::exp(-0.5 * (x1 * x1 - x2 * x2)); + return 1 / (1 + e); +}; + +static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, + double sigma0, double sigma1) { + DiscreteKey m(M(0), 2); + Key z = Z(0); + + auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); + auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); + + auto c0 = make_shared(z, Vector1(mu0), I_1x1, model0), + c1 = make_shared(z, Vector1(mu1), I_1x1, model1); + + HybridBayesNet hbn; + DiscreteKeys discreteParents{m}; + hbn.emplace_shared( + KeyVector{z}, KeyVector{}, discreteParents, + HybridGaussianConditional::Conditionals(discreteParents, + std::vector{c0, c1})); + + auto mixing = make_shared(m, "50/50"); + hbn.push_back(mixing); + + return hbn; +} + +} // namespace test_gmm + +/* ************************************************************************* */ +/** + * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) + * where m is a discrete variable and z is a continuous variable. + * m is binary and depending on m, we have 2 different means + * μ1 and μ2 for the Gaussian distribution around which we sample z. + * + * The resulting factor graph should eliminate to a Bayes net + * which represents a sigmoid function. + */ +TEST(HybridGaussianFactor, GaussianMixtureModel) { + using namespace test_gmm; + + double mu0 = 1.0, mu1 = 3.0; + double sigma = 2.0; + + DiscreteKey m(M(0), 2); + Key z = Z(0); + + auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma, sigma); + + // The result should be a sigmoid. + // So should be P(m=1|z) = 0.5 at z=3.0 - 1.0=2.0 + double midway = mu1 - mu0, lambda = 4; + { + VectorValues given; + given.insert(z, Vector1(midway)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + EXPECT_DOUBLES_EQUAL( + prob_m_z(mu0, mu1, sigma, sigma, midway), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), + 1e-8); + + // At the halfway point between the means, we should get P(m|z)=0.5 + HybridBayesNet expected; + expected.emplace_shared(m, "50/50"); + + EXPECT(assert_equal(expected, *bn)); + } + { + // Shift by -lambda + VectorValues given; + given.insert(z, Vector1(midway - lambda)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + EXPECT_DOUBLES_EQUAL( + prob_m_z(mu0, mu1, sigma, sigma, midway - lambda), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), + 1e-8); + } + { + // Shift by lambda + VectorValues given; + given.insert(z, Vector1(midway + lambda)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + EXPECT_DOUBLES_EQUAL( + prob_m_z(mu0, mu1, sigma, sigma, midway + lambda), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), + 1e-8); + } +} + +/* ************************************************************************* */ +/** + * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) + * where m is a discrete variable and z is a continuous variable. + * m is binary and depending on m, we have 2 different means + * and covariances each for the + * Gaussian distribution around which we sample z. + * + * The resulting factor graph should eliminate to a Bayes net + * which represents a Gaussian-like function + * where m1>m0 close to 3.1333. + */ +TEST(HybridGaussianFactor, GaussianMixtureModel2) { + using namespace test_gmm; + + double mu0 = 1.0, mu1 = 3.0; + double sigma0 = 8.0, sigma1 = 4.0; + + DiscreteKey m(M(0), 2); + Key z = Z(0); + + auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma0, sigma1); + + double m1_high = 3.133, lambda = 4; + { + // The result should be a bell curve like function + // with m1 > m0 close to 3.1333. + // We get 3.1333 by finding the maximum value of the function. + VectorValues given; + given.insert(z, Vector1(3.133)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + EXPECT_DOUBLES_EQUAL( + prob_m_z(mu0, mu1, sigma0, sigma1, m1_high), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{M(0), 1}}), 1e-8); + + // At the halfway point between the means + HybridBayesNet expected; + expected.emplace_shared( + m, DiscreteKeys{}, + vector{prob_m_z(mu1, mu0, sigma1, sigma0, m1_high), + prob_m_z(mu0, mu1, sigma0, sigma1, m1_high)}); + + EXPECT(assert_equal(expected, *bn)); + } + { + // Shift by -lambda + VectorValues given; + given.insert(z, Vector1(m1_high - lambda)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + EXPECT_DOUBLES_EQUAL( + prob_m_z(mu0, mu1, sigma0, sigma1, m1_high - lambda), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), + 1e-8); + } + { + // Shift by lambda + VectorValues given; + given.insert(z, Vector1(m1_high + lambda)); + + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + + EXPECT_DOUBLES_EQUAL( + prob_m_z(mu0, mu1, sigma0, sigma1, m1_high + lambda), + bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), + 1e-8); + } +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index fff8e48fb1..55c3bccfb0 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -213,193 +213,6 @@ TEST(HybridGaussianFactor, Error) { 4.0, hybridFactor.error({continuousValues, discreteValues}), 1e-9); } -namespace test_gmm { - -/** - * Function to compute P(m=1|z). For P(m=0|z), swap mus and sigmas. - * If sigma0 == sigma1, it simplifies to a sigmoid function. - * - * Follows equation 7.108 since it is more generic. - */ -double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, - double z) { - double x1 = ((z - mu0) / sigma0), x2 = ((z - mu1) / sigma1); - double d = sigma1 / sigma0; - double e = d * std::exp(-0.5 * (x1 * x1 - x2 * x2)); - return 1 / (1 + e); -}; - -static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, - double sigma0, double sigma1) { - DiscreteKey m(M(0), 2); - Key z = Z(0); - - auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); - auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); - - auto c0 = make_shared(z, Vector1(mu0), I_1x1, model0), - c1 = make_shared(z, Vector1(mu1), I_1x1, model1); - - HybridBayesNet hbn; - DiscreteKeys discreteParents{m}; - hbn.emplace_shared( - KeyVector{z}, KeyVector{}, discreteParents, - HybridGaussianConditional::Conditionals(discreteParents, - std::vector{c0, c1})); - - auto mixing = make_shared(m, "50/50"); - hbn.push_back(mixing); - - return hbn; -} - -} // namespace test_gmm - -/* ************************************************************************* */ -/** - * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) - * where m is a discrete variable and z is a continuous variable. - * m is binary and depending on m, we have 2 different means - * μ1 and μ2 for the Gaussian distribution around which we sample z. - * - * The resulting factor graph should eliminate to a Bayes net - * which represents a sigmoid function. - */ -TEST(HybridGaussianFactor, GaussianMixtureModel) { - using namespace test_gmm; - - double mu0 = 1.0, mu1 = 3.0; - double sigma = 2.0; - - DiscreteKey m(M(0), 2); - Key z = Z(0); - - auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma, sigma); - - // The result should be a sigmoid. - // So should be P(m=1|z) = 0.5 at z=3.0 - 1.0=2.0 - double midway = mu1 - mu0, lambda = 4; - { - VectorValues given; - given.insert(z, Vector1(midway)); - - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - - EXPECT_DOUBLES_EQUAL( - prob_m_z(mu0, mu1, sigma, sigma, midway), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), - 1e-8); - - // At the halfway point between the means, we should get P(m|z)=0.5 - HybridBayesNet expected; - expected.emplace_shared(m, "50/50"); - - EXPECT(assert_equal(expected, *bn)); - } - { - // Shift by -lambda - VectorValues given; - given.insert(z, Vector1(midway - lambda)); - - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - - EXPECT_DOUBLES_EQUAL( - prob_m_z(mu0, mu1, sigma, sigma, midway - lambda), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), - 1e-8); - } - { - // Shift by lambda - VectorValues given; - given.insert(z, Vector1(midway + lambda)); - - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - - EXPECT_DOUBLES_EQUAL( - prob_m_z(mu0, mu1, sigma, sigma, midway + lambda), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), - 1e-8); - } -} - -/* ************************************************************************* */ -/** - * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) - * where m is a discrete variable and z is a continuous variable. - * m is binary and depending on m, we have 2 different means - * and covariances each for the - * Gaussian distribution around which we sample z. - * - * The resulting factor graph should eliminate to a Bayes net - * which represents a Gaussian-like function - * where m1>m0 close to 3.1333. - */ -TEST(HybridGaussianFactor, GaussianMixtureModel2) { - using namespace test_gmm; - - double mu0 = 1.0, mu1 = 3.0; - double sigma0 = 8.0, sigma1 = 4.0; - - DiscreteKey m(M(0), 2); - Key z = Z(0); - - auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma0, sigma1); - - double m1_high = 3.133, lambda = 4; - { - // The result should be a bell curve like function - // with m1 > m0 close to 3.1333. - // We get 3.1333 by finding the maximum value of the function. - VectorValues given; - given.insert(z, Vector1(3.133)); - - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - - EXPECT_DOUBLES_EQUAL( - prob_m_z(mu0, mu1, sigma0, sigma1, m1_high), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{M(0), 1}}), 1e-8); - - // At the halfway point between the means - HybridBayesNet expected; - expected.emplace_shared( - m, DiscreteKeys{}, - vector{prob_m_z(mu1, mu0, sigma1, sigma0, m1_high), - prob_m_z(mu0, mu1, sigma0, sigma1, m1_high)}); - - EXPECT(assert_equal(expected, *bn)); - } - { - // Shift by -lambda - VectorValues given; - given.insert(z, Vector1(m1_high - lambda)); - - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - - EXPECT_DOUBLES_EQUAL( - prob_m_z(mu0, mu1, sigma0, sigma1, m1_high - lambda), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), - 1e-8); - } - { - // Shift by lambda - VectorValues given; - given.insert(z, Vector1(m1_high + lambda)); - - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - - EXPECT_DOUBLES_EQUAL( - prob_m_z(mu0, mu1, sigma0, sigma1, m1_high + lambda), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), - 1e-8); - } -} - namespace test_two_state_estimation { DiscreteKey m1(M(1), 2); From 314a8310cfea1eabc432c78731171a727496d738 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 25 Sep 2024 16:55:12 -0700 Subject: [PATCH 2/4] Simplify tests --- gtsam/hybrid/tests/testGaussianMixture.cpp | 196 ++++++--------------- 1 file changed, 55 insertions(+), 141 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 7f2bd99f47..e976994f88 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -44,39 +44,39 @@ using symbol_shorthand::M; using symbol_shorthand::X; using symbol_shorthand::Z; -namespace test_gmm { - /** - * Function to compute P(m=1|z). For P(m=0|z), swap mus and sigmas. + * Closed form computation of P(m=1|z). * If sigma0 == sigma1, it simplifies to a sigmoid function. - * - * Follows equation 7.108 since it is more generic. */ -double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, - double z) { +static double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, + double z) { double x1 = ((z - mu0) / sigma0), x2 = ((z - mu1) / sigma1); double d = sigma1 / sigma0; double e = d * std::exp(-0.5 * (x1 * x1 - x2 * x2)); return 1 / (1 + e); }; +// Define mode key and an assignment m==1 +static const DiscreteKey m(M(0), 2); +static const DiscreteValues m1Assignment{{M(0), 1}}; + +/** + * Create a simple Gaussian Mixture Model represented as p(z|m)P(m) + * where m is a discrete variable and z is a continuous variable. + * The "mode" m is binary and depending on m, we have 2 different means + * μ1 and μ2 for the Gaussian density p(z|m). + */ static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, double sigma0, double sigma1) { - DiscreteKey m(M(0), 2); - Key z = Z(0); - auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); - auto c0 = make_shared(z, Vector1(mu0), I_1x1, model0), - c1 = make_shared(z, Vector1(mu1), I_1x1, model1); + auto c0 = make_shared(Z(0), Vector1(mu0), I_1x1, model0), + c1 = make_shared(Z(0), Vector1(mu1), I_1x1, model1); HybridBayesNet hbn; - DiscreteKeys discreteParents{m}; - hbn.emplace_shared( - KeyVector{z}, KeyVector{}, discreteParents, - HybridGaussianConditional::Conditionals(discreteParents, - std::vector{c0, c1})); + hbn.emplace_shared(KeyVector{Z(0)}, KeyVector{}, m, + std::vector{c0, c1}); auto mixing = make_shared(m, "50/50"); hbn.push_back(mixing); @@ -84,150 +84,64 @@ static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, return hbn; } -} // namespace test_gmm +/// Given p(z,m) and z, use eliminate to obtain P(m|z). +static DiscreteConditional solveForMeasurement(const HybridBayesNet &hbn, + double z) { + VectorValues given; + given.insert(Z(0), Vector1(z)); -/* ************************************************************************* */ -/** - * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) - * where m is a discrete variable and z is a continuous variable. - * m is binary and depending on m, we have 2 different means - * μ1 and μ2 for the Gaussian distribution around which we sample z. - * - * The resulting factor graph should eliminate to a Bayes net - * which represents a sigmoid function. + HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); + return *gfg.eliminateSequential()->at(0)->asDiscrete(); +} + +/* + * Test a Gaussian Mixture Model P(m)p(z|m) with same sigma. + * The posterior, as a function of z, should be a sigmoid function. */ TEST(HybridGaussianFactor, GaussianMixtureModel) { - using namespace test_gmm; - double mu0 = 1.0, mu1 = 3.0; double sigma = 2.0; - DiscreteKey m(M(0), 2); - Key z = Z(0); - auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma, sigma); - // The result should be a sigmoid. - // So should be P(m=1|z) = 0.5 at z=3.0 - 1.0=2.0 - double midway = mu1 - mu0, lambda = 4; - { - VectorValues given; - given.insert(z, Vector1(midway)); + // At the halfway point between the means, we should get P(m|z)=0.5 + double midway = mu1 - mu0; + auto pMid = solveForMeasurement(hbn, midway); + EXPECT(assert_equal(DiscreteConditional(m, "50/50"), pMid)); - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + // Everywhere else, the result should be a sigmoid. + for (const double shift : {-4, -2, 0, 2, 4}) { + const double z = midway + shift; + const double expected = prob_m_z(mu0, mu1, sigma, sigma, z); - EXPECT_DOUBLES_EQUAL( - prob_m_z(mu0, mu1, sigma, sigma, midway), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), - 1e-8); - - // At the halfway point between the means, we should get P(m|z)=0.5 - HybridBayesNet expected; - expected.emplace_shared(m, "50/50"); - - EXPECT(assert_equal(expected, *bn)); - } - { - // Shift by -lambda - VectorValues given; - given.insert(z, Vector1(midway - lambda)); - - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - - EXPECT_DOUBLES_EQUAL( - prob_m_z(mu0, mu1, sigma, sigma, midway - lambda), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), - 1e-8); - } - { - // Shift by lambda - VectorValues given; - given.insert(z, Vector1(midway + lambda)); - - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - - EXPECT_DOUBLES_EQUAL( - prob_m_z(mu0, mu1, sigma, sigma, midway + lambda), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), - 1e-8); + auto posterior = solveForMeasurement(hbn, z); + EXPECT_DOUBLES_EQUAL(expected, posterior(m1Assignment), 1e-8); } } -/* ************************************************************************* */ -/** - * Test a simple Gaussian Mixture Model represented as P(m)P(z|m) - * where m is a discrete variable and z is a continuous variable. - * m is binary and depending on m, we have 2 different means - * and covariances each for the - * Gaussian distribution around which we sample z. - * - * The resulting factor graph should eliminate to a Bayes net - * which represents a Gaussian-like function - * where m1>m0 close to 3.1333. +/* + * Test a Gaussian Mixture Model P(m)p(z|m) with different sigmas. + * The posterior, as a function of z, should be a unimodal function. */ TEST(HybridGaussianFactor, GaussianMixtureModel2) { - using namespace test_gmm; - double mu0 = 1.0, mu1 = 3.0; double sigma0 = 8.0, sigma1 = 4.0; - DiscreteKey m(M(0), 2); - Key z = Z(0); - auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma0, sigma1); - double m1_high = 3.133, lambda = 4; - { - // The result should be a bell curve like function - // with m1 > m0 close to 3.1333. - // We get 3.1333 by finding the maximum value of the function. - VectorValues given; - given.insert(z, Vector1(3.133)); - - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - - EXPECT_DOUBLES_EQUAL( - prob_m_z(mu0, mu1, sigma0, sigma1, m1_high), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{M(0), 1}}), 1e-8); - - // At the halfway point between the means - HybridBayesNet expected; - expected.emplace_shared( - m, DiscreteKeys{}, - vector{prob_m_z(mu1, mu0, sigma1, sigma0, m1_high), - prob_m_z(mu0, mu1, sigma0, sigma1, m1_high)}); - - EXPECT(assert_equal(expected, *bn)); - } - { - // Shift by -lambda - VectorValues given; - given.insert(z, Vector1(m1_high - lambda)); - - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - - EXPECT_DOUBLES_EQUAL( - prob_m_z(mu0, mu1, sigma0, sigma1, m1_high - lambda), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), - 1e-8); - } - { - // Shift by lambda - VectorValues given; - given.insert(z, Vector1(m1_high + lambda)); - - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); - - EXPECT_DOUBLES_EQUAL( - prob_m_z(mu0, mu1, sigma0, sigma1, m1_high + lambda), - bn->at(0)->asDiscrete()->operator()(DiscreteValues{{m.first, 1}}), - 1e-8); + // We get zMax=3.1333 by finding the maximum value of the function, at which + // point the mode m==1 is about twice as probable as m==0. + double zMax = 3.133; + auto pMax = solveForMeasurement(hbn, zMax); + EXPECT(assert_equal(DiscreteConditional(m, "32.56/67.44"), pMax, 1e-5)); + + // Everywhere else, the result should be a bell curve like function. + for (const double shift : {-4, -2, 0, 2, 4}) { + const double z = zMax + shift; + const double expected = prob_m_z(mu0, mu1, sigma0, sigma1, z); + + auto posterior = solveForMeasurement(hbn, z); + EXPECT_DOUBLES_EQUAL(expected, posterior(m1Assignment), 1e-8); } } From a4a0a2a4247bb6ba31b2b6b47d744e457c3b8e1d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 25 Sep 2024 18:48:01 -0700 Subject: [PATCH 3/4] Test different workflows --- gtsam/hybrid/tests/testGaussianMixture.cpp | 122 +++++++++++---------- 1 file changed, 67 insertions(+), 55 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index e976994f88..c6263c44dc 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -10,56 +10,35 @@ * -------------------------------------------------------------------------- */ /** - * @file testHybridGaussianFactor.cpp - * @brief Unit tests for HybridGaussianFactor + * @file testGaussianMixture.cpp + * @brief test hybrid elimination with a simple mixture model * @author Varun Agrawal - * @author Fan Jiang * @author Frank Dellaert - * @date December 2021 + * @date September 2024 */ -#include -#include #include -#include #include -#include -#include -#include -#include #include -#include -#include -#include -#include // Include for test suite #include -#include - -using namespace std; using namespace gtsam; using symbol_shorthand::M; -using symbol_shorthand::X; using symbol_shorthand::Z; -/** - * Closed form computation of P(m=1|z). - * If sigma0 == sigma1, it simplifies to a sigmoid function. - */ -static double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, - double z) { - double x1 = ((z - mu0) / sigma0), x2 = ((z - mu1) / sigma1); - double d = sigma1 / sigma0; - double e = d * std::exp(-0.5 * (x1 * x1 - x2 * x2)); - return 1 / (1 + e); -}; - // Define mode key and an assignment m==1 static const DiscreteKey m(M(0), 2); static const DiscreteValues m1Assignment{{M(0), 1}}; +// Define a 50/50 prior on the mode +DiscreteConditional::shared_ptr mixing = + std::make_shared(m, "60/40"); + +// define Continuous keys +static const KeyVector continuousKeys{Z(0)}; + /** * Create a simple Gaussian Mixture Model represented as p(z|m)P(m) * where m is a discrete variable and z is a continuous variable. @@ -68,30 +47,45 @@ static const DiscreteValues m1Assignment{{M(0), 1}}; */ static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, double sigma0, double sigma1) { + HybridBayesNet hbn; auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); - - auto c0 = make_shared(Z(0), Vector1(mu0), I_1x1, model0), - c1 = make_shared(Z(0), Vector1(mu1), I_1x1, model1); - - HybridBayesNet hbn; - hbn.emplace_shared(KeyVector{Z(0)}, KeyVector{}, m, + auto c0 = std::make_shared(Z(0), Vector1(mu0), I_1x1, + model0), + c1 = std::make_shared(Z(0), Vector1(mu1), I_1x1, + model1); + hbn.emplace_shared(continuousKeys, KeyVector{}, m, std::vector{c0, c1}); - - auto mixing = make_shared(m, "50/50"); hbn.push_back(mixing); - return hbn; } -/// Given p(z,m) and z, use eliminate to obtain P(m|z). -static DiscreteConditional solveForMeasurement(const HybridBayesNet &hbn, - double z) { - VectorValues given; - given.insert(Z(0), Vector1(z)); +/// Gaussian density function +double Gaussian(double mu, double sigma, double z) { + return exp(-0.5 * pow((z - mu) / sigma, 2)) / sqrt(2 * M_PI * sigma * sigma); +}; + +/** + * Closed form computation of P(m=1|z). + * If sigma0 == sigma1, it simplifies to a sigmoid function. + * Hardcodes 60/40 prior on mode. + */ +static double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, + double z) { + const double p0 = 0.6 * Gaussian(mu0, sigma0, z); + const double p1 = 0.4 * Gaussian(mu1, sigma1, z); + return p1 / (p0 + p1); +}; + +/// Given \phi(m;z)\phi(m) use eliminate to obtain P(m|z). +static DiscreteConditional solveHFG(const HybridGaussianFactorGraph &hfg) { + return *hfg.eliminateSequential()->at(0)->asDiscrete(); +} - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - return *gfg.eliminateSequential()->at(0)->asDiscrete(); +/// Given p(z,m) and z, convert to HFG and solve. +static DiscreteConditional solveHBN(const HybridBayesNet &hbn, double z) { + VectorValues given{{Z(0), Vector1(z)}}; + return solveHFG(hbn.toFactorGraph(given)); } /* @@ -106,16 +100,25 @@ TEST(HybridGaussianFactor, GaussianMixtureModel) { // At the halfway point between the means, we should get P(m|z)=0.5 double midway = mu1 - mu0; - auto pMid = solveForMeasurement(hbn, midway); - EXPECT(assert_equal(DiscreteConditional(m, "50/50"), pMid)); + auto pMid = solveHBN(hbn, midway); + EXPECT(assert_equal(DiscreteConditional(m, "60/40"), pMid)); // Everywhere else, the result should be a sigmoid. for (const double shift : {-4, -2, 0, 2, 4}) { const double z = midway + shift; const double expected = prob_m_z(mu0, mu1, sigma, sigma, z); - auto posterior = solveForMeasurement(hbn, z); - EXPECT_DOUBLES_EQUAL(expected, posterior(m1Assignment), 1e-8); + // Workflow 1: convert HBN to HFG and solve + auto posterior1 = solveHBN(hbn, z); + EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); + + // Workflow 2: directly specify HFG and solve + HybridGaussianFactorGraph hfg1; + hfg1.emplace_shared( + m, std::vector{Gaussian(mu0, sigma, z), Gaussian(mu1, sigma, z)}); + hfg1.push_back(mixing); + auto posterior2 = solveHFG(hfg1); + EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8); } } @@ -132,16 +135,25 @@ TEST(HybridGaussianFactor, GaussianMixtureModel2) { // We get zMax=3.1333 by finding the maximum value of the function, at which // point the mode m==1 is about twice as probable as m==0. double zMax = 3.133; - auto pMax = solveForMeasurement(hbn, zMax); - EXPECT(assert_equal(DiscreteConditional(m, "32.56/67.44"), pMax, 1e-5)); + auto pMax = solveHBN(hbn, zMax); + EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4)); // Everywhere else, the result should be a bell curve like function. for (const double shift : {-4, -2, 0, 2, 4}) { const double z = zMax + shift; const double expected = prob_m_z(mu0, mu1, sigma0, sigma1, z); - auto posterior = solveForMeasurement(hbn, z); - EXPECT_DOUBLES_EQUAL(expected, posterior(m1Assignment), 1e-8); + // Workflow 1: convert HBN to HFG and solve + auto posterior1 = solveHBN(hbn, z); + EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); + + // Workflow 2: directly specify HFG and solve + HybridGaussianFactorGraph hfg; + hfg.emplace_shared( + m, std::vector{Gaussian(mu0, sigma0, z), Gaussian(mu1, sigma1, z)}); + hfg.push_back(mixing); + auto posterior2 = solveHFG(hfg); + EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8); } } From 4d10c1462b1d7966071384b4b877c50e4d187ad6 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 25 Sep 2024 18:54:53 -0700 Subject: [PATCH 4/4] Include what you use --- gtsam/hybrid/tests/testGaussianMixture.cpp | 49 ++++++++++++---------- 1 file changed, 28 insertions(+), 21 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index c6263c44dc..be4ba2ff4d 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -11,15 +11,22 @@ /** * @file testGaussianMixture.cpp - * @brief test hybrid elimination with a simple mixture model + * @brief Test hybrid elimination with a simple mixture model * @author Varun Agrawal * @author Frank Dellaert * @date September 2024 */ +#include #include +#include #include +#include +#include +#include #include +#include +#include // Include for test suite #include @@ -29,15 +36,15 @@ using symbol_shorthand::M; using symbol_shorthand::Z; // Define mode key and an assignment m==1 -static const DiscreteKey m(M(0), 2); -static const DiscreteValues m1Assignment{{M(0), 1}}; +const DiscreteKey m(M(0), 2); +const DiscreteValues m1Assignment{{M(0), 1}}; // Define a 50/50 prior on the mode DiscreteConditional::shared_ptr mixing = std::make_shared(m, "60/40"); // define Continuous keys -static const KeyVector continuousKeys{Z(0)}; +const KeyVector continuousKeys{Z(0)}; /** * Create a simple Gaussian Mixture Model represented as p(z|m)P(m) @@ -45,8 +52,8 @@ static const KeyVector continuousKeys{Z(0)}; * The "mode" m is binary and depending on m, we have 2 different means * μ1 and μ2 for the Gaussian density p(z|m). */ -static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, - double sigma0, double sigma1) { +HybridBayesNet GaussianMixtureModel(double mu0, double mu1, double sigma0, + double sigma1) { HybridBayesNet hbn; auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); @@ -70,37 +77,37 @@ double Gaussian(double mu, double sigma, double z) { * If sigma0 == sigma1, it simplifies to a sigmoid function. * Hardcodes 60/40 prior on mode. */ -static double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, - double z) { +double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, + double z) { const double p0 = 0.6 * Gaussian(mu0, sigma0, z); const double p1 = 0.4 * Gaussian(mu1, sigma1, z); return p1 / (p0 + p1); }; /// Given \phi(m;z)\phi(m) use eliminate to obtain P(m|z). -static DiscreteConditional solveHFG(const HybridGaussianFactorGraph &hfg) { +DiscreteConditional SolveHFG(const HybridGaussianFactorGraph &hfg) { return *hfg.eliminateSequential()->at(0)->asDiscrete(); } /// Given p(z,m) and z, convert to HFG and solve. -static DiscreteConditional solveHBN(const HybridBayesNet &hbn, double z) { +DiscreteConditional SolveHBN(const HybridBayesNet &hbn, double z) { VectorValues given{{Z(0), Vector1(z)}}; - return solveHFG(hbn.toFactorGraph(given)); + return SolveHFG(hbn.toFactorGraph(given)); } /* * Test a Gaussian Mixture Model P(m)p(z|m) with same sigma. * The posterior, as a function of z, should be a sigmoid function. */ -TEST(HybridGaussianFactor, GaussianMixtureModel) { +TEST(GaussianMixture, GaussianMixtureModel) { double mu0 = 1.0, mu1 = 3.0; double sigma = 2.0; - auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma, sigma); + auto hbn = GaussianMixtureModel(mu0, mu1, sigma, sigma); // At the halfway point between the means, we should get P(m|z)=0.5 double midway = mu1 - mu0; - auto pMid = solveHBN(hbn, midway); + auto pMid = SolveHBN(hbn, midway); EXPECT(assert_equal(DiscreteConditional(m, "60/40"), pMid)); // Everywhere else, the result should be a sigmoid. @@ -109,7 +116,7 @@ TEST(HybridGaussianFactor, GaussianMixtureModel) { const double expected = prob_m_z(mu0, mu1, sigma, sigma, z); // Workflow 1: convert HBN to HFG and solve - auto posterior1 = solveHBN(hbn, z); + auto posterior1 = SolveHBN(hbn, z); EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); // Workflow 2: directly specify HFG and solve @@ -117,7 +124,7 @@ TEST(HybridGaussianFactor, GaussianMixtureModel) { hfg1.emplace_shared( m, std::vector{Gaussian(mu0, sigma, z), Gaussian(mu1, sigma, z)}); hfg1.push_back(mixing); - auto posterior2 = solveHFG(hfg1); + auto posterior2 = SolveHFG(hfg1); EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8); } } @@ -126,16 +133,16 @@ TEST(HybridGaussianFactor, GaussianMixtureModel) { * Test a Gaussian Mixture Model P(m)p(z|m) with different sigmas. * The posterior, as a function of z, should be a unimodal function. */ -TEST(HybridGaussianFactor, GaussianMixtureModel2) { +TEST(GaussianMixture, GaussianMixtureModel2) { double mu0 = 1.0, mu1 = 3.0; double sigma0 = 8.0, sigma1 = 4.0; - auto hbn = GetGaussianMixtureModel(mu0, mu1, sigma0, sigma1); + auto hbn = GaussianMixtureModel(mu0, mu1, sigma0, sigma1); // We get zMax=3.1333 by finding the maximum value of the function, at which // point the mode m==1 is about twice as probable as m==0. double zMax = 3.133; - auto pMax = solveHBN(hbn, zMax); + auto pMax = SolveHBN(hbn, zMax); EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4)); // Everywhere else, the result should be a bell curve like function. @@ -144,7 +151,7 @@ TEST(HybridGaussianFactor, GaussianMixtureModel2) { const double expected = prob_m_z(mu0, mu1, sigma0, sigma1, z); // Workflow 1: convert HBN to HFG and solve - auto posterior1 = solveHBN(hbn, z); + auto posterior1 = SolveHBN(hbn, z); EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8); // Workflow 2: directly specify HFG and solve @@ -152,7 +159,7 @@ TEST(HybridGaussianFactor, GaussianMixtureModel2) { hfg.emplace_shared( m, std::vector{Gaussian(mu0, sigma0, z), Gaussian(mu1, sigma1, z)}); hfg.push_back(mixing); - auto posterior2 = solveHFG(hfg); + auto posterior2 = SolveHFG(hfg); EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8); } }