From 8391c783bf24e862ebf15992bd65b9bf63435ff3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 28 Dec 2022 12:00:37 -0500 Subject: [PATCH] Correct densities (error already includes 0.5!) --- gtsam/linear/GaussianBayesNet.h | 22 +++++--- gtsam/linear/GaussianConditional.cpp | 9 ++-- gtsam/linear/GaussianConditional.h | 51 ++++++++++++------- .../linear/tests/testGaussianConditional.cpp | 37 ++++++++++++-- 4 files changed, 85 insertions(+), 34 deletions(-) diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 381b8ad3a..426d3bd83 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -88,20 +88,26 @@ namespace gtsam { /// @name Standard Interface /// @{ + /** + * Calculate probability density for given values `x`: + * exp(-error(x)) / sqrt((2*pi)^n*det(Sigma)) + * where x is the vector of values, and Sigma is the covariance matrix. + * Note that error(x)=0.5*e'*e includes the 0.5 factor already. + */ + double evaluate(const VectorValues& x) const; + + /// Evaluate probability density, sugar. + double operator()(const VectorValues& x) const { + return evaluate(x); + } + /** * Calculate log-density for given values `x`: - * -0.5*(error + n*log(2*pi) + log det(Sigma)) + * -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma) * where x is the vector of values, and Sigma is the covariance matrix. */ double logDensity(const VectorValues& x) const; - /** - * Calculate probability density for given values `x`: - * exp(-0.5*error(x)) / sqrt((2*pi)^n*det(Sigma)) - * where x is the vector of values, and Sigma is the covariance matrix. - */ - double evaluate(const VectorValues& x) const; - /// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by /// back-substitution VectorValues optimize() const; diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index ec895b8e7..5e8a193cf 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -169,13 +169,14 @@ double GaussianConditional::logDeterminant() const { return logDet; } - /* ************************************************************************* */ +/* ************************************************************************* */ +// density = exp(-error(x)) / sqrt((2*pi)^n*det(Sigma)) +// log = -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma) double GaussianConditional::logDensity(const VectorValues& x) const { constexpr double log2pi = 1.8378770664093454835606594728112; size_t n = d().size(); - // log det(Sigma)) = - 2 * logDeterminant() - double sum = error(x) + n * log2pi - 2 * logDeterminant(); - return -0.5 * sum; + // log det(Sigma)) = - 2.0 * logDeterminant() + return - error(x) - 0.5 * n * log2pi + logDeterminant(); } /* ************************************************************************* */ diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index cf8cada82..a72a73973 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -121,20 +121,26 @@ namespace gtsam { /// @name Standard Interface /// @{ + /** + * Calculate probability density for given values `x`: + * exp(-error(x)) / sqrt((2*pi)^n*det(Sigma)) + * where x is the vector of values, and Sigma is the covariance matrix. + * Note that error(x)=0.5*e'*e includes the 0.5 factor already. + */ + double evaluate(const VectorValues& x) const; + + /// Evaluate probability density, sugar. + double operator()(const VectorValues& x) const { + return evaluate(x); + } + /** * Calculate log-density for given values `x`: - * -0.5*(error + n*log(2*pi) + log det(Sigma)) + * -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma) * where x is the vector of values, and Sigma is the covariance matrix. */ double logDensity(const VectorValues& x) const; - /** - * Calculate probability density for given values `x`: - * exp(-0.5*error(x)) / sqrt((2*pi)^n*det(Sigma)) - * where x is the vector of values, and Sigma is the covariance matrix. - */ - double evaluate(const VectorValues& x) const; - /** Return a view of the upper-triangular R block of the conditional */ constABlock R() const { return Ab_.range(0, nrFrontals()); } @@ -147,25 +153,32 @@ namespace gtsam { /** Get a view of the r.h.s. vector d */ const constBVector d() const { return BaseFactor::getb(); } - /** - * @brief Compute the log determinant of the R matrix. - * For numerical stability, the determinant is computed in log - * form, so it is a summation rather than a multiplication. - * - * @return double - */ - double logDeterminant() const; - /** * @brief Compute the determinant of the R matrix. * - * The determinant is computed in log form (hence summation) for numerical - * stability and then exponentiated. + * The determinant is computed in log form using logDeterminant for + * numerical stability and then exponentiated. + * + * Note, the covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$, and hence + * \f$ \det(\Sigma) = 1 / \det(R^T R) = 1 / determinant()^ 2 \f$. * * @return double */ double determinant() const { return exp(this->logDeterminant()); } + /** + * @brief Compute the log determinant of the R matrix. + * + * For numerical stability, the determinant is computed in log + * form, so it is a summation rather than a multiplication. + * + * Note, the covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$, and hence + * \f$ \log \det(\Sigma) = - \log \det(R^T R) = - 2 logDeterminant() \f$. + * + * @return double + */ + double logDeterminant() const; + /** * Solves a conditional Gaussian and writes the solution into the entries of * \c x for each frontal variable of the conditional. The parents are diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index a3b64528b..20d730856 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -130,13 +130,15 @@ TEST( GaussianConditional, equals ) EXPECT( expected.equals(actual) ); } +/* ************************************************************************* */ namespace density { static const Key key = 77; +static constexpr double sigma = 3.0; static const auto unitPrior = GaussianConditional(key, Vector1::Constant(5), I_1x1), - widerPrior = - GaussianConditional(key, Vector1::Constant(5), I_1x1, - noiseModel::Isotropic::Sigma(1, 3.0)); + widerPrior = GaussianConditional( + key, Vector1::Constant(5), I_1x1, + noiseModel::Isotropic::Sigma(1, sigma)); } // namespace density /* ************************************************************************* */ @@ -155,8 +157,23 @@ TEST(GaussianConditional, Evaluate1) { const double expected = sqrt((invSigma / (2 * M_PI)).determinant()); const double actual = density::unitPrior.evaluate(mean); EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9); + + using density::key; + using density::sigma; + + // 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: + for (double x = -5.0 * sigma; x <= 5.0 * sigma; x += 0.1 * sigma) { + VectorValues xValues; + xValues.insert(key, mean.at(key) + Vector1(x)); + const double density = density::unitPrior.evaluate(xValues); + integral += 0.1 * sigma * density; + } + EXPECT_DOUBLES_EQUAL(1.0, integral, 1e-9); } +/* ************************************************************************* */ // Check the evaluate with non-unit noise. TEST(GaussianConditional, Evaluate2) { // See comments in test above. @@ -166,6 +183,20 @@ TEST(GaussianConditional, Evaluate2) { const double expected = sqrt((invSigma / (2 * M_PI)).determinant()); const double actual = density::widerPrior.evaluate(mean); EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9); + + using density::key; + using density::sigma; + + // 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: + for (double x = -5.0 * sigma; x <= 5.0 * sigma; x += 0.1 * sigma) { + VectorValues xValues; + xValues.insert(key, mean.at(key) + Vector1(x)); + const double density = density::widerPrior.evaluate(xValues); + integral += 0.1 * sigma * density; + } + EXPECT_DOUBLES_EQUAL(1.0, integral, 1e-5); } /* ************************************************************************* */