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) {