add logNormalizingConstant test for GaussianConditional and make some doc fixes

gtsam/linear/GaussianConditional.cpp

@@ -67,7 +67,7 @@ namespace gtsam {
   GaussianConditional GaussianConditional::FromMeanAndStddev(Key key,
                                                              const Vector& mu,
                                                              double sigma) {
-    // |Rx - d| = |x-(Ay + b)|/sigma
+    // |Rx - d| = |x - mu|/sigma
     const Matrix R = Matrix::Identity(mu.size(), mu.size());
     const Vector& d = mu;
     return GaussianConditional(key, d, R,
@@ -189,7 +189,7 @@ double GaussianConditional::logNormalizationConstant() const {
 
 /* ************************************************************************* */
 //  density = k exp(-error(x))
-//  log = log(k) -error(x) - 0.5 * n*log(2*pi)
+//  log = log(k) -error(x)
 double GaussianConditional::logDensity(const VectorValues& x) const {
   return logNormalizationConstant() - error(x);
 }

gtsam/linear/tests/testGaussianConditional.cpp

@@ -466,6 +466,31 @@ TEST(GaussianConditional, sample) {
   // EXPECT(assert_equal(Vector2(31.0111856, 64.9850775), actual2[X(0)], 1e-5));
 }
 
+/* ************************************************************************* */
+TEST(GaussianConditional, LogNormalizationConstant) {
+  // Create univariate standard gaussian conditional
+  auto std_gaussian =
+      GaussianConditional::FromMeanAndStddev(X(0), Vector1::Zero(), 1.0);
+  VectorValues values;
+  values.insert(X(0), Vector1::Zero());
+  double logDensity = std_gaussian.logDensity(values);
+
+  // Regression.
+  // These values were computed by hand for a univariate standard gaussian.
+  EXPECT_DOUBLES_EQUAL(-0.9189385332046727, logDensity, 1e-9);
+  EXPECT_DOUBLES_EQUAL(0.3989422804014327, exp(logDensity), 1e-9);
+
+  // Similar test for multivariate gaussian but with sigma 2.0
+  double sigma = 2.0;
+  auto conditional = GaussianConditional::FromMeanAndStddev(X(0), Vector3::Zero(), sigma);
+  VectorValues x;
+  x.insert(X(0), Vector3::Zero());
+  Matrix3 Sigma = I_3x3 * sigma * sigma;
+  double expectedLogNormalizingConstant = log(1 / sqrt((2 * M_PI * Sigma).determinant()));
+
+  EXPECT_DOUBLES_EQUAL(expectedLogNormalizingConstant, conditional.logNormalizationConstant(), 1e-9);
+}
+
 /* ************************************************************************* */
 TEST(GaussianConditional, Print) {
   Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.).finished();