diff --git a/gtsam/hybrid/HybridGaussianFactor.cpp b/gtsam/hybrid/HybridGaussianFactor.cpp index cabe6defcc..13b26db6c3 100644 --- a/gtsam/hybrid/HybridGaussianFactor.cpp +++ b/gtsam/hybrid/HybridGaussianFactor.cpp @@ -30,8 +30,8 @@ namespace gtsam { /** * @brief Helper function to augment the [A|b] matrices in the factor components - * with the normalizer values. - * This is done by storing the normalizer value in + * with the additional scalar values. + * This is done by storing the value in * the `b` vector as an additional row. * * @param factors DecisionTree of GaussianFactors and arbitrary scalars. @@ -56,7 +56,7 @@ HybridGaussianFactor::Factors augment( const HybridGaussianFactor::sharedFactor &gf) { auto jf = std::dynamic_pointer_cast(gf); if (!jf) return gf; - // If the log_normalizer is 0, do nothing + // If the value is 0, do nothing if (values(assignment) == 0.0) return gf; GaussianFactorGraph gfg; diff --git a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp index 246b8d4ce7..63b37e7d47 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactor.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactor.cpp @@ -771,7 +771,8 @@ static HybridGaussianFactorGraph CreateFactorGraph( // Create HybridGaussianFactor std::vector factors{ - {f0, ComputeLogNormalizer(model0)}, {f1, ComputeLogNormalizer(model1)}}; + {f0, ComputeLogNormalizerConstant(model0)}, + {f1, ComputeLogNormalizerConstant(model1)}}; 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 133b165c1a..98bbd36d89 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -869,7 +869,8 @@ static HybridNonlinearFactorGraph CreateFactorGraph( // Create HybridNonlinearFactor std::vector factors{ - {f0, ComputeLogNormalizer(model0)}, {f1, ComputeLogNormalizer(model1)}}; + {f0, ComputeLogNormalizerConstant(model0)}, + {f1, ComputeLogNormalizerConstant(model1)}}; HybridNonlinearFactor mixtureFactor({X(0), X(1)}, m1, factors); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index a586d119b7..d6526fb9cc 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -710,7 +710,7 @@ const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise){ } // namespace noiseModel /* *******************************************************************************/ -double ComputeLogNormalizer( +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 @@ -725,7 +725,7 @@ double ComputeLogNormalizer( size_t n = noise_model->dim(); constexpr double log2pi = 1.8378770664093454835606594728112; - return n * log2pi + logDeterminantSigma; + return 0.5*(n * log2pi + logDeterminantSigma); } } // gtsam diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index ffc1a3ebcd..d2ceb24dbe 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -757,10 +757,10 @@ namespace gtsam { * We compute this in the log-space for numerical accuracy. * * @param noise_model The Gaussian noise model - * whose normalizer we wish to compute. + * whose normalization constant we wish to compute. * @return double */ - GTSAM_EXPORT double ComputeLogNormalizer( + 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 49874f901c..4248f5beba 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -807,11 +807,11 @@ TEST(NoiseModel, NonDiagonalGaussian) } } -TEST(NoiseModel, ComputeLogNormalizer) { +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 = ComputeLogNormalizer(noise_model); + double actual_value = ComputeLogNormalizerConstant(noise_model); // Compute log(|2πΣ|) by hand. // = log(2π) + log(Σ) (since it is 1D) constexpr double log2pi = 1.8378770664093454835606594728112; @@ -821,7 +821,7 @@ TEST(NoiseModel, ComputeLogNormalizer) { // Similar situation in the 3D case size_t n = 3; auto noise_model2 = Isotropic::Sigma(n, sigma); - double actual_value2 = ComputeLogNormalizer(noise_model2); + double actual_value2 = ComputeLogNormalizerConstant(noise_model2); // We multiply by 3 due to the determinant double expected_value2 = n * (log2pi + log(sigma * sigma)); EXPECT_DOUBLES_EQUAL(expected_value2, actual_value2, 1e-9);