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

@@ -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<double>::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;
   }
 

tests/testDoglegOptimizer.cpp

@@ -72,6 +72,22 @@ 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 n(Vector3(0.3233546123, -0.2133456123, 0.3664345632), {{0, 3}});
+  VectorValues u(Vector3(0.0023456342, -0.04535687, 0.087345661212), {{0, 3}});
+
+  // Test upper edge case where trust region is equal to magnitude of newton step
+  DOUBLES_EQUAL(1.0, ComputeBlend(n.norm(), u, n, false), 1e-10);
+  // Test lower edge case where trust region is equal to magnitude of gradient step 
+  DOUBLES_EQUAL(0.0, ComputeBlend(u.norm(), u, n, false), 1e-10);
+}
+
 /* ************************************************************************* */
 TEST(DoglegOptimizer, ComputeDoglegPoint) {
   // Create an arbitrary Bayes Net