correct documentation and test for ComputeLogNormalizer

gtsam/linear/NoiseModel.cpp

@@ -714,6 +714,9 @@ double ComputeLogNormalizer(
     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); })

gtsam/linear/NoiseModel.h

@@ -752,7 +752,7 @@ namespace gtsam {
   template<> struct traits<noiseModel::Unit> : public Testable<noiseModel::Unit> {};
 
   /**
-   * @brief Helper function to compute the sqrt(|2πΣ|) normalizer values
+   * @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.
    *

gtsam/linear/tests/testNoiseModel.cpp

@@ -807,6 +807,26 @@ TEST(NoiseModel, NonDiagonalGaussian)
   }
 }
 
+TEST(NoiseModel, ComputeLogNormalizer) {
+  // 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);
+  // Compute log(|2πΣ|) by hand.
+  // = log(2π) + log(Σ) (since it is 1D)
+  constexpr double log2pi = 1.8378770664093454835606594728112;
+  double expected_value = log2pi + log(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 = ComputeLogNormalizer(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);
+}
+
 /* ************************************************************************* */
 int main() {  TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */