diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 7f2bd99f4..e976994f8 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)); + // 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)); - HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given); - HybridBayesNet::shared_ptr bn = gfg.eliminateSequential(); + // 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); - 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); + auto posterior = solveForMeasurement(hbn, z); + EXPECT_DOUBLES_EQUAL(expected, posterior(m1Assignment), 1e-8); } }