diff --git a/gtsam/hybrid/HybridGaussianFactor.h b/gtsam/hybrid/HybridGaussianFactor.h index d724bdff3..a86714863 100644 --- a/gtsam/hybrid/HybridGaussianFactor.h +++ b/gtsam/hybrid/HybridGaussianFactor.h @@ -45,7 +45,7 @@ using GaussianFactorValuePair = std::pair; * where the set of discrete variables indexes to * the continuous gaussian distribution. * - * In factor graphs the error function typically returns 0.5*|h(x)-z|^2, i.e., + * In factor graphs the error function typically returns 0.5*|A*x - b|^2, i.e., * the negative log-likelihood for a Gaussian noise model. * In hybrid factor graphs we allow *adding* an arbitrary scalar dependent on * the discrete assignment. diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 63b37e7d4..1ecd77132 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -770,9 +770,10 @@ static HybridGaussianFactorGraph CreateFactorGraph( ->linearize(values); // Create HybridGaussianFactor + // We multiply by -2 since the we want the underlying scalar to be log(|2πΣ|) std::vector factors{ - {f0, ComputeLogNormalizerConstant(model0)}, - {f1, ComputeLogNormalizerConstant(model1)}}; + {f0, -2 * model0->logNormalizationConstant()}, + {f1, -2 * model1->logNormalizationConstant()}}; HybridGaussianFactor motionFactor({X(0), X(1)}, m1, factors); HybridGaussianFactorGraph hfg; diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 98bbd36d8..8d2a5a74b 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -868,9 +868,10 @@ static HybridNonlinearFactorGraph CreateFactorGraph( std::make_shared>(X(0), X(1), means[1], model1); // Create HybridNonlinearFactor + // We multiply by -2 since the we want the underlying scalar to be log(|2πΣ|) std::vector factors{ - {f0, ComputeLogNormalizerConstant(model0)}, - {f1, ComputeLogNormalizerConstant(model1)}}; + {f0, -2 * model0->logNormalizationConstant()}, + {f1, -2 * model1->logNormalizationConstant()}}; HybridNonlinearFactor mixtureFactor({X(0), X(1)}, m1, factors); 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); }