Fix probPrime

gtsam/linear/GaussianFactorGraph.h

@@ -154,7 +154,8 @@ namespace gtsam {
 
     /** Unnormalized probability. O(n) */
     double probPrime(const VectorValues& c) const {
-      return exp(-0.5 * error(c));
+      // NOTE the 0.5 constant is handled by the factor error.
+      return exp(-error(c));
     }
 
     /**

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -426,13 +426,36 @@ TEST(GaussianFactorGraph, hessianDiagonal) {
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
 TEST(GaussianFactorGraph, DenseSolve) {
   GaussianFactorGraph fg = createSimpleGaussianFactorGraph();
   VectorValues expected = fg.optimize();
   VectorValues actual = fg.optimizeDensely();
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST(GaussianFactorGraph, ProbPrime) {
+  GaussianFactorGraph gfg;
+  gfg.emplace_shared<JacobianFactor>(1, I_1x1, Z_1x1,
+                                     noiseModel::Isotropic::Sigma(1, 1.0));
+
+  VectorValues values;
+  values.insert(1, I_1x1);
+
+  // We are testing the normal distribution PDF where info matrix Σ = 1,
+  // mean mu = 0  and x = 1.
+  // Therefore factor squared error: y = 0.5 * (Σ*x - mu)^2 =
+  // 0.5 * (1.0 - 0)^2 = 0.5
+  // NOTE the 0.5 constant is a part of the factor error.
+  EXPECT_DOUBLES_EQUAL(0.5, gfg.error(values), 1e-12);
+
+  // The gaussian PDF value is: exp^(-0.5 * (Σ*x - mu)^2) / sqrt(2 * PI)
+  // Ignore the denominator and we get: exp^(-0.5 * (1.0)^2) = exp^(-0.5)
+  double expected = exp(-0.5);
+  EXPECT_DOUBLES_EQUAL(expected, gfg.probPrime(values), 1e-12);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -45,7 +45,8 @@ template class FactorGraph<NonlinearFactor>;
 
 /* ************************************************************************* */
 double NonlinearFactorGraph::probPrime(const Values& values) const {
-  return exp(-0.5 * error(values));
+  // NOTE the 0.5 constant is handled by the factor error.
+  return exp(-error(values));
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -90,7 +90,7 @@ namespace gtsam {
     /** Test equality */
     bool equals(const NonlinearFactorGraph& other, double tol = 1e-9) const;
 
-    /** unnormalized error, \f$ 0.5 \sum_i (h_i(X_i)-z)^2/\sigma^2 \f$ in the most common case */
+    /** unnormalized error, \f$ \sum_i 0.5 (h_i(X_i)-z)^2 / \sigma^2 \f$ in the most common case */
     double error(const Values& values) const;
 
     /** Unnormalized probability. O(n) */

tests/testNonlinearFactorGraph.cpp

@@ -107,6 +107,24 @@ TEST( NonlinearFactorGraph, probPrime )
   DOUBLES_EQUAL(expected,actual,0);
 }
 
+/* ************************************************************************* */
+TEST(NonlinearFactorGraph, ProbPrime2) {
+  NonlinearFactorGraph fg;
+  fg.emplace_shared<PriorFactor<double>>(1, 0.0,
+                                         noiseModel::Isotropic::Sigma(1, 1.0));
+
+  Values values;
+  values.insert(1, 1.0);
+
+  // The prior factor squared error is: 0.5.
+  EXPECT_DOUBLES_EQUAL(0.5, fg.error(values), 1e-12);
+
+  // The probability value is: exp^(-factor_error) / sqrt(2 * PI)
+  // Ignore the denominator and we get: exp^(-factor_error) = exp^(-0.5)
+  double expected = exp(-0.5);
+  EXPECT_DOUBLES_EQUAL(expected, fg.probPrime(values), 1e-12);
+}
+
 /* ************************************************************************* */
 TEST( NonlinearFactorGraph, linearize )
 {