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

Small improvements

release/4.3a0
Frank Dellaert 2024-09-13 01:18:05 -07:00
parent 8fbabf5c24
commit bf00ca891d
1 changed files with 22 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
gtsam/hybrid/tests/testGaussianMixtureFactor.cpp
View File

@ -392,6 +392,12 @@ namespace test_two_state_estimation {
DiscreteKey m1(M(1), 2);
void addMeasurement(HybridBayesNet& hbn, Key z_key, Key x_key, double sigma) {
auto measurement_model = noiseModel::Isotropic::Sigma(1, sigma);
hbn.emplace_shared<GaussianConditional>(z_key, Vector1(0.0), I_1x1, x_key,
-I_1x1, measurement_model);
}
/// Create hybrid motion model p(x1 | x0, m1)
static GaussianMixture::shared_ptr CreateHybridMotionModel(double mu0,
double mu1,
@ -414,15 +420,11 @@ HybridBayesNet CreateBayesNet(
HybridBayesNet hbn;
// Add measurement model p(z0 | x0)
const double measurement_sigma = 3.0;
auto measurement_model = noiseModel::Isotropic::Sigma(1, measurement_sigma);
hbn.emplace_shared<GaussianConditional>(Z(0), Vector1(0.0), I_1x1, X(0),
-I_1x1, measurement_model);
addMeasurement(hbn, Z(0), X(0), 3.0);
// Optionally add second measurement model p(z1 | x1)
if (add_second_measurement) {
hbn.emplace_shared<GaussianConditional>(Z(1), Vector1(0.0), I_1x1, X(1),
-I_1x1, measurement_model);
addMeasurement(hbn, Z(1), X(1), 3.0);
}
// Add hybrid motion model
@ -434,18 +436,27 @@ HybridBayesNet CreateBayesNet(
return hbn;
}
/**
* @brief Approximates the discrete marginal P(m1) using importance sampling.
* @note Not typically called as expensive, but values are used in the tests.
*
* @param hbn The hybrid Bayesian network.
* @param hybridMotionModel The hybrid motion model.
* @param given Observed values for variables.
* @param N Number of samples for importance sampling.
* @return std::pair<double, double> Probabilities for m1 = 0 and m1 = 1.
*/
/// Approximate the discrete marginal P(m1) using importance sampling
/// Not typically called as expensive, but values are used in the tests.
void approximateDiscreteMarginal(
std::pair<double, double> approximateDiscreteMarginal(
const HybridBayesNet& hbn,
const GaussianMixture::shared_ptr& hybridMotionModel,
const VectorValues& given, size_t N = 100000) {
/// Create importance sampling network q(x0,x1,m) = p(x1|x0,m1) q(x0) P(m1),
/// using q(x0) = N(z0, sigma_Q) to sample x0.
/// using q(x0) = N(z0, sigmaQ) to sample x0.
HybridBayesNet q;
q.push_back(hybridMotionModel); // Add hybrid motion model
q.emplace_shared<GaussianConditional>(GaussianConditional::FromMeanAndStddev(
X(0), given.at(Z(0)), /* sigma_Q = */ 3.0)); // Add proposal q(x0) for x0
X(0), given.at(Z(0)), /* sigmaQ = */ 3.0)); // Add proposal q(x0) for x0
q.emplace_shared<DiscreteConditional>(m1, "50/50"); // Discrete prior.
// Do importance sampling
@ -460,6 +471,7 @@ void approximateDiscreteMarginal(
double pm1 = w1 / (w0 + w1);
std::cout << "p(m0) = " << 100 * (1.0 - pm1) << std::endl;
std::cout << "p(m1) = " << 100 * pm1 << std::endl;
return {1.0 - pm1, pm1};
}
} // namespace test_two_state_estimation