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

Separated out NFG setup and added test.

release/4.3a0
Frank Dellaert 2022-12-25 19:31:54 -05:00
parent a7573e8e6f
commit 8a319c5a49
1 changed files with 61 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
gtsam/hybrid/tests/testHybridEstimation.cpp
View File

@ -432,8 +432,65 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
EXPECT(assert_equal(discrete_seq, hybrid_values.discrete())); EXPECT(assert_equal(discrete_seq, hybrid_values.discrete()));
} }
/****************************************************************************/ /*********************************************************************************
/** // Create a hybrid nonlinear factor graph f(x0, x1, m0; z0, z1)
********************************************************************************/
static HybridNonlinearFactorGraph createHybridNonlinearFactorGraph() {
HybridNonlinearFactorGraph nfg;
constexpr double sigma = 0.5; // measurement noise
const auto noise_model = noiseModel::Isotropic::Sigma(1, sigma);
// Add "measurement" factors:
nfg.emplace_nonlinear<PriorFactor<double>>(X(0), 0.0, noise_model);
nfg.emplace_nonlinear<PriorFactor<double>>(X(1), 1.0, noise_model);
// Add mixture factor:
DiscreteKey m(M(0), 2);
const auto zero_motion =
boost::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
const auto one_motion =
boost::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
nfg.emplace_hybrid<MixtureFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{m},
std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
return nfg;
}
/*********************************************************************************
// Create a hybrid nonlinear factor graph f(x0, x1, m0; z0, z1)
********************************************************************************/
static HybridGaussianFactorGraph::shared_ptr createHybridGaussianFactorGraph() {
HybridNonlinearFactorGraph nfg = createHybridNonlinearFactorGraph();
Values initial;
double z0 = 0.0, z1 = 1.0;
initial.insert<double>(X(0), z0);
initial.insert<double>(X(1), z1);
return nfg.linearize(initial);
}
/*********************************************************************************
* Do hybrid elimination and do regression test on discrete conditional.
********************************************************************************/
TEST(HybridEstimation, eliminateSequentialRegression) {
// 1. Create the factor graph from the nonlinear factor graph.
HybridGaussianFactorGraph::shared_ptr fg = createHybridGaussianFactorGraph();
// 2. Eliminate into BN
const Ordering ordering = fg->getHybridOrdering();
HybridBayesNet::shared_ptr bn = fg->eliminateSequential(ordering);
// GTSAM_PRINT(*bn);
// TODO(dellaert): dc should be discrete conditional on m0, but it is an unnormalized factor?
// DiscreteKey m(M(0), 2);
// DiscreteConditional expected(m % "0.51341712/1");
// auto dc = bn->back()->asDiscreteConditional();
// EXPECT(assert_equal(expected, *dc, 1e-9));
}
/*********************************************************************************
* Test for correctness via sampling. * Test for correctness via sampling.
* *
* Compute the conditional P(x0, m0, x1| z0, z1) * Compute the conditional P(x0, m0, x1| z0, z1)
@ -442,32 +499,10 @@ TEST(HybridEstimation, ProbabilityMultifrontal) {
* 2. Eliminate the factor graph into a Bayes Net `BN`. * 2. Eliminate the factor graph into a Bayes Net `BN`.
* 3. Sample from the Bayes Net. * 3. Sample from the Bayes Net.
* 4. Check that the ratio `BN(x)/FG(x) = constant` for all samples `x`. * 4. Check that the ratio `BN(x)/FG(x) = constant` for all samples `x`.
*/ ********************************************************************************/
TEST(HybridEstimation, CorrectnessViaSampling) { TEST(HybridEstimation, CorrectnessViaSampling) {
HybridNonlinearFactorGraph nfg;
// First we create a hybrid nonlinear factor graph
// which represents f(x0, x1, m0; z0, z1).
// We linearize and eliminate this to get
// the required Factor Graph FG and Bayes Net BN.
const auto noise_model = noiseModel::Isotropic::Sigma(1, 1.0);
const auto zero_motion =
boost::make_shared<BetweenFactor<double>>(X(0), X(1), 0, noise_model);
const auto one_motion =
boost::make_shared<BetweenFactor<double>>(X(0), X(1), 1, noise_model);
nfg.emplace_nonlinear<PriorFactor<double>>(X(0), 0.0, noise_model);
nfg.emplace_hybrid<MixtureFactor>(
KeyVector{X(0), X(1)}, DiscreteKeys{DiscreteKey(M(0), 2)},
std::vector<NonlinearFactor::shared_ptr>{zero_motion, one_motion});
Values initial;
double z0 = 0.0, z1 = 1.0;
initial.insert<double>(X(0), z0);
initial.insert<double>(X(1), z1);
// 1. Create the factor graph from the nonlinear factor graph. // 1. Create the factor graph from the nonlinear factor graph.
HybridGaussianFactorGraph::shared_ptr fg = nfg.linearize(initial); HybridGaussianFactorGraph::shared_ptr fg = createHybridGaussianFactorGraph();
// 2. Eliminate into BN // 2. Eliminate into BN
const Ordering ordering = fg->getHybridOrdering(); const Ordering ordering = fg->getHybridOrdering();