From ecbf3d872ea467c94110b2f822d9e515e6757df6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 21 Sep 2024 05:15:35 -0400 Subject: [PATCH] make logNormalizationConstant return -log(k) --- gtsam/inference/Conditional-inst.h | 2 +- gtsam/inference/Conditional.h | 12 +++++++----- gtsam/linear/GaussianBayesNet.cpp | 13 +++++++------ gtsam/linear/GaussianConditional.cpp | 16 ++++++++-------- gtsam/linear/GaussianConditional.h | 6 ++++-- gtsam/linear/NoiseModel.cpp | 10 +++++----- gtsam/linear/NoiseModel.h | 4 ++-- gtsam/linear/tests/testGaussianBayesNet.cpp | 4 ++-- gtsam/linear/tests/testGaussianConditional.cpp | 11 ++++++----- gtsam/linear/tests/testGaussianDensity.cpp | 2 +- gtsam/linear/tests/testNoiseModel.cpp | 12 ++++++------ 11 files changed, 49 insertions(+), 43 deletions(-) diff --git a/gtsam/inference/Conditional-inst.h b/gtsam/inference/Conditional-inst.h index 9377e8cc4..b61a5ac22 100644 --- a/gtsam/inference/Conditional-inst.h +++ b/gtsam/inference/Conditional-inst.h @@ -89,7 +89,7 @@ bool Conditional::CheckInvariants( // normalization constant const double error = conditional.error(values); if (error < 0.0) return false; // prob_or_density is negative. - const double expected = conditional.logNormalizationConstant() - error; + const double expected = -(conditional.logNormalizationConstant() + error); if (std::abs(logProb - expected) > 1e-9) return false; // logProb is not consistent with error return true; diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index c76c05ab1..936a078a8 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -34,9 +34,10 @@ namespace gtsam { * `logProbability` is the main methods that need to be implemented in derived * classes. These two methods relate to the `error` method in the factor by: * probability(x) = k exp(-error(x)) - * where k is a normalization constant making \int probability(x) == 1.0, and - * logProbability(x) = K - error(x) - * i.e., K = log(K). The normalization constant K is assumed to *not* depend + * where k is a normalization constant making + * \int probability(x) = \int k exp(-error(x)) == 1.0, and + * logProbability(x) = -(K + error(x)) + * i.e., K = -log(k). The normalization constant k is assumed to *not* depend * on any argument, only (possibly) on the conditional parameters. * This class provides a default logNormalizationConstant() == 0.0. * @@ -163,8 +164,9 @@ namespace gtsam { } /** - * All conditional types need to implement a log normalization constant to - * make it such that error>=0. + * All conditional types need to implement a + * (negative) log normalization constant + * to make it such that error>=0. */ virtual double logNormalizationConstant() const; diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index 861e19cc9..b7f296ea5 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -246,14 +246,15 @@ namespace gtsam { double GaussianBayesNet::logNormalizationConstant() const { /* normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) - logConstant = -0.5 * n*log(2*pi) - 0.5 * log det(Sigma) + logConstant = -log(normalizationConstant) + = 0.5 * n*log(2*pi) + 0.5 * log(det(Sigma)) - log det(Sigma)) = -2.0 * logDeterminant() - thus, logConstant = -0.5*n*log(2*pi) + logDeterminant() + log(det(Sigma)) = -2.0 * logDeterminant() + thus, logConstant = 0.5*n*log(2*pi) - logDeterminant() - BayesNet logConstant = sum(-0.5*n_i*log(2*pi) + logDeterminant_i()) - = sum(-0.5*n_i*log(2*pi)) + sum(logDeterminant_i()) - = sum(-0.5*n_i*log(2*pi)) + bn->logDeterminant() + BayesNet logConstant = sum(0.5*n_i*log(2*pi) - logDeterminant_i()) + = sum(0.5*n_i*log(2*pi)) - sum(logDeterminant_i()) + = sum(0.5*n_i*log(2*pi)) - bn->logDeterminant() */ double logNormConst = 0.0; for (const sharedConditional& cg : *this) { diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 585bbdb33..fc633d04d 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -181,24 +181,24 @@ namespace gtsam { /* ************************************************************************* */ // normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) - // log = - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma) + // neg-log = 0.5 * n*log(2*pi) + 0.5 * log det(Sigma) double GaussianConditional::logNormalizationConstant() const { constexpr double log2pi = 1.8378770664093454835606594728112; size_t n = d().size(); // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1} // log det(Sigma) = -log(det(R'R)) = -2*log(det(R)) // Hence, log det(Sigma)) = -2.0 * logDeterminant() - // which gives log = -0.5*n*log(2*pi) - 0.5*(-2.0 * logDeterminant()) - // = -0.5*n*log(2*pi) + (0.5*2.0 * logDeterminant()) - // = -0.5*n*log(2*pi) + logDeterminant() - return -0.5 * n * log2pi + logDeterminant(); + // which gives neg-log = 0.5*n*log(2*pi) + 0.5*(-2.0 * logDeterminant()) + // = 0.5*n*log(2*pi) - (0.5*2.0 * logDeterminant()) + // = 0.5*n*log(2*pi) - logDeterminant() + return 0.5 * n * log2pi - logDeterminant(); } /* ************************************************************************* */ - // density = k exp(-error(x)) - // log = log(k) - error(x) + // density = 1/k exp(-error(x)) + // log = -log(k) - error(x) double GaussianConditional::logProbability(const VectorValues& x) const { - return logNormalizationConstant() - error(x); + return -logNormalizationConstant() - error(x); } double GaussianConditional::logProbability(const HybridValues& x) const { diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 420efabca..757687ed8 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -133,8 +133,10 @@ namespace gtsam { /// @{ /** - * normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) - * log = - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma) + * Return normalization constant in negative log space. + * + * normalization constant k = 1.0 / sqrt((2*pi)^n*det(Sigma)) + * -log(k) = 0.5 * n*log(2*pi) + 0.5 * log det(Sigma) */ double logNormalizationConstant() const override; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index de69fdce9..ea9ec1aec 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -257,13 +257,13 @@ double Gaussian::logDeterminant() const { /* *******************************************************************************/ double Gaussian::logNormalizationConstant() const { // log(det(Sigma)) = -2.0 * logDetR - // which gives log = -0.5*n*log(2*pi) - 0.5*(-2.0 * logDetR()) - // = -0.5*n*log(2*pi) + (0.5*2.0 * logDetR()) - // = -0.5*n*log(2*pi) + logDetR() + // which gives neg-log = 0.5*n*log(2*pi) + 0.5*(-2.0 * logDetR()) + // = 0.5*n*log(2*pi) - (0.5*2.0 * logDetR()) + // = 0.5*n*log(2*pi) - logDetR() size_t n = dim(); constexpr double log2pi = 1.8378770664093454835606594728112; - // Get 1/log(\sqrt(|2pi Sigma|)) = -0.5*log(|2pi Sigma|) - return -0.5 * n * log2pi + logDetR(); + // Get -log(1/\sqrt(|2pi Sigma|)) = 0.5*log(|2pi Sigma|) + return 0.5 * n * log2pi - logDetR(); } diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 91c11d410..31ffd1107 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -274,8 +274,8 @@ namespace gtsam { /** * @brief Method to compute the normalization constant * for a Gaussian noise model k = \sqrt(1/|2πΣ|). - * We compute this in the log-space for numerical accuracy, - * thus returning log(k). + * We compute this in the negative log-space for numerical accuracy, + * thus returning -log(k). * * @return double */ diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 99453ee4e..a186eb2b2 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -76,11 +76,11 @@ 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 constant = sqrt((invSigma / (2 * M_PI)).determinant()); - EXPECT_DOUBLES_EQUAL(log(constant), + EXPECT_DOUBLES_EQUAL(-log(constant), smallBayesNet.at(0)->logNormalizationConstant() + smallBayesNet.at(1)->logNormalizationConstant(), 1e-9); - EXPECT_DOUBLES_EQUAL(log(constant), smallBayesNet.logNormalizationConstant(), + EXPECT_DOUBLES_EQUAL(-log(constant), smallBayesNet.logNormalizationConstant(), 1e-9); const double actual = smallBayesNet.evaluate(mean); EXPECT_DOUBLES_EQUAL(constant, actual, 1e-9); diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index dcd821889..b03e0a060 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -492,9 +492,10 @@ TEST(GaussianConditional, LogNormalizationConstant) { VectorValues x; x.insert(X(0), Vector3::Zero()); Matrix3 Sigma = I_3x3 * sigma * sigma; - double expectedLogNormalizingConstant = log(1 / sqrt((2 * M_PI * Sigma).determinant())); + double expectedLogNormalizationConstant = + -log(1 / sqrt((2 * M_PI * Sigma).determinant())); - EXPECT_DOUBLES_EQUAL(expectedLogNormalizingConstant, + EXPECT_DOUBLES_EQUAL(expectedLogNormalizationConstant, conditional.logNormalizationConstant(), 1e-9); } @@ -516,7 +517,7 @@ TEST(GaussianConditional, Print) { " d = [ 20 40 ]\n" " mean: 1 elements\n" " x0: 20 40\n" - " logNormalizationConstant: -4.0351\n" + " logNormalizationConstant: 4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected, conditional, "GaussianConditional")); @@ -531,7 +532,7 @@ TEST(GaussianConditional, Print) { " S[x1] = [ -1 -2 ]\n" " [ -3 -4 ]\n" " d = [ 20 40 ]\n" - " logNormalizationConstant: -4.0351\n" + " logNormalizationConstant: 4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected1, conditional1, "GaussianConditional")); @@ -547,7 +548,7 @@ TEST(GaussianConditional, Print) { " S[y1] = [ -5 -6 ]\n" " [ -7 -8 ]\n" " d = [ 20 40 ]\n" - " logNormalizationConstant: -4.0351\n" + " logNormalizationConstant: 4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected2, conditional2, "GaussianConditional")); } diff --git a/gtsam/linear/tests/testGaussianDensity.cpp b/gtsam/linear/tests/testGaussianDensity.cpp index 97eb0de70..3226f40ab 100644 --- a/gtsam/linear/tests/testGaussianDensity.cpp +++ b/gtsam/linear/tests/testGaussianDensity.cpp @@ -55,7 +55,7 @@ TEST(GaussianDensity, FromMeanAndStddev) { double expected1 = 0.5 * e.dot(e); EXPECT_DOUBLES_EQUAL(expected1, density.error(values), 1e-9); - double expected2 = density.logNormalizationConstant()- 0.5 * e.dot(e); + double expected2 = -(density.logNormalizationConstant() + 0.5 * e.dot(e)); EXPECT_DOUBLES_EQUAL(expected2, density.logProbability(values), 1e-9); } diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 6aea154ee..59ee05d07 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -810,9 +810,9 @@ TEST(NoiseModel, NonDiagonalGaussian) TEST(NoiseModel, LogNormalizationConstant1D) { // Very simple 1D noise model, which we can compute by hand. double sigma = 0.1; - // For expected values, we compute 1/log(sqrt(|2πΣ|)) by hand. - // = -0.5*(log(2π) + log(Σ)) (since it is 1D) - double expected_value = -0.5 * log(2 * M_PI * sigma * sigma); + // For expected values, we compute -log(1/sqrt(|2πΣ|)) by hand. + // = 0.5*(log(2π) + log(Σ)) (since it is 1D) + double expected_value = 0.5 * log(2 * M_PI * sigma * sigma); // Gaussian { @@ -839,7 +839,7 @@ TEST(NoiseModel, LogNormalizationConstant1D) { auto noise_model = Unit::Create(1); double actual_value = noise_model->logNormalizationConstant(); double sigma = 1.0; - expected_value = -0.5 * log(2 * M_PI * sigma * sigma); + expected_value = 0.5 * log(2 * M_PI * sigma * sigma); EXPECT_DOUBLES_EQUAL(expected_value, actual_value, 1e-9); } } @@ -850,7 +850,7 @@ TEST(NoiseModel, LogNormalizationConstant3D) { size_t n = 3; // We compute the expected values just like in the LogNormalizationConstant1D // test, but we multiply by 3 due to the determinant. - double expected_value = -0.5 * n * log(2 * M_PI * sigma * sigma); + double expected_value = 0.5 * n * log(2 * M_PI * sigma * sigma); // Gaussian { @@ -879,7 +879,7 @@ TEST(NoiseModel, LogNormalizationConstant3D) { auto noise_model = Unit::Create(3); double actual_value = noise_model->logNormalizationConstant(); double sigma = 1.0; - expected_value = -0.5 * n * log(2 * M_PI * sigma * sigma); + expected_value = 0.5 * n * log(2 * M_PI * sigma * sigma); EXPECT_DOUBLES_EQUAL(expected_value, actual_value, 1e-9); } }