Updated NumericalDiff API to expose abs_tol and make setting rel_tol simpler

libeigen/eigen!3330

Co-authored-by: leake <carl.leake@karanadyn.com>
diff --git a/CHANGELOG.md b/CHANGELOG.md
index deca581..2a6ef15 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -21,6 +21,9 @@
 - `JacobiSVD` with `FullPivHouseholderQRPreconditioner` accepted `ComputeThinV` and returned a non-orthogonal `V` for wide inputs; it now fails to compile, or asserts when requested at runtime [!3212]
 - `HouseholderQR` (and the decompositions and `SplineFitting` built on it) and `JacobiSVD` did not compile with `AutoDiffScalar` [#3192, !3265]
 
+Breaking changes:
+- The default `epsfcn` for central mode differences was changed to `cbrt(NumTraits<Scalar>::epsilon())` (used to be `sqrt(NumTraits<Scalar>::epsilon())`). The new default should reduce numerical error in most cases.
+
 Deprecations:
 - `ArrayBase::shiftRight<N>()` and `ArrayBase::shiftLeft<N>()`, in favour of `arithmeticShiftRight<N>()` and `logicalShiftLeft<N>()` [!2816]
 
@@ -1957,4 +1960,4 @@
 
 ## [2.0.0] - 2009-02-02
 
-First public release.
\ No newline at end of file
+First public release.
diff --git a/contrib/Eigen/src/NumericalDiff/NumericalDiff.h b/contrib/Eigen/src/NumericalDiff/NumericalDiff.h
index 1944f98..2d0ffbe 100644
--- a/contrib/Eigen/src/NumericalDiff/NumericalDiff.h
+++ b/contrib/Eigen/src/NumericalDiff/NumericalDiff.h
@@ -56,34 +56,71 @@
   typedef typename Functor::ValueType ValueType;
   typedef typename Functor::JacobianType JacobianType;
 
-  NumericalDiff(Scalar _epsfcn = 0.) : Functor(), epsfcn(_epsfcn) {}
-  NumericalDiff(const Functor& f, Scalar _epsfcn = 0.) : Functor(f), epsfcn(_epsfcn) {}
+ private:
+  // Fallback/Forward mode helper
+  static Scalar defaultRelTol() {
+    return mode == Central ? numext::cbrt(NumTraits<Scalar>::epsilon()) : numext::sqrt(NumTraits<Scalar>::epsilon());
+  }
+
+ public:
+  NumericalDiff(Scalar epsfcn = 0.)
+      : Functor(),
+        rel_tol(mode == Central ? numext::cbrt(numext::maxi(epsfcn, NumTraits<Scalar>::epsilon()))
+                                : numext::sqrt(numext::maxi(epsfcn, NumTraits<Scalar>::epsilon()))) {
+    setAbsTol(rel_tol);
+  }
+  NumericalDiff(const Functor& f, Scalar epsfcn = 0.)
+      : Functor(f),
+        rel_tol(mode == Central ? numext::cbrt(numext::maxi(epsfcn, NumTraits<Scalar>::epsilon()))
+                                : numext::sqrt(numext::maxi(epsfcn, NumTraits<Scalar>::epsilon()))) {
+    setAbsTol(rel_tol);
+  }
 
   // forward constructors
-  template <typename T0>
-  NumericalDiff(const T0& a0) : Functor(a0), epsfcn(0) {}
-  template <typename T0, typename T1>
-  NumericalDiff(const T0& a0, const T1& a1) : Functor(a0, a1), epsfcn(0) {}
-  template <typename T0, typename T1, typename T2>
-  NumericalDiff(const T0& a0, const T1& a1, const T2& a2) : Functor(a0, a1, a2), epsfcn(0) {}
+  template <typename T0, std::enable_if_t<std::is_constructible<Functor, const T0&>::value, int> = 0>
+  NumericalDiff(const T0& a0) : Functor(a0), rel_tol(defaultRelTol()), abs_tol(defaultRelTol()) {}
+
+  template <typename T0, typename T1,
+            std::enable_if_t<std::is_constructible<Functor, const T0&, const T1&>::value, int> = 0>
+  NumericalDiff(const T0& a0, const T1& a1) : Functor(a0, a1), rel_tol(defaultRelTol()), abs_tol(defaultRelTol()) {}
+
+  template <typename T0, typename T1, typename T2,
+            std::enable_if_t<std::is_constructible<Functor, const T0&, const T1&, const T2&>::value, int> = 0>
+  NumericalDiff(const T0& a0, const T1& a1, const T2& a2)
+      : Functor(a0, a1, a2), rel_tol(defaultRelTol()), abs_tol(defaultRelTol()) {}
+
+  // Setters/getters for tolerances
+  void setRelTol(Scalar _rel_tol) {
+    eigen_assert(_rel_tol >= NumTraits<Scalar>::epsilon());
+    rel_tol = _rel_tol;
+  }
+  void setAbsTol(Scalar _abs_tol) {
+    eigen_assert(_abs_tol > 0);
+    abs_tol = _abs_tol;
+  }
+  Scalar relTol() const { return rel_tol; }
+  Scalar absTol() const { return abs_tol; }
 
   enum { InputsAtCompileTime = Functor::InputsAtCompileTime, ValuesAtCompileTime = Functor::ValuesAtCompileTime };
 
   /**
    * Computes the Jacobian of the functor at \a _x into \a jac and returns the number of functor evaluations.
    *
-   * The step along coordinate \c j is <tt>h = eps * max(|x[j]|, 1)</tt> with <tt>eps = sqrt(max(epsfcn, epsilon))</tt>
-   * and \c epsilon the machine precision NumTraits<Scalar>::epsilon(); the difference quotient divides by the
-   * representable step <tt>fl(x[j] + h) - x[j]</tt> actually applied.
+   * The step along coordinate \c j is <tt>h = max(|x[j]| * rel_tol, abs_tol)</tt> with rel_tol and abs_tol selected by
+   * the user. The rel_tol selected by the user must be greater than or equal to epsilon, and abs_tol
+   * must be greater than 0, where \c epsilon is the machine precision NumTraits<Scalar>::epsilon(); the
+   * difference quotient divides by the representable step <tt>fl(x[j] + h) - x[j]</tt> actually applied.
+   *
+   * In forward mode, the default rel_tol is <tt> sqrt(NumTraits<Scalar>::epsilon()) </tt> and the default abs_tol
+   * is abs_tol = rel_tol. In central mode, the default rel_tol is <tt> cbrt(NumTraits<Scalar>::epsilon()) </tt>
+   * and the default abs_tol = rel_tol.
    */
   int df(const InputType& _x, JacobianType& jac) const {
     using std::abs;
-    using std::sqrt;
     /* Local variables */
     Scalar h;
     int nfev = 0;
     const typename InputType::Index n = _x.size();
-    const Scalar eps = sqrt(((std::max)(epsfcn, NumTraits<Scalar>::epsilon())));
     ValueType val1, val2;
     InputType x = _x;
     // TODO: We should do this only if the size is not already known.
@@ -107,9 +144,9 @@
     // Function Body
     for (int j = 0; j < n; ++j) {
       const Scalar x_abs = abs(x[j]);
-      h = numext::maxi(x_abs, Scalar(1)) * eps;
+      h = numext::maxi(x_abs * rel_tol, abs_tol);
       // The functor is evaluated at fl(x[j] + h), so divide by that representable step: the rounding
-      // of x[j] + h perturbs h by up to ulp(x[j]) <= epsilon/eps * h <= sqrt(epsilon) * h, comparable
+      // of x[j] + h perturbs h by up to ulp(x[j]) <= epsilon |x[j]| <= (epsilon/rel_tol) * h <= h, comparable
       // to the error of the difference quotient itself.
       Scalar x_plus = _x[j] + h;
       internal::numerical_diff_barrier(x_plus);
@@ -145,7 +182,8 @@
   }
 
  private:
-  Scalar epsfcn;
+  Scalar rel_tol;
+  Scalar abs_tol;
 
   NumericalDiff& operator=(const NumericalDiff&) = delete;
 };
diff --git a/contrib/test/NumericalDiff.cpp b/contrib/test/NumericalDiff.cpp
index f985d4d..06b8af6 100644
--- a/contrib/test/NumericalDiff.cpp
+++ b/contrib/test/NumericalDiff.cpp
@@ -70,6 +70,31 @@
   VectorXd scale;
 };
 
+// When used with forward or central differences about x=1000, this will just return the relative step size used *
+// 1000.0.
+struct step_size_functor : Functor<double> {
+  step_size_functor() : Functor<double>(1, 1) {}
+  int operator()(const VectorXd &x, VectorXd &fvec) const {
+    fvec[0] = (x[0] - 1000.0) * (x[0] - 1000.0);
+    if (x[0] < 1000.0) {
+      fvec[0] *= -1.0;
+    }
+    return 0;
+  }
+};
+
+// When used with forward or central difference about x=0, this will return the absolute step size used.
+struct step_size_functor_abs : Functor<double> {
+  step_size_functor_abs() : Functor<double>(1, 1) {}
+  int operator()(const VectorXd &x, VectorXd &fvec) const {
+    fvec[0] = x[0] * x[0];
+    if (x[0] < 0.0) {
+      fvec[0] *= -1.0;
+    }
+    return 0;
+  }
+};
+
 void test_forward() {
   VectorXd x(3);
   MatrixXd jac(15, 3);
@@ -106,6 +131,10 @@
   numDiff.df(x, jac);
 
   VERIFY_IS_APPROX(jac, actual_jac);
+
+  // With the default changed to cbrt(epsilon), the maximum error has become lower
+  // This verifies we get the new, lower maximum error.
+  VERIFY_LE((jac - actual_jac).cwiseAbs().maxCoeff(), 1e-10);
 }
 
 void test_small() {
@@ -142,10 +171,162 @@
   VERIFY_IS_EQUAL(jac, expected);
 }
 
+void test_step_size() {
+  step_size_functor functor;
+  step_size_functor_abs functor_abs;
+  VectorXd x(1);
+  MatrixXd jac(1, 1);
+  MatrixXd expected(1, 1);
+
+  // Test that setting relative step size and absolute step sizes gives the expected answer
+  NumericalDiff<step_size_functor> numDiffBothSet(functor);
+  numDiffBothSet.setRelTol(1e-4);
+  numDiffBothSet.setAbsTol(1e-6);
+  x << 1000.0;
+  numDiffBothSet.df(x, jac);
+  expected << 1e-1;  // rel_tol * 1000.0
+  VERIFY_IS_APPROX(jac, expected);
+
+  NumericalDiff<step_size_functor_abs> numDiffBothSetAbs(functor_abs);
+  numDiffBothSetAbs.setRelTol(1e-4);
+  numDiffBothSetAbs.setAbsTol(1e-6);
+  x << 0.0;
+  numDiffBothSetAbs.df(x, jac);
+  expected << 1e-6;  // abs_tol
+  VERIFY_IS_APPROX(jac, expected);
+
+  // Test that setting relative step size only gives abs_tol = rel_tol
+  NumericalDiff<step_size_functor> numDiffRelOnly(functor, 1e-8);
+  x << 1000.0;
+  expected << 1e-1;  // rel_tol * 1000.0
+  numDiffRelOnly.df(x, jac);
+  VERIFY_IS_APPROX(jac, expected);
+
+  NumericalDiff<step_size_functor_abs> numDiffRelOnlyAbs(functor_abs, 1e-8);
+  x << 0.0;
+  numDiffRelOnlyAbs.df(x, jac);
+  expected << 1e-4;  // abs_tol = rel_tol
+  VERIFY_IS_APPROX(jac, expected);
+
+  // Test that setting neither gives rel_tol = abs_tol = sqrt(eps)
+  NumericalDiff<step_size_functor> numDiffDefaults(functor);
+  x << 1000.0;
+  expected << 1000.0 * std::sqrt(std::numeric_limits<double>::epsilon());  // 1000.0 * rel_tol
+  numDiffDefaults.df(x, jac);
+  VERIFY_IS_APPROX(jac, expected);
+
+  NumericalDiff<step_size_functor_abs> numDiffDefaultsAbs(functor_abs);
+  x << 0.0;
+  numDiffDefaultsAbs.df(x, jac);
+  expected << std::sqrt(std::numeric_limits<double>::epsilon());  // abs_tol = rel_tol
+  VERIFY_IS_APPROX(jac, expected);
+
+  // Run the same tests, but for central differences
+
+  // Test that setting relative step size and absolute step sizes gives the expected answer
+  NumericalDiff<step_size_functor, Central> numDiffCentralBothSet(functor);
+  numDiffCentralBothSet.setRelTol(1e-4);
+  numDiffCentralBothSet.setAbsTol(1e-6);
+  x << 1000.0;
+  numDiffCentralBothSet.df(x, jac);
+  expected << 1e-1;  // rel_tol * 1000.0
+  VERIFY_IS_APPROX(jac, expected);
+
+  NumericalDiff<step_size_functor_abs, Central> numDiffCentralBothSetAbs(functor_abs);
+  numDiffCentralBothSetAbs.setRelTol(1e-4);
+  numDiffCentralBothSetAbs.setAbsTol(1e-6);
+  x << 0.0;
+  numDiffCentralBothSetAbs.df(x, jac);
+  expected << 1e-6;  // abs_tol
+  VERIFY_IS_APPROX(jac, expected);
+
+  // Test that setting relative step size only gives abs_tol = rel_tol
+  NumericalDiff<step_size_functor, Central> numDiffCentralRelOnly(functor, 1e-12);
+  x << 1000.0;
+  expected << 1e-1;  // rel_tol * 1000.0
+  numDiffCentralRelOnly.df(x, jac);
+  VERIFY_IS_APPROX(jac, expected);
+
+  NumericalDiff<step_size_functor_abs, Central> numDiffCentralRelOnlyAbs(functor_abs, 1e-12);
+  x << 0.0;
+  numDiffCentralRelOnlyAbs.df(x, jac);
+  expected << 1e-4;  // abs_tol = rel_tol
+  VERIFY_IS_APPROX(jac, expected);
+
+  // Test that setting neither gives rel_tol = abs_tol = cbrt(eps)
+  NumericalDiff<step_size_functor, Central> numDiffCentralDefaults(functor);
+  x << 1000.0;
+  expected << 1000.0 * std::cbrt(std::numeric_limits<double>::epsilon());  // 1000.0 * rel_tol
+  numDiffCentralDefaults.df(x, jac);
+  VERIFY_IS_APPROX(jac, expected);
+
+  NumericalDiff<step_size_functor_abs, Central> numDiffCentralDefaultsAbs(functor_abs);
+  x << 0.0;
+  numDiffCentralDefaultsAbs.df(x, jac);
+  expected << std::cbrt(std::numeric_limits<double>::epsilon());  // abs_tol = rel_tol
+  VERIFY_IS_APPROX(jac, expected);
+}
+
+// Test functor to ensure constructors work. The operator returns a1 at
+// x = 0.0 and 0.0 otherwise.
+struct G : Functor<double> {
+  G() : Functor<double>(1, 1) {}
+  G(double _a0, double _a1) : Functor<double>(1, 1), a0(_a0), a1(_a1) {}
+  int operator()(const VectorXd &x, VectorXd &fvec) const {
+    if (x[0] == 0.0) {
+      fvec[0] = a1;
+    } else {
+      fvec[0] = 0.0;
+    }
+    return 0;
+  }
+  double a0 = 0.0;
+  double a1 = 0.0;
+};
+
+struct float_functor : Functor<float> {
+  float_functor() : Functor<float>(1, 1) {}
+  int operator()(const VectorXf &x, VectorXf &fvec) const {
+    if (x[0] == 0.0f) {
+      fvec[0] = a0;
+    } else {
+      fvec[0] = 0.0;
+    }
+    return 0;
+  }
+  float a0 = 5.0;
+};
+
+void test_constructors() {
+  VectorXd x(1);
+  MatrixXd jac(1, 1);
+  MatrixXd expected(1, 1);
+  x << -1.0;
+
+  NumericalDiff<G> g(2.0, 3.0);
+  g.setRelTol(1.0);
+  g.df(x, jac);
+  expected << 3.0;  // Should be a1 if the constructor worked
+  VERIFY_IS_APPROX(jac, expected);
+
+  VectorXf fx(1);
+  MatrixXf fjac(1, 1);
+  MatrixXf fexpected(1, 1);
+  float_functor ff;
+  fx << -1.0;
+
+  NumericalDiff<float_functor> nd(ff, 1.0);
+  nd.df(fx, fjac);
+  fexpected << 5.0;  // Should be 5.0
+  VERIFY_IS_APPROX(fjac, fexpected);
+}
+
 EIGEN_DECLARE_TEST(NumericalDiff) {
   CALL_SUBTEST(test_forward());
   CALL_SUBTEST(test_central());
   CALL_SUBTEST(test_small());
+  CALL_SUBTEST(test_step_size());
+  CALL_SUBTEST(test_constructors());
   CALL_SUBTEST(test_step_alignment<Forward>(0.0));
   CALL_SUBTEST(test_step_alignment<Central>(0.0));
   // A non-dyadic eps also misaligns the absolute step taken for |x| < 1.