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

Test different workflows

release/4.3a0
Frank Dellaert 2024-09-25 18:48:01 -07:00
parent 314a8310cf
commit a4a0a2a424
1 changed files with 67 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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 <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/discrete/DiscreteConditional.h>
#include <gtsam/discrete/DiscreteValues.h>
#include <gtsam/hybrid/HybridBayesNet.h>
#include <gtsam/hybrid/HybridGaussianConditional.h>
#include <gtsam/hybrid/HybridGaussianFactor.h>
#include <gtsam/hybrid/HybridGaussianFactorGraph.h>
#include <gtsam/hybrid/HybridValues.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// Include for test suite
#include <CppUnitLite/TestHarness.h>
#include <memory>
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<DiscreteConditional>(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<GaussianConditional>(Z(0), Vector1(mu0), I_1x1, model0),
c1 = make_shared<GaussianConditional>(Z(0), Vector1(mu1), I_1x1, model1);
HybridBayesNet hbn;
hbn.emplace_shared<HybridGaussianConditional>(KeyVector{Z(0)}, KeyVector{}, m,
auto c0 = std::make_shared<GaussianConditional>(Z(0), Vector1(mu0), I_1x1,
model0),
c1 = std::make_shared<GaussianConditional>(Z(0), Vector1(mu1), I_1x1,
model1);
hbn.emplace_shared<HybridGaussianConditional>(continuousKeys, KeyVector{}, m,
std::vector{c0, c1});
auto mixing = make_shared<DiscreteConditional>(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);
};
HybridGaussianFactorGraph gfg = hbn.toFactorGraph(given);
return *gfg.eliminateSequential()->at(0)->asDiscrete();
/**
* 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();
}
/// 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<DecisionTreeFactor>(
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<DecisionTreeFactor>(
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);
}
}