Correct densities (error already includes 0.5!)

gtsam/linear/GaussianBayesNet.h

@@ -88,20 +88,26 @@ namespace gtsam {
     /// @name Standard Interface
     /// @{
 
+    /**
+     * Calculate probability density for given values `x`:
+     *   exp(-error(x)) / sqrt((2*pi)^n*det(Sigma))
+     * where x is the vector of values, and Sigma is the covariance matrix.
+     * Note that error(x)=0.5*e'*e includes the 0.5 factor already.
+     */
+    double evaluate(const VectorValues& x) const;
+
+    /// Evaluate probability density, sugar.
+    double operator()(const VectorValues& x) const {
+      return evaluate(x);
+    }
+
     /**
      * Calculate log-density for given values `x`:
-     *   -0.5*(error + n*log(2*pi) + log det(Sigma))
+     *  -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
      * where x is the vector of values, and Sigma is the covariance matrix.
      */
     double logDensity(const VectorValues& x) const;
 
-    /**
-     * Calculate probability density for given values `x`:
-     *   exp(-0.5*error(x)) / sqrt((2*pi)^n*det(Sigma))
-     * where x is the vector of values, and Sigma is the covariance matrix.
-     */
-    double evaluate(const VectorValues& x) const;
-
     /// Solve the GaussianBayesNet, i.e. return \f$ x = R^{-1}*d \f$, by
     /// back-substitution
     VectorValues optimize() const;

gtsam/linear/GaussianConditional.cpp

@@ -169,13 +169,14 @@ double GaussianConditional::logDeterminant() const {
   return logDet;
 }
 
-  /* ************************************************************************* */
+/* ************************************************************************* */
+//  density = exp(-error(x)) / sqrt((2*pi)^n*det(Sigma))
+//  log = -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
 double GaussianConditional::logDensity(const VectorValues& x) const {
   constexpr double log2pi = 1.8378770664093454835606594728112;
   size_t n = d().size();
-  // log det(Sigma)) = - 2 * logDeterminant()
-  double sum = error(x) + n * log2pi - 2 * logDeterminant();
-  return -0.5 * sum;
+  // log det(Sigma)) = - 2.0 * logDeterminant()
+  return - error(x) - 0.5 * n * log2pi + logDeterminant();
 }
 
 /* ************************************************************************* */

gtsam/linear/GaussianConditional.h

@@ -121,20 +121,26 @@ namespace gtsam {
     /// @name Standard Interface
     /// @{
 
+    /**
+     * Calculate probability density for given values `x`:
+     *   exp(-error(x)) / sqrt((2*pi)^n*det(Sigma))
+     * where x is the vector of values, and Sigma is the covariance matrix.
+     * Note that error(x)=0.5*e'*e includes the 0.5 factor already.
+     */
+    double evaluate(const VectorValues& x) const;
+
+    /// Evaluate probability density, sugar.
+    double operator()(const VectorValues& x) const {
+      return evaluate(x);
+    }
+
     /**
      * Calculate log-density for given values `x`:
-     *   -0.5*(error + n*log(2*pi) + log det(Sigma))
+     *  -error(x) - 0.5 * n*log(2*pi) - 0.5 * log det(Sigma)
      * where x is the vector of values, and Sigma is the covariance matrix.
      */
     double logDensity(const VectorValues& x) const;
 
-    /**
-     * Calculate probability density for given values `x`:
-     *   exp(-0.5*error(x)) / sqrt((2*pi)^n*det(Sigma))
-     * where x is the vector of values, and Sigma is the covariance matrix.
-     */
-    double evaluate(const VectorValues& x) const;
-
     /** Return a view of the upper-triangular R block of the conditional */
     constABlock R() const { return Ab_.range(0, nrFrontals()); }
 
@@ -147,25 +153,32 @@ namespace gtsam {
     /** Get a view of the r.h.s. vector d */
     const constBVector d() const { return BaseFactor::getb(); }
 
-    /**
-     * @brief Compute the log determinant of the R matrix.
-     * For numerical stability, the determinant is computed in log
-     * form, so it is a summation rather than a multiplication.
-     *
-     * @return double
-     */
-    double logDeterminant() const;
-
     /**
      * @brief Compute the determinant of the R matrix.
      *
-     * The determinant is computed in log form (hence summation) for numerical
-     * stability and then exponentiated.
+     * The determinant is computed in log form using logDeterminant for
+     * numerical stability and then exponentiated.
+     * 
+     * Note, the covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$, and hence
+     * \f$ \det(\Sigma) = 1 / \det(R^T R) = 1 / determinant()^ 2 \f$.
      *
      * @return double
      */
     double determinant() const { return exp(this->logDeterminant()); }
 
+    /**
+     * @brief Compute the log determinant of the R matrix.
+     * 
+     * For numerical stability, the determinant is computed in log
+     * form, so it is a summation rather than a multiplication.
+     *
+     * Note, the covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$, and hence
+     * \f$ \log \det(\Sigma) = - \log \det(R^T R) = - 2 logDeterminant() \f$.
+     *
+     * @return double
+     */
+    double logDeterminant() const;
+
     /**
     * Solves a conditional Gaussian and writes the solution into the entries of
     * \c x for each frontal variable of the conditional.  The parents are

gtsam/linear/tests/testGaussianConditional.cpp

@@ -130,13 +130,15 @@ TEST( GaussianConditional, equals )
   EXPECT( expected.equals(actual) );
 }
 
+/* ************************************************************************* */
 namespace density {
 static const Key key = 77;
+static constexpr double sigma = 3.0;
 static const auto unitPrior =
                       GaussianConditional(key, Vector1::Constant(5), I_1x1),
-                  widerPrior =
-                      GaussianConditional(key, Vector1::Constant(5), I_1x1,
-                                          noiseModel::Isotropic::Sigma(1, 3.0));
+                  widerPrior = GaussianConditional(
+                      key, Vector1::Constant(5), I_1x1,
+                      noiseModel::Isotropic::Sigma(1, sigma));
 }  // namespace density
 
 /* ************************************************************************* */
@@ -155,8 +157,23 @@ TEST(GaussianConditional, Evaluate1) {
   const double expected = sqrt((invSigma / (2 * M_PI)).determinant());
   const double actual = density::unitPrior.evaluate(mean);
   EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9);
+
+  using density::key;
+  using density::sigma;
+
+  // Let's numerically integrate and see that we integrate to 1.0.
+  double integral = 0.0;
+  // Loop from -5*sigma to 5*sigma in 0.1*sigma steps:
+  for (double x = -5.0 * sigma; x <= 5.0 * sigma; x += 0.1 * sigma) {
+    VectorValues xValues;
+    xValues.insert(key, mean.at(key) + Vector1(x));
+    const double density = density::unitPrior.evaluate(xValues);
+    integral += 0.1 * sigma * density;
+  }
+  EXPECT_DOUBLES_EQUAL(1.0, integral, 1e-9);
 }
 
+/* ************************************************************************* */
 // Check the evaluate with non-unit noise.
 TEST(GaussianConditional, Evaluate2) {
   // See comments in test above.
@@ -166,6 +183,20 @@ TEST(GaussianConditional, Evaluate2) {
   const double expected = sqrt((invSigma / (2 * M_PI)).determinant());
   const double actual = density::widerPrior.evaluate(mean);
   EXPECT_DOUBLES_EQUAL(expected, actual, 1e-9);
+
+  using density::key;
+  using density::sigma;
+
+  // Let's numerically integrate and see that we integrate to 1.0.
+  double integral = 0.0;
+  // Loop from -5*sigma to 5*sigma in 0.1*sigma steps:
+  for (double x = -5.0 * sigma; x <= 5.0 * sigma; x += 0.1 * sigma) {
+    VectorValues xValues;
+    xValues.insert(key, mean.at(key) + Vector1(x));
+    const double density = density::widerPrior.evaluate(xValues);
+    integral += 0.1 * sigma * density;
+  }
+  EXPECT_DOUBLES_EQUAL(1.0, integral, 1e-5);
 }
 
 /* ************************************************************************* */