diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 7bee4c9fb..f39222122 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/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)); } /** diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index bb07a36aa..41464a110 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -426,6 +426,7 @@ TEST(GaussianFactorGraph, hessianDiagonal) { EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ TEST(GaussianFactorGraph, DenseSolve) { GaussianFactorGraph fg = createSimpleGaussianFactorGraph(); VectorValues expected = fg.optimize(); @@ -433,6 +434,28 @@ TEST(GaussianFactorGraph, DenseSolve) { EXPECT(assert_equal(expected, actual)); } +/* ************************************************************************* */ +TEST(GaussianFactorGraph, ProbPrime) { + GaussianFactorGraph gfg; + gfg.emplace_shared(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; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.cpp b/gtsam/nonlinear/NonlinearFactorGraph.cpp index 0e0d70268..89236ea87 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.cpp +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -45,7 +45,8 @@ template class FactorGraph; /* ************************************************************************* */ 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)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 160e46924..ea8748f63 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/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) */ diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 4dec08f45..8a360e454 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -107,6 +107,24 @@ TEST( NonlinearFactorGraph, probPrime ) DOUBLES_EQUAL(expected,actual,0); } +/* ************************************************************************* */ +TEST(NonlinearFactorGraph, ProbPrime2) { + NonlinearFactorGraph fg; + fg.emplace_shared>(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 ) {