diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 006a3b026..dd2108d2f 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -229,6 +229,82 @@ TEST(MixtureFactor, DifferentCovariances) { EXPECT(assert_equal(expected_values, actual_values)); } +/* ************************************************************************* */ +// Test components with differing means and covariances +TEST(MixtureFactor, DifferentMeansAndCovariances) { + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 0.0, x2 = 7.0; + values.insert(X(1), x1); + + double between = 0.0; + + auto model0 = noiseModel::Isotropic::Sigma(1, 1e2); + auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2); + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + + auto f0 = + std::make_shared>(X(1), X(2), between, model0); + auto f1 = + std::make_shared>(X(1), X(2), between, model1); + std::vector factors{f0, f1}; + + // Create via toFactorGraph + using symbol_shorthand::Z; + Matrix H0_1, H0_2, H1_1, H1_2; + Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); + std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H0_1 /*Sp1*/}, + {X(2), H0_2 /*Tp2*/}}; + + Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); + std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H1_1 /*Sp1*/}, + {X(2), H1_2 /*Tp2*/}}; + auto gm = new gtsam::GaussianMixture( + {Z(1)}, {X(1), X(2)}, {m1}, + {std::make_shared(terms0, 1, -d0, model0), + std::make_shared(terms1, 1, -d1, model1)}); + gtsam::HybridBayesNet bn; + bn.emplace_back(gm); + + gtsam::VectorValues measurements; + measurements.insert(Z(1), gtsam::Z_1x1); + // Create FG with single GaussianMixtureFactor + HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements); + + // Linearized prior factor on X1 + auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); + mixture_fg.push_back(prior); + + // bn.print("BayesNet:"); + // mixture_fg.print("\n\n"); + + VectorValues vv{{X(1), x1 * I_1x1}, {X(2), x2 * I_1x1}}; + // std::cout << "FG error for m1=0: " + // << mixture_fg.error(HybridValues(vv, DiscreteValues{{m1.first, 0}})) + // << std::endl; + // std::cout << "FG error for m1=1: " + // << mixture_fg.error(HybridValues(vv, DiscreteValues{{m1.first, 1}})) + // << std::endl; + + auto hbn = mixture_fg.eliminateSequential(); + + HybridValues actual_values = hbn->optimize(); + + VectorValues cv; + cv.insert(X(1), Vector1(0.0)); + cv.insert(X(2), Vector1(-7.0)); + DiscreteValues dv; + dv.insert({M(1), 1}); + HybridValues expected_values(cv, dv); + + EXPECT(assert_equal(expected_values, actual_values)); +} + /* ************************************************************************* */ int main() { TestResult tr;