Implement some C++11 math functions for Jet

Change-Id: Ie164aeaac980cd2fe48a86230e1948c9fd75424a

include/ceres/jet.h

@@ -446,6 +446,16 @@ inline double ceil    (double x) { return std::ceil(x);     }
 inline double pow  (double x, double y) { return std::pow(x, y);   }
 inline double atan2(double y, double x) { return std::atan2(y, x); }
 
+#ifdef CERES_USE_CXX11
+// Some new additions to C++11:
+inline double cbrt  (double x) { return std::cbrt(x);  }
+inline double exp2  (double x) { return std::exp2(x);  }
+inline double log2  (double x) { return std::log2(x);  }
+inline double hypot(double x, double y) { return std::hypot(x, y); }
+inline double fmax(double  x, double y) { return std::fmax(x, y);  }
+inline double fmin(double  x, double y) { return std::fmin(x, y);  }
+#endif
+
 // In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule.
 
 // abs(x + h) ~= x + h or -(x + h)
@@ -555,6 +565,57 @@ Jet<T, N> ceil(const Jet<T, N>& f) {
   return Jet<T, N>(ceil(f.a));
 }
 
+#ifdef CERES_USE_CXX11
+// Some new additions to C++11:
+
+// cbrt(a + h) ~= cbrt(a) + h / (3 a ^ (2/3))
+template <typename T, int N> inline
+Jet<T, N> cbrt(const Jet<T, N>& f) {
+  const T derivative = T(1.0) / (T(3.0) * cbrt(f.a * f.a));
+  return Jet<T, N>(cbrt(f.a), f.v * derivative);
+}
+
+// exp2(x + h) = 2^(x+h) ~= 2^x + h*2^x*log(2)
+template <typename T, int N> inline
+Jet<T, N> exp2(const Jet<T, N>& f) {
+  const T tmp = exp2(f.a);
+  const T derivative = tmp * log(T(2));
+  return Jet<T, N>(tmp, f.v * derivative);
+}
+
+// log2(x + h) ~= log2(x) + h / (x * log(2))
+template <typename T, int N> inline
+Jet<T, N> log2(const Jet<T, N>& f) {
+  const T derivative = T(1.0) / (f.a * log(T(2)));
+  return Jet<T, N>(log2(f.a), f.v * derivative);
+}
+
+// Like sqrt(x^2 + y^2),
+// but acts to prevent underflow/overflow for small/large x/y.
+// Note that the function is non-smooth at x=y=0,
+// so the derivative is undefined there.
+template <typename T, int N> inline
+Jet<T, N> hypot(const Jet<T, N>& x, const Jet<T, N>& y) {
+  // d/da sqrt(a) = 0.5 / sqrt(a)
+  // d/dx x^2 + y^2 = 2x
+  // So by the chain rule:
+  // d/dx sqrt(x^2 + y^2) = 0.5 / sqrt(x^2 + y^2) * 2x = x / sqrt(x^2 + y^2)
+  // d/dy sqrt(x^2 + y^2) = y / sqrt(x^2 + y^2)
+  const T tmp = hypot(x.a, y.a);
+  return Jet<T, N>(tmp, x.a / tmp * x.v + y.a / tmp * y.v);
+}
+
+template <typename T, int N> inline
+const Jet<T, N>& fmax(const Jet<T, N>& x, const Jet<T, N>& y) {
+  return x < y ? y : x;
+}
+
+template <typename T, int N> inline
+const Jet<T, N>& fmin(const Jet<T, N>& x, const Jet<T, N>& y) {
+  return y < x ? y : x;
+}
+#endif // CERES_USE_CXX11
+
 // Bessel functions of the first kind with integer order equal to 0, 1, n.
 //
 // Microsoft has deprecated the j[0,1,n]() POSIX Bessel functions in favour of

internal/ceres/jet_test.cc

@@ -66,6 +66,46 @@ void ExpectJetsClose(const J &x, const J &y) {
   ExpectClose(x.v[1], y.v[1], kTolerance);
 }
 
+const double kStep = 1e-8;
+const double kNumericalTolerance = 1e-6; // Numeric derivation is quite inexact
+
+// Differentiate using Jet and confirm results with numerical derivation.
+template<typename Function>
+void NumericalTest(const char* name, const Function& f, const double x) {
+  const double exact_dx = f(MakeJet(x, 1.0, 0.0)).v[0];
+  const double estimated_dx =
+    (f(J(x + kStep)).a - f(J(x - kStep)).a) / (2.0 * kStep);
+  VL << name << "(" << x << "), exact dx: "
+     << exact_dx << ", estimated dx: " << estimated_dx;
+  ExpectClose(exact_dx, estimated_dx, kNumericalTolerance);
+}
+
+// Same as NumericalTest, but given a function taking two arguments.
+template<typename Function>
+void NumericalTest2(const char* name, const Function& f,
+                    const double x, const double y) {
+  const J exact_delta = f(MakeJet(x, 1.0, 0.0), MakeJet(y, 0.0, 1.0));
+  const double exact_dx = exact_delta.v[0];
+  const double exact_dy = exact_delta.v[1];
+
+  // Sanity check – these should be equivalent:
+  EXPECT_EQ(exact_dx, f(MakeJet(x, 1.0, 0.0), MakeJet(y, 0.0, 0.0)).v[0]);
+  EXPECT_EQ(exact_dx, f(MakeJet(x, 0.0, 1.0), MakeJet(y, 0.0, 0.0)).v[1]);
+  EXPECT_EQ(exact_dy, f(MakeJet(x, 0.0, 0.0), MakeJet(y, 1.0, 0.0)).v[0]);
+  EXPECT_EQ(exact_dy, f(MakeJet(x, 0.0, 0.0), MakeJet(y, 0.0, 1.0)).v[1]);
+
+  const double estimated_dx =
+    (f(J(x + kStep), J(y)).a - f(J(x - kStep), J(y)).a) / (2.0 * kStep);
+  const double estimated_dy =
+    (f(J(x), J(y + kStep)).a - f(J(x), J(y - kStep)).a) / (2.0 * kStep);
+  VL << name << "(" << x << ", " << y << "), exact dx: "
+     << exact_dx << ", estimated dx: " << estimated_dx;
+  ExpectClose(exact_dx, estimated_dx, kNumericalTolerance);
+  VL << name << "(" << x << ", " << y << "), exact dy: "
+     << exact_dy << ", estimated dy: " << estimated_dy;
+  ExpectClose(exact_dy, estimated_dy, kNumericalTolerance);
+}
+
 TEST(Jet, Jet) {
   // Pick arbitrary values for x and y.
   J x = MakeJet(2.3, -2.7, 1e-3);
@@ -106,6 +146,9 @@ TEST(Jet, Jet) {
     ExpectJetsClose(w, y);
   }
 
+  NumericalTest("sqrt", sqrt<double, 2>, 0.00001);
+  NumericalTest("sqrt", sqrt<double, 2>, 1.0);
+
   { // Check that cos(2*x) = cos(x)^2 - sin(x)^2
     J z = cos(J(2.0) * x);
     J w = cos(x)*cos(x) - sin(x)*sin(x);
@@ -523,6 +566,137 @@ TEST(Jet, Jet) {
     J expected = MakeJet(ceil(a.a), 0.0, 0.0);
     EXPECT_EQ(expected, b);
   }
+
+  #ifdef CERES_USE_CXX11
+  { // Check that cbrt(x * x * x) == x.
+    J z = x * x * x;
+    J w = cbrt(z);
+    VL << "z = " << z;
+    VL << "w = " << w;
+    ExpectJetsClose(w, x);
+  }
+
+  { // Check that cbrt(y) * cbrt(y) * cbrt(y) == y.
+    J z = cbrt(y);
+    J w = z * z * z;
+    VL << "z = " << z;
+    VL << "w = " << w;
+    ExpectJetsClose(w, y);
+  }
+
+  { // Check that cbrt(x) == pow(x, 1/3).
+    J z = cbrt(x);
+    J w = pow(x, 1.0 / 3.0);
+    VL << "z = " << z;
+    VL << "w = " << w;
+    ExpectJetsClose(z, w);
+  }
+  NumericalTest("cbrt", cbrt<double, 2>, -1.0);
+  NumericalTest("cbrt", cbrt<double, 2>, -1e-5);
+  NumericalTest("cbrt", cbrt<double, 2>, 1e-5);
+  NumericalTest("cbrt", cbrt<double, 2>, 1.0);
+
+  { // Check that exp2(x) == exp(x * log(2))
+    J z = exp2(x);
+    J w = exp(x * log(2.0));
+    VL << "z = " << z;
+    VL << "w = " << w;
+    ExpectJetsClose(z, w);
+  }
+  NumericalTest("exp2", exp2<double, 2>, -1.0);
+  NumericalTest("exp2", exp2<double, 2>, -1e-5);
+  NumericalTest("exp2", exp2<double, 2>, -1e-200);
+  NumericalTest("exp2", exp2<double, 2>, 0.0);
+  NumericalTest("exp2", exp2<double, 2>, 1e-200);
+  NumericalTest("exp2", exp2<double, 2>, 1e-5);
+  NumericalTest("exp2", exp2<double, 2>, 1.0);
+
+  { // Check that log2(x) == log(x) / log(2)
+    J z = log2(x);
+    J w = log(x) / log(2.0);
+    VL << "z = " << z;
+    VL << "w = " << w;
+    ExpectJetsClose(z, w);
+  }
+  NumericalTest("log2", log2<double, 2>, 1e-5);
+  NumericalTest("log2", log2<double, 2>, 1.0);
+  NumericalTest("log2", log2<double, 2>, 100.0);
+
+  { // Check that hypot(x, y) == sqrt(x^2 + y^2)
+    J h = hypot(x, y);
+    J s = sqrt(x*x + y*y);
+    VL << "h = " << h;
+    VL << "s = " << s;
+    ExpectJetsClose(h, s);
+  }
+
+  { // Check that hypot(x, x) == sqrt(2) * abs(x)
+    J h = hypot(x, x);
+    J s = sqrt(2.0) * abs(x);
+    VL << "h = " << h;
+    VL << "s = " << s;
+    ExpectJetsClose(h, s);
+  }
+
+  { // Check that the derivative is zero tangentially to the circle:
+    J h = hypot(MakeJet(2.0, 1.0, 1.0), MakeJet(2.0, 1.0, -1.0));
+    VL << "h = " << h;
+    ExpectJetsClose(h, MakeJet(sqrt(8.0), std::sqrt(2.0), 0.0));
+  }
+
+  { // Check that hypot(x, 0) == x
+    J zero = MakeJet(0.0, 2.0, 3.14);
+    J h = hypot(x, zero);
+    VL << "h = " << h;
+    ExpectJetsClose(x, h);
+  }
+
+  { // Check that hypot(0, y) == y
+    J zero = MakeJet(0.0, 2.0, 3.14);
+    J h = hypot(zero, y);
+    VL << "h = " << h;
+    ExpectJetsClose(y, h);
+  }
+
+  { // Check that hypot(x, 0) == sqrt(x * x) == x, even when x * x underflows:
+    EXPECT_EQ(DBL_MIN * DBL_MIN, 0.0); // Make sure it underflows
+    J huge = MakeJet(DBL_MIN, 2.0, 3.14);
+    J h = hypot(huge, J(0.0));
+    VL << "h = " << h;
+    ExpectJetsClose(h, huge);
+  }
+
+  { // Check that hypot(x, 0) == sqrt(x * x) == x, even when x * x overflows:
+    EXPECT_EQ(DBL_MAX * DBL_MAX, std::numeric_limits<double>::infinity());
+    J huge = MakeJet(DBL_MAX, 2.0, 3.14);
+    J h = hypot(huge, J(0.0));
+    VL << "h = " << h;
+    ExpectJetsClose(h, huge);
+  }
+
+  NumericalTest2("hypot", hypot<double, 2>,  0.0,   1e-5);
+  NumericalTest2("hypot", hypot<double, 2>, -1e-5,  0.0);
+  NumericalTest2("hypot", hypot<double, 2>,  1e-5,  1e-5);
+  NumericalTest2("hypot", hypot<double, 2>,  0.0,   1.0);
+  NumericalTest2("hypot", hypot<double, 2>,  1e-3,  1.0);
+  NumericalTest2("hypot", hypot<double, 2>,  1e-3, -1.0);
+  NumericalTest2("hypot", hypot<double, 2>, -1e-3,  1.0);
+  NumericalTest2("hypot", hypot<double, 2>, -1e-3, -1.0);
+  NumericalTest2("hypot", hypot<double, 2>,  1.0,   2.0);
+
+  {
+    J z = fmax(x, y);
+    VL << "z = " << z;
+    ExpectJetsClose(x, z);
+  }
+
+  {
+    J z = fmin(x, y);
+    VL << "z = " << z;
+    ExpectJetsClose(y, z);
+  }
+
+  #endif
 }
 
 TEST(Jet, JetsInEigenMatrices) {