diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index ecfa02282..3174c2f44 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -67,7 +67,7 @@ namespace gtsam { GaussianConditional GaussianConditional::FromMeanAndStddev(Key key, const Vector& mu, double sigma) { - // |Rx - d| = |x-(Ay + b)|/sigma + // |Rx - d| = |x - mu|/sigma const Matrix R = Matrix::Identity(mu.size(), mu.size()); const Vector& d = mu; return GaussianConditional(key, d, R, @@ -189,7 +189,7 @@ double GaussianConditional::logNormalizationConstant() const { /* ************************************************************************* */ // density = k exp(-error(x)) -// log = log(k) -error(x) - 0.5 * n*log(2*pi) +// log = log(k) -error(x) double GaussianConditional::logDensity(const VectorValues& x) const { return logNormalizationConstant() - error(x); } diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index 20d730856..144269b44 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -466,6 +466,31 @@ TEST(GaussianConditional, sample) { // EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5)); } +/* ************************************************************************* */ +TEST(GaussianConditional, LogNormalizationConstant) { + // Create univariate standard gaussian conditional + auto std_gaussian = + GaussianConditional::FromMeanAndStddev(X(0), Vector1::Zero(), 1.0); + VectorValues values; + values.insert(X(0), Vector1::Zero()); + double logDensity = std_gaussian.logDensity(values); + + // Regression. + // These values were computed by hand for a univariate standard gaussian. + EXPECT_DOUBLES_EQUAL(-0.9189385332046727, logDensity, 1e-9); + EXPECT_DOUBLES_EQUAL(0.3989422804014327, exp(logDensity), 1e-9); + + // Similar test for multivariate gaussian but with sigma 2.0 + double sigma = 2.0; + auto conditional = GaussianConditional::FromMeanAndStddev(X(0), Vector3::Zero(), sigma); + VectorValues x; + x.insert(X(0), Vector3::Zero()); + Matrix3 Sigma = I_3x3 * sigma * sigma; + double expectedLogNormalizingConstant = log(1 / sqrt((2 * M_PI * Sigma).determinant())); + + EXPECT_DOUBLES_EQUAL(expectedLogNormalizingConstant, conditional.logNormalizationConstant(), 1e-9); +} + /* ************************************************************************* */ TEST(GaussianConditional, Print) { Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();