add better tests for probPrime and add a fix

release/4.3a0
Varun Agrawal 2022-01-12 16:50:10 -05:00
parent 7cc3eb844b
commit 3c804d89b5
5 changed files with 46 additions and 3 deletions

View File

@ -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));
}
/**

View File

@ -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<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;

View File

@ -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));
}
/* ************************************************************************* */

View File

@ -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) */

View File

@ -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 )
{