From 7ea8bd0fbacf5cba4cf2300ab66b0b957de67135 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 13 Jan 2023 11:58:17 -0800 Subject: [PATCH] Add check --- gtsam/linear/tests/testGaussianConditional.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index eb90f8aab..0bfb95351 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -134,6 +134,21 @@ static const auto unitPrior = noiseModel::Isotropic::Sigma(1, sigma)); } // namespace density +/* ************************************************************************* */ +bool checkInvariants(const GaussianConditional* self, + const HybridValues& values) { + const double probability = self->evaluate(values); + if (probability < 0.0 || probability > 1.0) + return false; // probability is not in [0,1] + const double logProb = self->logProbability(values); + if (std::abs(probability - std::exp(logProb)) > 1e-9) + return false; // logProb is not consistent with probability + const double expected = + self->logNormalizationConstant() - self->error(values); + if (std::abs(logProb - expected) > 1e-9) + return false; // logProb is not consistent with error +} + /* ************************************************************************* */ // Check that the evaluate function matches direct calculation with R. TEST(GaussianConditional, Evaluate1) { @@ -164,6 +179,7 @@ TEST(GaussianConditional, Evaluate1) { integral += 0.1 * sigma * density; } EXPECT_DOUBLES_EQUAL(1.0, integral, 1e-9); + EXPECT(checkInvariants(&density::unitPrior, mean)); } /* ************************************************************************* */