From a3087ffff791cfc68e16a105c3ce6608d34320ba Mon Sep 17 00:00:00 2001 From: Dan McGann Date: Tue, 8 Oct 2024 09:57:39 -0400 Subject: [PATCH] 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. --- gtsam/nonlinear/DoglegOptimizerImpl.cpp | 8 +++++--- tests/testDoglegOptimizer.cpp | 18 ++++++++++++++++++ 2 files changed, 23 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.cpp b/gtsam/nonlinear/DoglegOptimizerImpl.cpp index 7e9db6b643..4f1c6fb54e 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.cpp +++ b/gtsam/nonlinear/DoglegOptimizerImpl.cpp @@ -67,12 +67,14 @@ VectorValues DoglegOptimizerImpl::ComputeBlend(double delta, const VectorValues& double tau1 = (-b + sqrt_b_m4ac) / (2.*a); double tau2 = (-b - sqrt_b_m4ac) / (2.*a); + // Determine correct solution accounting for machine precision double tau; - if(0.0 <= tau1 && tau1 <= 1.0) { - assert(!(0.0 <= tau2 && tau2 <= 1.0)); + const double eps = std::numeric_limits::epsilon(); + if(-eps <= tau1 && tau1 <= 1.0 + eps) { + assert(!(-eps <= tau2 && tau2 <= 1.0 + eps)); tau = tau1; } else { - assert(0.0 <= tau2 && tau2 <= 1.0); + assert(-eps <= tau2 && tau2 <= 1.0 + eps); tau = tau2; } diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 6fbc522d36..870ad5fb8d 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -72,6 +72,24 @@ TEST(DoglegOptimizer, ComputeBlend) { 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) { // Create an arbitrary Bayes Net