diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index d6526fb9c..af3081b9f 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -238,6 +238,25 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const Matrix Gaussian::information() const { return R().transpose() * R(); } +/* *******************************************************************************/ +double Gaussian::logNormalizationConstant() const { + // Since noise models are Gaussian, we can get the logDeterminant easily + // 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() + double logDetR = + R().diagonal().unaryExpr([](double x) { return log(x); }).sum(); + + 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; +} + + /* ************************************************************************* */ // Diagonal /* ************************************************************************* */ @@ -708,24 +727,4 @@ const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){ /* ************************************************************************* */ } // namespace noiseModel - -/* *******************************************************************************/ -double ComputeLogNormalizerConstant( - const noiseModel::Gaussian::shared_ptr& noise_model) { - // Since noise models are Gaussian, we can get the logDeterminant using - // the same trick as in GaussianConditional - // 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 * logDetR() - double logDetR = noise_model->R() - .diagonal() - .unaryExpr([](double x) { return log(x); }) - .sum(); - double logDeterminantSigma = -2.0 * logDetR; - - size_t n = noise_model->dim(); - constexpr double log2pi = 1.8378770664093454835606594728112; - return 0.5*(n * log2pi + logDeterminantSigma); -} - } // gtsam diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index d2ceb24db..b8e6c09bc 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -266,7 +266,20 @@ namespace gtsam { /// Compute covariance matrix virtual Matrix covariance() const; - private: + /** + * @brief Helper method to compute the normalization constant + * for a Gaussian noise model. + * k = 1/log(\sqrt(|2πΣ|)) + * + * We compute this in the log-space for numerical accuracy. + * + * @param noise_model The Gaussian noise model + * whose normalization constant we wish to compute. + * @return double + */ + double logNormalizationConstant() const; + + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; @@ -751,18 +764,6 @@ namespace gtsam { template<> struct traits : public Testable {}; template<> struct traits : public Testable {}; - /** - * @brief Helper function to compute the log(|2πΣ|) normalizer values - * for a Gaussian noise model. - * We compute this in the log-space for numerical accuracy. - * - * @param noise_model The Gaussian noise model - * whose normalization constant we wish to compute. - * @return double - */ - GTSAM_EXPORT double ComputeLogNormalizerConstant( - const noiseModel::Gaussian::shared_ptr& noise_model); - } //\ namespace gtsam diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 4248f5beb..b6e5785b6 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -811,19 +811,18 @@ TEST(NoiseModel, ComputeLogNormalizerConstant) { // Very simple 1D noise model, which we can compute by hand. double sigma = 0.1; auto noise_model = Isotropic::Sigma(1, sigma); - double actual_value = ComputeLogNormalizerConstant(noise_model); - // Compute log(|2πΣ|) by hand. - // = log(2π) + log(Σ) (since it is 1D) - constexpr double log2pi = 1.8378770664093454835606594728112; - double expected_value = log2pi + log(sigma * sigma); + double actual_value = noise_model->logNormalizationConstant(); + // 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); EXPECT_DOUBLES_EQUAL(expected_value, actual_value, 1e-9); // Similar situation in the 3D case size_t n = 3; auto noise_model2 = Isotropic::Sigma(n, sigma); - double actual_value2 = ComputeLogNormalizerConstant(noise_model2); + double actual_value2 = noise_model2->logNormalizationConstant(); // We multiply by 3 due to the determinant - double expected_value2 = n * (log2pi + log(sigma * sigma)); + double expected_value2 = -0.5 * n * log(2 * M_PI * sigma * sigma); EXPECT_DOUBLES_EQUAL(expected_value2, actual_value2, 1e-9); }