From b99d464049ac45e5da862f865c6d2ef989f1b184 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 13 Jan 2023 12:29:11 -0800 Subject: [PATCH] Consistency test in testGaussianConditional --- gtsam/inference/Conditional-inst.h | 16 -------- gtsam/inference/Conditional.h | 5 +-- .../linear/tests/testGaussianConditional.cpp | 40 +++++++++++++------ 3 files changed, 28 insertions(+), 33 deletions(-) diff --git a/gtsam/inference/Conditional-inst.h b/gtsam/inference/Conditional-inst.h index 5a17c44cc..1b439649e 100644 --- a/gtsam/inference/Conditional-inst.h +++ b/gtsam/inference/Conditional-inst.h @@ -63,20 +63,4 @@ double Conditional::normalizationConstant() const { return std::exp(logNormalizationConstant()); } -/* ************************************************************************* */ -template -bool Conditional::checkInvariants( - const HybridValues& values) const { - const double probability = evaluate(values); - if (probability < 0.0 || probability > 1.0) - return false; // probability is not in [0,1] - const double logProb = logProbability(values); - if (std::abs(probability - std::exp(logProb)) > 1e-9) - return false; // logProb is not consistent with probability - const double expected = - this->logNormalizationConstant() - this->error(values); - if (std::abs(logProb - expected) > 1e-9) - return false; // logProb is not consistent with error -} - } // namespace gtsam diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index bb75f9c6e..bba4c7bd5 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -145,7 +145,7 @@ namespace gtsam { * By default, log normalization constant = 0.0. * Override if this depends on the parameters. */ - virtual double logNormalizationConstant() const; + virtual double logNormalizationConstant() const { return 0.0; } /** Non-virtual, exponentiate logNormalizationConstant. */ double normalizationConstant() const; @@ -181,9 +181,6 @@ namespace gtsam { /** Mutable iterator pointing past the last parent key. */ typename FACTOR::iterator endParents() { return asFactor().end(); } - /** Check that the invariants hold for derived class at a given point. */ - bool checkInvariants(const HybridValues& values) const; - /// @} private: diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 0bfb95351..12c668c25 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include @@ -135,18 +136,20 @@ static const auto unitPrior = } // namespace density /* ************************************************************************* */ -bool checkInvariants(const GaussianConditional* self, - const HybridValues& values) { - const double probability = self->evaluate(values); +template +bool checkInvariants(const GaussianConditional& conditional, + const VALUES& values) { + const double probability = conditional.evaluate(values); if (probability < 0.0 || probability > 1.0) return false; // probability is not in [0,1] - const double logProb = self->logProbability(values); + const double logProb = conditional.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); + conditional.logNormalizationConstant() - conditional.error(values); if (std::abs(logProb - expected) > 1e-9) return false; // logProb is not consistent with error + return true; } /* ************************************************************************* */ @@ -169,6 +172,12 @@ TEST(GaussianConditional, Evaluate1) { using density::key; using density::sigma; + // Check Invariants at the mean and a different value + for (auto vv : {mean, VectorValues{{key, Vector1(4)}}}) { + EXPECT(checkInvariants(density::unitPrior, vv)); + EXPECT(checkInvariants(density::unitPrior, HybridValues{vv, {}, {}})); + } + // Let's numerically integrate and see that we integrate to 1.0. double integral = 0.0; // Loop from -5*sigma to 5*sigma in 0.1*sigma steps: @@ -179,7 +188,6 @@ TEST(GaussianConditional, Evaluate1) { integral += 0.1 * sigma * density; } EXPECT_DOUBLES_EQUAL(1.0, integral, 1e-9); - EXPECT(checkInvariants(&density::unitPrior, mean)); } /* ************************************************************************* */ @@ -196,6 +204,12 @@ TEST(GaussianConditional, Evaluate2) { using density::key; using density::sigma; + // Check Invariants at the mean and a different value + for (auto vv : {mean, VectorValues{{key, Vector1(4)}}}) { + EXPECT(checkInvariants(density::widerPrior, vv)); + EXPECT(checkInvariants(density::widerPrior, HybridValues{vv, {}, {}})); + } + // Let's numerically integrate and see that we integrate to 1.0. double integral = 0.0; // Loop from -5*sigma to 5*sigma in 0.1*sigma steps: @@ -400,17 +414,17 @@ TEST(GaussianConditional, FromMeanAndStddev) { double expected1 = 0.5 * e1.dot(e1); EXPECT_DOUBLES_EQUAL(expected1, conditional1.error(values), 1e-9); - double expected2 = conditional1.logNormalizationConstant() - 0.5 * e1.dot(e1); - EXPECT_DOUBLES_EQUAL(expected2, conditional1.logProbability(values), 1e-9); - auto conditional2 = GaussianConditional::FromMeanAndStddev(X(0), A1, X(1), A2, X(2), b, sigma); Vector2 e2 = (x0 - (A1 * x1 + A2 * x2 + b)) / sigma; - double expected3 = 0.5 * e2.dot(e2); - EXPECT_DOUBLES_EQUAL(expected3, conditional2.error(values), 1e-9); + double expected2 = 0.5 * e2.dot(e2); + EXPECT_DOUBLES_EQUAL(expected2, conditional2.error(values), 1e-9); - double expected4 = conditional2.logNormalizationConstant() - 0.5 * e2.dot(e2); - EXPECT_DOUBLES_EQUAL(expected4, conditional2.logProbability(values), 1e-9); + // Check Invariants for both conditionals + for (auto conditional : {conditional1, conditional2}) { + EXPECT(checkInvariants(conditional, values)); + EXPECT(checkInvariants(conditional, HybridValues{values, {}, {}})); + } } /* ************************************************************************* */