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