diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp new file mode 100644 index 0000000000..be4ba2ff4d --- /dev/null +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -0,0 +1,172 @@ +/* ---------------------------------------------------------------------------- + + * 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 testGaussianMixture.cpp + * @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 + +using namespace gtsam; +using symbol_shorthand::M; +using symbol_shorthand::Z; + +// Define mode key and an assignment m==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 +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. + * The "mode" m is binary and depending on m, we have 2 different means + * μ1 and μ2 for the Gaussian density p(z|m). + */ +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); + 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}); + hbn.push_back(mixing); + return hbn; +} + +/// 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. + */ +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). +DiscreteConditional SolveHFG(const HybridGaussianFactorGraph &hfg) { + return *hfg.eliminateSequential()->at(0)->asDiscrete(); +} + +/// Given p(z,m) and z, convert to HFG and solve. +DiscreteConditional SolveHBN(const HybridBayesNet &hbn, double z) { + VectorValues given{{Z(0), Vector1(z)}}; + 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(GaussianMixture, GaussianMixtureModel) { + double mu0 = 1.0, mu1 = 3.0; + double sigma = 2.0; + + 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); + 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); + + // 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); + } +} + +/* + * 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(GaussianMixture, GaussianMixtureModel2) { + double mu0 = 1.0, mu1 = 3.0; + double sigma0 = 8.0, sigma1 = 4.0; + + 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); + 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); + + // 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); + } +} + +/* ************************************************************************* */ +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);