Implement some C++11 math functions for Jet
Change-Id: Ie164aeaac980cd2fe48a86230e1948c9fd75424a
diff --git a/include/ceres/jet.h b/include/ceres/jet.h
index b711ea2..7d28e37 100644
--- a/include/ceres/jet.h
+++ b/include/ceres/jet.h
@@ -446,6 +446,16 @@
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 @@
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
diff --git a/internal/ceres/jet_test.cc b/internal/ceres/jet_test.cc
index 16d37c6..f3e42fd 100644
--- a/internal/ceres/jet_test.cc
+++ b/internal/ceres/jet_test.cc
@@ -66,6 +66,46 @@
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 @@
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 @@
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) {
