From 7a41180e82c2c21419643d9ea8a1194fae01fe3a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 16 Jan 2023 15:34:00 -0800 Subject: [PATCH] Refactored tests and removed incorrect (R not upper-triangular) test. --- gtsam/hybrid/tests/testGaussianMixture.cpp | 273 +++++++++++---------- 1 file changed, 145 insertions(+), 128 deletions(-) diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index 4cca91b72..14e8932e6 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -30,137 +30,142 @@ // Include for test suite #include -using namespace std; using namespace gtsam; using noiseModel::Isotropic; using symbol_shorthand::M; using symbol_shorthand::X; using symbol_shorthand::Z; -/* ************************************************************************* */ -/* Check construction of GaussianMixture P(x1 | x2, m1) as well as accessing a - * specific mode i.e. P(x1 | x2, m1=1). - */ -TEST(GaussianMixture, Equals) { - // create a conditional gaussian node - Matrix S1(2, 2); - S1(0, 0) = 1; - S1(1, 0) = 2; - S1(0, 1) = 3; - S1(1, 1) = 4; - - Matrix S2(2, 2); - S2(0, 0) = 6; - S2(1, 0) = 0.2; - S2(0, 1) = 8; - S2(1, 1) = 0.4; - - Matrix R1(2, 2); - R1(0, 0) = 0.1; - R1(1, 0) = 0.3; - R1(0, 1) = 0.0; - R1(1, 1) = 0.34; - - Matrix R2(2, 2); - R2(0, 0) = 0.1; - R2(1, 0) = 0.3; - R2(0, 1) = 0.0; - R2(1, 1) = 0.34; - - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34)); - - Vector2 d1(0.2, 0.5), d2(0.5, 0.2); - - auto conditional0 = boost::make_shared(X(1), d1, R1, - X(2), S1, model), - conditional1 = boost::make_shared(X(1), d2, R2, - X(2), S2, model); - - // Create decision tree - DiscreteKey m1(1, 2); - GaussianMixture::Conditionals conditionals( - {m1}, - vector{conditional0, conditional1}); - GaussianMixture mixture({X(1)}, {X(2)}, {m1}, conditionals); - - // Let's check that this worked: - DiscreteValues mode; - mode[m1.first] = 1; - auto actual = mixture(mode); - EXPECT(actual == conditional1); -} +// Common constants +static const Key modeKey = M(0); +static const DiscreteKey mode(modeKey, 2); +static const VectorValues vv{{Z(0), Vector1(4.9)}, {X(0), Vector1(5.0)}}; +static const DiscreteValues assignment0{{M(0), 0}}, assignment1{{M(0), 1}}; +static const HybridValues hv0{vv, assignment0}; +static const HybridValues hv1{vv, assignment1}; /* ************************************************************************* */ -/// Test error method of GaussianMixture. -TEST(GaussianMixture, Error) { - Matrix22 S1 = Matrix22::Identity(); - Matrix22 S2 = Matrix22::Identity() * 2; - Matrix22 R1 = Matrix22::Ones(); - Matrix22 R2 = Matrix22::Ones(); - Vector2 d1(1, 2), d2(2, 1); +namespace equal_constants { +// Create a simple GaussianMixture +const double commonSigma = 2.0; +const std::vector conditionals{ + GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), + commonSigma), + GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), + commonSigma)}; +const GaussianMixture mixture({Z(0)}, {X(0)}, {mode}, conditionals); +} // namespace equal_constants - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector2(1.0, 0.34)); - - auto conditional0 = boost::make_shared(X(1), d1, R1, - X(2), S1, model), - conditional1 = boost::make_shared(X(1), d2, R2, - X(2), S2, model); - - // Create Gaussian Mixture. - DiscreteKey m1(M(1), 2); - GaussianMixture::Conditionals conditionals( - {m1}, - vector{conditional0, conditional1}); - GaussianMixture mixture({X(1)}, {X(2)}, {m1}, conditionals); +/* ************************************************************************* */ +/// Check that invariants hold +TEST(GaussianMixture, Invariants) { + using namespace equal_constants; // Check that normalizationConstants returns nullptr, as all constants equal. CHECK(!mixture.normalizationConstants()); - VectorValues values; - values.insert(X(1), Vector2::Ones()); - values.insert(X(2), Vector2::Zero()); - auto error_tree = mixture.logProbability(values); + // Check that the mixture normalization constant is the max of all constants + // which are all equal, in this case, hence: + const double K = mixture.logNormalizationConstant(); + EXPECT_DOUBLES_EQUAL(K, conditionals[0]->logNormalizationConstant(), 1e-8); + EXPECT_DOUBLES_EQUAL(K, conditionals[1]->logNormalizationConstant(), 1e-8); - // Check result. - std::vector discrete_keys = {m1}; - std::vector leaves = {conditional0->logProbability(values), - conditional1->logProbability(values)}; - AlgebraicDecisionTree expected_error(discrete_keys, leaves); - - EXPECT(assert_equal(expected_error, error_tree, 1e-6)); - - // Regression for non-tree version. - DiscreteValues assignment; - assignment[M(1)] = 0; - EXPECT_DOUBLES_EQUAL(conditional0->logProbability(values), - mixture.logProbability({values, assignment}), 1e-8); - assignment[M(1)] = 1; - EXPECT_DOUBLES_EQUAL(conditional1->logProbability(values), - mixture.logProbability({values, assignment}), 1e-8); + EXPECT(GaussianMixture::CheckInvariants(mixture, hv0)); + EXPECT(GaussianMixture::CheckInvariants(mixture, hv1)); } /* ************************************************************************* */ -// Create mode key: 0 is low-noise, 1 is high-noise. -static const Key modeKey = M(0); -static const DiscreteKey mode(modeKey, 2); +/// Check LogProbability. +TEST(GaussianMixture, LogProbability) { + using namespace equal_constants; + auto actual = mixture.logProbability(vv); -// Create a simple GaussianMixture -static GaussianMixture createSimpleGaussianMixture() { - // Create Gaussian mixture Z(0) = X(0) + noise. - // TODO(dellaert): making copies below is not ideal ! - Matrix1 I = Matrix1::Identity(); - const auto conditional0 = boost::make_shared( - GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 0.5)); - const auto conditional1 = boost::make_shared( - GaussianConditional::FromMeanAndStddev(Z(0), I, X(0), Vector1(0), 3)); - return GaussianMixture({Z(0)}, {X(0)}, {mode}, {conditional0, conditional1}); + // Check result. + std::vector discrete_keys = {mode}; + std::vector leaves = {conditionals[0]->logProbability(vv), + conditionals[1]->logProbability(vv)}; + AlgebraicDecisionTree expected(discrete_keys, leaves); + + EXPECT(assert_equal(expected, actual, 1e-6)); + + // Check for non-tree version. + for (size_t mode : {0, 1}) { + const HybridValues hv{vv, {{M(0), mode}}}; + EXPECT_DOUBLES_EQUAL(conditionals[mode]->logProbability(vv), + mixture.logProbability(hv), 1e-8); + } } +/* ************************************************************************* */ +/// Check error. +TEST(GaussianMixture, Error) { + using namespace equal_constants; + auto actual = mixture.error(vv); + + // Check result. + std::vector discrete_keys = {mode}; + std::vector leaves = {conditionals[0]->error(vv), + conditionals[1]->error(vv)}; + AlgebraicDecisionTree expected(discrete_keys, leaves); + + EXPECT(assert_equal(expected, actual, 1e-6)); + + // Check for non-tree version. + for (size_t mode : {0, 1}) { + const HybridValues hv{vv, {{M(0), mode}}}; + EXPECT_DOUBLES_EQUAL(conditionals[mode]->error(vv), mixture.error(hv), + 1e-8); + } +} + +/* ************************************************************************* */ +/// Check that the likelihood is proportional to the conditional density given +/// the measurements. +TEST(GaussianMixture, Likelihood) { + using namespace equal_constants; + + // Compute likelihood + auto likelihood = mixture.likelihood(vv); + + // Check that the mixture error and the likelihood error are the same. + EXPECT_DOUBLES_EQUAL(mixture.error(hv0), likelihood->error(hv0), 1e-8); + EXPECT_DOUBLES_EQUAL(mixture.error(hv1), likelihood->error(hv1), 1e-8); + + // Check that likelihood error is as expected, i.e., just the errors of the + // individual likelihoods, in the `equal_constants` case. + std::vector discrete_keys = {mode}; + std::vector leaves = {conditionals[0]->likelihood(vv)->error(vv), + conditionals[1]->likelihood(vv)->error(vv)}; + AlgebraicDecisionTree expected(discrete_keys, leaves); + EXPECT(assert_equal(expected, likelihood->error(vv), 1e-6)); + + // Check that the ratio of probPrime to evaluate is the same for all modes. + std::vector ratio(2); + for (size_t mode : {0, 1}) { + const HybridValues hv{vv, {{M(0), mode}}}; + // Print error of mixture and likelihood: + ratio[mode] = std::exp(-likelihood->error(hv)) / mixture.evaluate(hv); + } + EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8); +} + +/* ************************************************************************* */ +namespace mode_dependent_constants { +// Create a GaussianMixture with mode-dependent noise models. +// 0 is low-noise, 1 is high-noise. +const std::vector conditionals{ + GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), + 0.5), + GaussianConditional::sharedMeanAndStddev(Z(0), I_1x1, X(0), Vector1(0.0), + 3.0)}; +const GaussianMixture mixture({Z(0)}, {X(0)}, {mode}, conditionals); +} // namespace mode_dependent_constants + /* ************************************************************************* */ // Create a test for continuousParents. TEST(GaussianMixture, ContinuousParents) { - const GaussianMixture gm = createSimpleGaussianMixture(); - const KeyVector continuousParentKeys = gm.continuousParents(); + using namespace mode_dependent_constants; + const KeyVector continuousParentKeys = mixture.continuousParents(); // Check that the continuous parent keys are correct: EXPECT(continuousParentKeys.size() == 1); EXPECT(continuousParentKeys[0] == X(0)); @@ -169,9 +174,9 @@ TEST(GaussianMixture, ContinuousParents) { /* ************************************************************************* */ /// Check we can create a DecisionTreeFactor with all normalization constants. TEST(GaussianMixture, NormalizationConstants) { - const GaussianMixture gm = createSimpleGaussianMixture(); + using namespace mode_dependent_constants; - const auto factor = gm.normalizationConstants(); + const auto factor = mixture.normalizationConstants(); // Test with 1D Gaussian normalization constants for sigma 0.5 and 3: auto c = [](double sigma) { return 1.0 / (sqrt(2 * M_PI) * sigma); }; @@ -180,27 +185,39 @@ TEST(GaussianMixture, NormalizationConstants) { } /* ************************************************************************* */ -/// Check that likelihood returns a mixture factor on the parents. -TEST(GaussianMixture, Likelihood) { - const GaussianMixture gm = createSimpleGaussianMixture(); +/// Check that the likelihood is proportional to the conditional density given +/// the measurements. +TEST(GaussianMixture, Likelihood2) { + using namespace mode_dependent_constants; - // Call the likelihood function: - VectorValues measurements; - measurements.insert(Z(0), Vector1(0)); - const auto factor = gm.likelihood(measurements); + // Compute likelihood + auto likelihood = mixture.likelihood(vv); - // Check that the factor is a mixture factor on the parents. - // Loop over all discrete assignments over the discrete parents: - const DiscreteKeys discreteParentKeys = gm.discreteKeys(); + GTSAM_PRINT(mixture); + GTSAM_PRINT(*likelihood); - // Apply the likelihood function to all conditionals: - const GaussianMixtureFactor::Factors factors( - gm.conditionals(), - [measurements](const GaussianConditional::shared_ptr& conditional) { - return conditional->likelihood(measurements); - }); - const GaussianMixtureFactor expected({X(0)}, {mode}, factors); - EXPECT(assert_equal(expected, *factor)); + // Check that the mixture error and the likelihood error are the same. + EXPECT_DOUBLES_EQUAL(mixture.error(hv0), likelihood->error(hv0), 1e-8); + EXPECT_DOUBLES_EQUAL(mixture.error(hv1), likelihood->error(hv1), 1e-8); + + // Check that likelihood error is as expected, i.e., just the errors of the + // individual likelihoods, in the `equal_constants` case. + std::vector discrete_keys = {mode}; + std::vector leaves = {conditionals[0]->likelihood(vv)->error(vv), + conditionals[1]->likelihood(vv)->error(vv)}; + AlgebraicDecisionTree expected(discrete_keys, leaves); + EXPECT(assert_equal(expected, likelihood->error(vv), 1e-6)); + + // Check that the ratio of probPrime to evaluate is the same for all modes. + std::vector ratio(2); + for (size_t mode : {0, 1}) { + const HybridValues hv{vv, {{M(0), mode}}}; + // Print error of mixture and likelihood: + std::cout << "mode " << mode << " mixture: " << mixture.error(hv) + << " likelihood: " << likelihood->error(hv) << std::endl; + ratio[mode] = std::exp(-likelihood->error(hv)) / mixture.evaluate(hv); + } + EXPECT_DOUBLES_EQUAL(ratio[0], ratio[1], 1e-8); } /* ************************************************************************* */