add better tests for probPrime and add a fix
parent
7cc3eb844b
commit
3c804d89b5
|
@ -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));
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
@ -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) */
|
||||
|
|
|
@ -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 )
|
||||
{
|
||||
|
|
Loading…
Reference in New Issue