diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 229d4a932..4c1338435 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -224,5 +224,19 @@ namespace gtsam { } /* ************************************************************************* */ + double GaussianBayesNet::logDensity(const VectorValues& x) const { + double sum = 0.0; + for (const auto& conditional : *this) { + if (conditional) sum += conditional->logDensity(x); + } + return sum; + } + + /* ************************************************************************* */ + double GaussianBayesNet::evaluate(const VectorValues& x) const { + return exp(logDensity(x)); + } + + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index e81d6af33..381b8ad3a 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -88,6 +88,20 @@ namespace gtsam { /// @name Standard Interface /// @{ + /** + * Calculate log-density for given values `x`: + * -0.5*(error + n*log(2*pi) + 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 4597156bc..ec895b8e7 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -169,6 +169,20 @@ double GaussianConditional::logDeterminant() const { return logDet; } + /* ************************************************************************* */ +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; +} + +/* ************************************************************************* */ +double GaussianConditional::evaluate(const VectorValues& x) const { + return exp(logDensity(x)); +} + /* ************************************************************************* */ VectorValues GaussianConditional::solve(const VectorValues& x) const { // Concatenate all vector values that correspond to parent variables diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 8af7f6602..cf8cada82 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -121,6 +121,20 @@ namespace gtsam { /// @name Standard Interface /// @{ + /** + * Calculate log-density for given values `x`: + * -0.5*(error + n*log(2*pi) + 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()); } @@ -134,9 +148,7 @@ namespace gtsam { const constBVector d() const { return BaseFactor::getb(); } /** - * @brief Compute the log determinant of the Gaussian conditional. - * The determinant is computed using the R matrix, which is upper - * triangular. + * @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. * @@ -145,8 +157,7 @@ namespace gtsam { double logDeterminant() const; /** - * @brief Compute the determinant of the conditional from the - * upper-triangular R matrix. + * @brief Compute the determinant of the R matrix. * * The determinant is computed in log form (hence summation) for numerical * stability and then exponentiated. diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index b8534a02b..e5e8840c0 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -68,39 +68,6 @@ TEST( GaussianBayesNet, Matrix ) } /* ************************************************************************* */ -/** - * Calculate log-density for given values `x`: - * -0.5*(error + n*log(2*pi) + log det(Sigma)) - * where x is the vector of values, and Sigma is the covariance matrix. - */ -double logDensity(const GaussianConditional::shared_ptr& gc, - const VectorValues& x) { - constexpr double log2pi = 1.8378770664093454835606594728112; - size_t n = gc->d().size(); - // log det(Sigma)) = - 2 * gc->logDeterminant() - double sum = gc->error(x) + n * log2pi - 2 * gc->logDeterminant(); - return -0.5 * sum; -} - -/** - * 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 GaussianConditional::shared_ptr& gc, - const VectorValues& x) { - return exp(logDensity(gc, x)); -} - -/** Calculate probability for given values `x` */ -double evaluate(const GaussianBayesNet& gbn, const VectorValues& x) { - double density = 1.0; - for (const auto& conditional : gbn) { - if (conditional) density *= evaluate(conditional, x); - } - return density; -} - // Check that the evaluate function matches direct calculation with R. TEST(GaussianBayesNet, Evaluate1) { // Let's evaluate at the mean @@ -115,7 +82,7 @@ TEST(GaussianBayesNet, Evaluate1) { // the normalization constant 1.0/sqrt((2*pi*Sigma).det()). // The covariance matrix inv(Sigma) = R'*R, so the determinant is const double expected = sqrt((invSigma / (2 * M_PI)).determinant()); - const double actual = evaluate(smallBayesNet, mean); + const double actual = smallBayesNet.evaluate(mean); EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9); } @@ -126,7 +93,7 @@ TEST(GaussianBayesNet, Evaluate2) { const Matrix R = noisyBayesNet.matrix().first; const Matrix invSigma = R.transpose() * R; const double expected = sqrt((invSigma / (2 * M_PI)).determinant()); - const double actual = evaluate(noisyBayesNet, mean); + const double actual = noisyBayesNet.evaluate(mean); EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9); } diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 6ec06a0ce..a3b64528b 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -130,6 +130,44 @@ TEST( GaussianConditional, equals ) EXPECT( expected.equals(actual) ); } +namespace density { +static const Key key = 77; +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)); +} // namespace density + +/* ************************************************************************* */ +// Check that the evaluate function matches direct calculation with R. +TEST(GaussianConditional, Evaluate1) { + // Let's evaluate at the mean + const VectorValues mean = density::unitPrior.solve(VectorValues()); + + // We get the Hessian matrix, which has noise model applied! + const Matrix invSigma = density::unitPrior.information(); + + // A Gaussian density ~ exp (-0.5*(Rx-d)'*(Rx-d)) + // which at the mean is 1.0! So, the only thing we need to calculate is + // the normalization constant 1.0/sqrt((2*pi*Sigma).det()). + // The covariance matrix inv(Sigma) = R'*R, so the determinant is + const double expected = sqrt((invSigma / (2 * M_PI)).determinant()); + const double actual = density::unitPrior.evaluate(mean); + EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9); +} + +// Check the evaluate with non-unit noise. +TEST(GaussianConditional, Evaluate2) { + // See comments in test above. + const VectorValues mean = density::widerPrior.solve(VectorValues()); + const Matrix R = density::widerPrior.R(); + const Matrix invSigma = density::widerPrior.information(); + const double expected = sqrt((invSigma / (2 * M_PI)).determinant()); + const double actual = density::widerPrior.evaluate(mean); + EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9); +} + /* ************************************************************************* */ TEST( GaussianConditional, solve ) {