12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix machine precision bug in DogLeg compute blend 

This commit fixes a bug that could cause the incorrect solution to be
returned from ComputeBlend that is documented in Issue #1861.
release/4.3a0
Dan McGann 2024-10-08 09:57:39 -04:00
parent 498d0b38af
commit a3087ffff7
2 changed files with 23 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam/nonlinear/DoglegOptimizerImpl.cpp
View File

@ -67,12 +67,14 @@ VectorValues DoglegOptimizerImpl::ComputeBlend(double delta, const VectorValues&
double tau1 = (-b + sqrt_b_m4ac) / (2.*a); double tau1 = (-b + sqrt_b_m4ac) / (2.*a);
double tau2 = (-b - sqrt_b_m4ac) / (2.*a); double tau2 = (-b - sqrt_b_m4ac) / (2.*a);
// Determine correct solution accounting for machine precision
double tau; double tau;
if(0.0 <= tau1 && tau1 <= 1.0) { const double eps = std::numeric_limits<double>::epsilon();
assert(!(0.0 <= tau2 && tau2 <= 1.0)); if(-eps <= tau1 && tau1 <= 1.0 + eps) {
assert(!(-eps <= tau2 && tau2 <= 1.0 + eps));
tau = tau1; tau = tau1;
} else { } else {
assert(0.0 <= tau2 && tau2 <= 1.0); assert(-eps <= tau2 && tau2 <= 1.0 + eps);
tau = tau2; tau = tau2;
} }

18
tests/testDoglegOptimizer.cpp
View File

@ -72,6 +72,24 @@ TEST(DoglegOptimizer, ComputeBlend) {
DOUBLES_EQUAL(Delta, xb.vector().norm(), 1e-10); DOUBLES_EQUAL(Delta, xb.vector().norm(), 1e-10);
} }
/* ************************************************************************* */
TEST(DoglegOptimizer, ComputeBlendEdgeCases) {
// Test Derived from Issue #1861
// Evaluate ComputeBlend Behavior for edge cases where the trust region
// is equal in size to that of the newton step or the gradient step.
// Simulated Newton (n) and Gradient Descent (u) step vectors w/ ||n|| > ||u||
VectorValues::Dims dims;
dims[0] = 3;
VectorValues n(Vector3(0.3233546123, -0.2133456123, 0.3664345632), dims);
VectorValues u(Vector3(0.0023456342, -0.04535687, 0.087345661212), dims);
// Test upper edge case where trust region is equal to magnitude of newton step
EXPECT(assert_equal(n, DoglegOptimizerImpl::ComputeBlend(n.norm(), u, n, false)));
// Test lower edge case where trust region is equal to magnitude of gradient step
EXPECT(assert_equal(u, DoglegOptimizerImpl::ComputeBlend(u.norm(), u, n, false)));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(DoglegOptimizer, ComputeDoglegPoint) { TEST(DoglegOptimizer, ComputeDoglegPoint) {
// Create an arbitrary Bayes Net // Create an arbitrary Bayes Net