12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Include what you use

release/4.3a0
Frank Dellaert 2024-09-25 18:54:53 -07:00
parent a4a0a2a424
commit 4d10c1462b
1 changed files with 28 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
gtsam/hybrid/tests/testGaussianMixture.cpp
View File

@ -11,15 +11,22 @@
/** /**
* @file testGaussianMixture.cpp * @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 Varun Agrawal
* @author Frank Dellaert * @author Frank Dellaert
* @date September 2024 * @date September 2024
*/ */
#include <gtsam/discrete/DecisionTreeFactor.h>
#include <gtsam/discrete/DiscreteConditional.h> #include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/DiscreteKey.h>
#include <gtsam/hybrid/HybridBayesNet.h> #include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/NoiseModel.h>
// Include for test suite // Include for test suite
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -29,15 +36,15 @@ using symbol_shorthand::M;
using symbol_shorthand::Z; using symbol_shorthand::Z;
// Define mode key and an assignment m==1 // Define mode key and an assignment m==1
static const DiscreteKey m(M(0), 2); const DiscreteKey m(M(0), 2);
static const DiscreteValues m1Assignment{{M(0), 1}}; const DiscreteValues m1Assignment{{M(0), 1}};
// Define a 50/50 prior on the mode // Define a 50/50 prior on the mode
DiscreteConditional::shared_ptr mixing = DiscreteConditional::shared_ptr mixing =
std::make_shared<DiscreteConditional>(m, "60/40"); std::make_shared<DiscreteConditional>(m, "60/40");
// define Continuous keys // 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) * 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 * The "mode" m is binary and depending on m, we have 2 different means
* μ1 and μ2 for the Gaussian density p(z|m). * μ1 and μ2 for the Gaussian density p(z|m).
*/ */
static HybridBayesNet GetGaussianMixtureModel(double mu0, double mu1, HybridBayesNet GaussianMixtureModel(double mu0, double mu1, double sigma0,
double sigma0, double sigma1) { double sigma1) {
HybridBayesNet hbn; HybridBayesNet hbn;
auto model0 = noiseModel::Isotropic::Sigma(1, sigma0); auto model0 = noiseModel::Isotropic::Sigma(1, sigma0);
auto model1 = noiseModel::Isotropic::Sigma(1, sigma1); 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. * If sigma0 == sigma1, it simplifies to a sigmoid function.
* Hardcodes 60/40 prior on mode. * Hardcodes 60/40 prior on mode.
*/ */
static double prob_m_z(double mu0, double mu1, double sigma0, double sigma1, double prob_m_z(double mu0, double mu1, double sigma0, double sigma1,
double z) { double z) {
const double p0 = 0.6 * Gaussian(mu0, sigma0, z); const double p0 = 0.6 * Gaussian(mu0, sigma0, z);
const double p1 = 0.4 * Gaussian(mu1, sigma1, z); const double p1 = 0.4 * Gaussian(mu1, sigma1, z);
return p1 / (p0 + p1); return p1 / (p0 + p1);
}; };
/// Given \phi(m;z)\phi(m) use eliminate to obtain P(m|z). /// 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(); return *hfg.eliminateSequential()->at(0)->asDiscrete();
} }
/// Given p(z,m) and z, convert to HFG and solve. /// 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)}}; 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. * 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. * 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 mu0 = 1.0, mu1 = 3.0;
double sigma = 2.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 // At the halfway point between the means, we should get P(m|z)=0.5
double midway = mu1 - mu0; double midway = mu1 - mu0;
auto pMid = solveHBN(hbn, midway); auto pMid = SolveHBN(hbn, midway);
EXPECT(assert_equal(DiscreteConditional(m, "60/40"), pMid)); EXPECT(assert_equal(DiscreteConditional(m, "60/40"), pMid));
// Everywhere else, the result should be a sigmoid. // 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); const double expected = prob_m_z(mu0, mu1, sigma, sigma, z);
// Workflow 1: convert HBN to HFG and solve // 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); EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
// Workflow 2: directly specify HFG and solve // Workflow 2: directly specify HFG and solve
@ -117,7 +124,7 @@ TEST(HybridGaussianFactor, GaussianMixtureModel) {
hfg1.emplace_shared<DecisionTreeFactor>( hfg1.emplace_shared<DecisionTreeFactor>(
m, std::vector{Gaussian(mu0, sigma, z), Gaussian(mu1, sigma, z)}); m, std::vector{Gaussian(mu0, sigma, z), Gaussian(mu1, sigma, z)});
hfg1.push_back(mixing); hfg1.push_back(mixing);
auto posterior2 = solveHFG(hfg1); auto posterior2 = SolveHFG(hfg1);
EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8); 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. * 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. * 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 mu0 = 1.0, mu1 = 3.0;
double sigma0 = 8.0, sigma1 = 4.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 // 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. // point the mode m==1 is about twice as probable as m==0.
double zMax = 3.133; double zMax = 3.133;
auto pMax = solveHBN(hbn, zMax); auto pMax = SolveHBN(hbn, zMax);
EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4)); EXPECT(assert_equal(DiscreteConditional(m, "42/58"), pMax, 1e-4));
// Everywhere else, the result should be a bell curve like function. // 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); const double expected = prob_m_z(mu0, mu1, sigma0, sigma1, z);
// Workflow 1: convert HBN to HFG and solve // 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); EXPECT_DOUBLES_EQUAL(expected, posterior1(m1Assignment), 1e-8);
// Workflow 2: directly specify HFG and solve // Workflow 2: directly specify HFG and solve
@ -152,7 +159,7 @@ TEST(HybridGaussianFactor, GaussianMixtureModel2) {
hfg.emplace_shared<DecisionTreeFactor>( hfg.emplace_shared<DecisionTreeFactor>(
m, std::vector{Gaussian(mu0, sigma0, z), Gaussian(mu1, sigma1, z)}); m, std::vector{Gaussian(mu0, sigma0, z), Gaussian(mu1, sigma1, z)});
hfg.push_back(mixing); hfg.push_back(mixing);
auto posterior2 = solveHFG(hfg); auto posterior2 = SolveHFG(hfg);
EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8); EXPECT_DOUBLES_EQUAL(expected, posterior2(m1Assignment), 1e-8);
} }
} }