diff options
Diffstat (limited to 'extern/ceres/include/ceres/jet.h')
| -rw-r--r-- | extern/ceres/include/ceres/jet.h | 693 |
1 files changed, 415 insertions, 278 deletions
diff --git a/extern/ceres/include/ceres/jet.h b/extern/ceres/include/ceres/jet.h index a104707298c..7aafaa01d30 100644 --- a/extern/ceres/include/ceres/jet.h +++ b/extern/ceres/include/ceres/jet.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2019 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -31,7 +31,7 @@ // A simple implementation of N-dimensional dual numbers, for automatically // computing exact derivatives of functions. // -// While a complete treatment of the mechanics of automatic differentation is +// While a complete treatment of the mechanics of automatic differentiation is // beyond the scope of this header (see // http://en.wikipedia.org/wiki/Automatic_differentiation for details), the // basic idea is to extend normal arithmetic with an extra element, "e," often @@ -49,7 +49,7 @@ // f(x) = x^2 , // // evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20. -// Next, augument 10 with an infinitesimal to get: +// Next, argument 10 with an infinitesimal to get: // // f(10 + e) = (10 + e)^2 // = 100 + 2 * 10 * e + e^2 @@ -102,8 +102,9 @@ // } // // // The "2" means there should be 2 dual number components. -// Jet<double, 2> x(0); // Pick the 0th dual number for x. -// Jet<double, 2> y(1); // Pick the 1st dual number for y. +// // It computes the partial derivative at x=10, y=20. +// Jet<double, 2> x(10, 0); // Pick the 0th dual number for x. +// Jet<double, 2> y(20, 1); // Pick the 1st dual number for y. // Jet<double, 2> z = f(x, y); // // LOG(INFO) << "df/dx = " << z.v[0] @@ -124,7 +125,7 @@ // // x = a + \sum_i v[i] t_i // -// A shorthand is to write an element as x = a + u, where u is the pertubation. +// A shorthand is to write an element as x = a + u, where u is the perturbation. // Then, the main point about the arithmetic of jets is that the product of // perturbations is zero: // @@ -163,7 +164,6 @@ #include <string> #include "Eigen/Core" -#include "ceres/fpclassify.h" #include "ceres/internal/port.h" namespace ceres { @@ -171,26 +171,25 @@ namespace ceres { template <typename T, int N> struct Jet { enum { DIMENSION = N }; + typedef T Scalar; // Default-construct "a" because otherwise this can lead to false errors about // uninitialized uses when other classes relying on default constructed T // (where T is a Jet<T, N>). This usually only happens in opt mode. Note that // the C++ standard mandates that e.g. default constructed doubles are // initialized to 0.0; see sections 8.5 of the C++03 standard. - Jet() : a() { - v.setZero(); - } + Jet() : a() { v.setConstant(Scalar()); } // Constructor from scalar: a + 0. explicit Jet(const T& value) { a = value; - v.setZero(); + v.setConstant(Scalar()); } // Constructor from scalar plus variable: a + t_i. Jet(const T& value, int k) { a = value; - v.setZero(); + v.setConstant(Scalar()); v[k] = T(1.0); } @@ -198,58 +197,66 @@ struct Jet { // The use of Eigen::DenseBase allows Eigen expressions // to be passed in without being fully evaluated until // they are assigned to v - template<typename Derived> - EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase<Derived> &v) - : a(a), v(v) { - } + template <typename Derived> + EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase<Derived>& v) + : a(a), v(v) {} // Compound operators - Jet<T, N>& operator+=(const Jet<T, N> &y) { + Jet<T, N>& operator+=(const Jet<T, N>& y) { *this = *this + y; return *this; } - Jet<T, N>& operator-=(const Jet<T, N> &y) { + Jet<T, N>& operator-=(const Jet<T, N>& y) { *this = *this - y; return *this; } - Jet<T, N>& operator*=(const Jet<T, N> &y) { + Jet<T, N>& operator*=(const Jet<T, N>& y) { *this = *this * y; return *this; } - Jet<T, N>& operator/=(const Jet<T, N> &y) { + Jet<T, N>& operator/=(const Jet<T, N>& y) { *this = *this / y; return *this; } + // Compound with scalar operators. + Jet<T, N>& operator+=(const T& s) { + *this = *this + s; + return *this; + } + + Jet<T, N>& operator-=(const T& s) { + *this = *this - s; + return *this; + } + + Jet<T, N>& operator*=(const T& s) { + *this = *this * s; + return *this; + } + + Jet<T, N>& operator/=(const T& s) { + *this = *this / s; + return *this; + } + // The scalar part. T a; // The infinitesimal part. + Eigen::Matrix<T, N, 1> v; - // We allocate Jets on the stack and other places they - // might not be aligned to 16-byte boundaries. If we have C++11, we - // can specify their alignment anyway, and thus can safely enable - // vectorization on those matrices; in C++99, we are out of luck. Figure out - // what case we're in and do the right thing. -#ifndef CERES_USE_CXX11 - // fall back to safe version: - Eigen::Matrix<T, N, 1, Eigen::DontAlign> v; -#else - static constexpr bool kShouldAlignMatrix = - 16 <= ::ceres::port_constants::kMaxAlignBytes; - static constexpr int kAlignHint = kShouldAlignMatrix ? - Eigen::AutoAlign : Eigen::DontAlign; - static constexpr size_t kAlignment = kShouldAlignMatrix ? 16 : 1; - alignas(kAlignment) Eigen::Matrix<T, N, 1, kAlignHint> v; -#endif + // This struct needs to have an Eigen aligned operator new as it contains + // fixed-size Eigen types. + EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // Unary + -template<typename T, int N> inline -Jet<T, N> const& operator+(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> const& operator+(const Jet<T, N>& f) { return f; } @@ -257,72 +264,68 @@ Jet<T, N> const& operator+(const Jet<T, N>& f) { // see if it causes a performance increase. // Unary - -template<typename T, int N> inline -Jet<T, N> operator-(const Jet<T, N>&f) { +template <typename T, int N> +inline Jet<T, N> operator-(const Jet<T, N>& f) { return Jet<T, N>(-f.a, -f.v); } // Binary + -template<typename T, int N> inline -Jet<T, N> operator+(const Jet<T, N>& f, - const Jet<T, N>& g) { +template <typename T, int N> +inline Jet<T, N> operator+(const Jet<T, N>& f, const Jet<T, N>& g) { return Jet<T, N>(f.a + g.a, f.v + g.v); } // Binary + with a scalar: x + s -template<typename T, int N> inline -Jet<T, N> operator+(const Jet<T, N>& f, T s) { +template <typename T, int N> +inline Jet<T, N> operator+(const Jet<T, N>& f, T s) { return Jet<T, N>(f.a + s, f.v); } // Binary + with a scalar: s + x -template<typename T, int N> inline -Jet<T, N> operator+(T s, const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> operator+(T s, const Jet<T, N>& f) { return Jet<T, N>(f.a + s, f.v); } // Binary - -template<typename T, int N> inline -Jet<T, N> operator-(const Jet<T, N>& f, - const Jet<T, N>& g) { +template <typename T, int N> +inline Jet<T, N> operator-(const Jet<T, N>& f, const Jet<T, N>& g) { return Jet<T, N>(f.a - g.a, f.v - g.v); } // Binary - with a scalar: x - s -template<typename T, int N> inline -Jet<T, N> operator-(const Jet<T, N>& f, T s) { +template <typename T, int N> +inline Jet<T, N> operator-(const Jet<T, N>& f, T s) { return Jet<T, N>(f.a - s, f.v); } // Binary - with a scalar: s - x -template<typename T, int N> inline -Jet<T, N> operator-(T s, const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> operator-(T s, const Jet<T, N>& f) { return Jet<T, N>(s - f.a, -f.v); } // Binary * -template<typename T, int N> inline -Jet<T, N> operator*(const Jet<T, N>& f, - const Jet<T, N>& g) { +template <typename T, int N> +inline Jet<T, N> operator*(const Jet<T, N>& f, const Jet<T, N>& g) { return Jet<T, N>(f.a * g.a, f.a * g.v + f.v * g.a); } // Binary * with a scalar: x * s -template<typename T, int N> inline -Jet<T, N> operator*(const Jet<T, N>& f, T s) { +template <typename T, int N> +inline Jet<T, N> operator*(const Jet<T, N>& f, T s) { return Jet<T, N>(f.a * s, f.v * s); } // Binary * with a scalar: s * x -template<typename T, int N> inline -Jet<T, N> operator*(T s, const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> operator*(T s, const Jet<T, N>& f) { return Jet<T, N>(f.a * s, f.v * s); } // Binary / -template<typename T, int N> inline -Jet<T, N> operator/(const Jet<T, N>& f, - const Jet<T, N>& g) { +template <typename T, int N> +inline Jet<T, N> operator/(const Jet<T, N>& f, const Jet<T, N>& g) { // This uses: // // a + u (a + u)(b - v) (a + u)(b - v) @@ -332,43 +335,43 @@ Jet<T, N> operator/(const Jet<T, N>& f, // which holds because v*v = 0. const T g_a_inverse = T(1.0) / g.a; const T f_a_by_g_a = f.a * g_a_inverse; - return Jet<T, N>(f.a * g_a_inverse, (f.v - f_a_by_g_a * g.v) * g_a_inverse); + return Jet<T, N>(f_a_by_g_a, (f.v - f_a_by_g_a * g.v) * g_a_inverse); } // Binary / with a scalar: s / x -template<typename T, int N> inline -Jet<T, N> operator/(T s, const Jet<T, N>& g) { +template <typename T, int N> +inline Jet<T, N> operator/(T s, const Jet<T, N>& g) { const T minus_s_g_a_inverse2 = -s / (g.a * g.a); return Jet<T, N>(s / g.a, g.v * minus_s_g_a_inverse2); } // Binary / with a scalar: x / s -template<typename T, int N> inline -Jet<T, N> operator/(const Jet<T, N>& f, T s) { - const T s_inverse = 1.0 / s; +template <typename T, int N> +inline Jet<T, N> operator/(const Jet<T, N>& f, T s) { + const T s_inverse = T(1.0) / s; return Jet<T, N>(f.a * s_inverse, f.v * s_inverse); } // Binary comparison operators for both scalars and jets. -#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ -template<typename T, int N> inline \ -bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \ - return f.a op g.a; \ -} \ -template<typename T, int N> inline \ -bool operator op(const T& s, const Jet<T, N>& g) { \ - return s op g.a; \ -} \ -template<typename T, int N> inline \ -bool operator op(const Jet<T, N>& f, const T& s) { \ - return f.a op s; \ -} -CERES_DEFINE_JET_COMPARISON_OPERATOR( < ) // NOLINT -CERES_DEFINE_JET_COMPARISON_OPERATOR( <= ) // NOLINT -CERES_DEFINE_JET_COMPARISON_OPERATOR( > ) // NOLINT -CERES_DEFINE_JET_COMPARISON_OPERATOR( >= ) // NOLINT -CERES_DEFINE_JET_COMPARISON_OPERATOR( == ) // NOLINT -CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT +#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ + template <typename T, int N> \ + inline bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \ + return f.a op g.a; \ + } \ + template <typename T, int N> \ + inline bool operator op(const T& s, const Jet<T, N>& g) { \ + return s op g.a; \ + } \ + template <typename T, int N> \ + inline bool operator op(const Jet<T, N>& f, const T& s) { \ + return f.a op s; \ + } +CERES_DEFINE_JET_COMPARISON_OPERATOR(<) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(<=) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(>) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(>=) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(==) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR(!=) // NOLINT #undef CERES_DEFINE_JET_COMPARISON_OPERATOR // Pull some functions from namespace std. @@ -376,112 +379,128 @@ CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT // This is necessary because we want to use the same name (e.g. 'sqrt') for // double-valued and Jet-valued functions, but we are not allowed to put // Jet-valued functions inside namespace std. -// -// TODO(keir): Switch to "using". -inline double abs (double x) { return std::abs(x); } -inline double log (double x) { return std::log(x); } -inline double exp (double x) { return std::exp(x); } -inline double sqrt (double x) { return std::sqrt(x); } -inline double cos (double x) { return std::cos(x); } -inline double acos (double x) { return std::acos(x); } -inline double sin (double x) { return std::sin(x); } -inline double asin (double x) { return std::asin(x); } -inline double tan (double x) { return std::tan(x); } -inline double atan (double x) { return std::atan(x); } -inline double sinh (double x) { return std::sinh(x); } -inline double cosh (double x) { return std::cosh(x); } -inline double tanh (double x) { return std::tanh(x); } -inline double floor (double x) { return std::floor(x); } -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); } +using std::abs; +using std::acos; +using std::asin; +using std::atan; +using std::atan2; +using std::cbrt; +using std::ceil; +using std::cos; +using std::cosh; +using std::exp; +using std::exp2; +using std::floor; +using std::fmax; +using std::fmin; +using std::hypot; +using std::isfinite; +using std::isinf; +using std::isnan; +using std::isnormal; +using std::log; +using std::log2; +using std::pow; +using std::sin; +using std::sinh; +using std::sqrt; +using std::tan; +using std::tanh; + +// Legacy names from pre-C++11 days. +// clang-format off +inline bool IsFinite(double x) { return std::isfinite(x); } +inline bool IsInfinite(double x) { return std::isinf(x); } +inline bool IsNaN(double x) { return std::isnan(x); } +inline bool IsNormal(double x) { return std::isnormal(x); } +// clang-format on // In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule. // abs(x + h) ~= x + h or -(x + h) -template <typename T, int N> inline -Jet<T, N> abs(const Jet<T, N>& f) { - return f.a < T(0.0) ? -f : f; +template <typename T, int N> +inline Jet<T, N> abs(const Jet<T, N>& f) { + return (f.a < T(0.0) ? -f : f); } // log(a + h) ~= log(a) + h / a -template <typename T, int N> inline -Jet<T, N> log(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> log(const Jet<T, N>& f) { const T a_inverse = T(1.0) / f.a; return Jet<T, N>(log(f.a), f.v * a_inverse); } // exp(a + h) ~= exp(a) + exp(a) h -template <typename T, int N> inline -Jet<T, N> exp(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> exp(const Jet<T, N>& f) { const T tmp = exp(f.a); return Jet<T, N>(tmp, tmp * f.v); } // sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a)) -template <typename T, int N> inline -Jet<T, N> sqrt(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> sqrt(const Jet<T, N>& f) { const T tmp = sqrt(f.a); const T two_a_inverse = T(1.0) / (T(2.0) * tmp); return Jet<T, N>(tmp, f.v * two_a_inverse); } // cos(a + h) ~= cos(a) - sin(a) h -template <typename T, int N> inline -Jet<T, N> cos(const Jet<T, N>& f) { - return Jet<T, N>(cos(f.a), - sin(f.a) * f.v); +template <typename T, int N> +inline Jet<T, N> cos(const Jet<T, N>& f) { + return Jet<T, N>(cos(f.a), -sin(f.a) * f.v); } // acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h -template <typename T, int N> inline -Jet<T, N> acos(const Jet<T, N>& f) { - const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a); +template <typename T, int N> +inline Jet<T, N> acos(const Jet<T, N>& f) { + const T tmp = -T(1.0) / sqrt(T(1.0) - f.a * f.a); return Jet<T, N>(acos(f.a), tmp * f.v); } // sin(a + h) ~= sin(a) + cos(a) h -template <typename T, int N> inline -Jet<T, N> sin(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> sin(const Jet<T, N>& f) { return Jet<T, N>(sin(f.a), cos(f.a) * f.v); } // asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h -template <typename T, int N> inline -Jet<T, N> asin(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> asin(const Jet<T, N>& f) { const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a); return Jet<T, N>(asin(f.a), tmp * f.v); } // tan(a + h) ~= tan(a) + (1 + tan(a)^2) h -template <typename T, int N> inline -Jet<T, N> tan(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> tan(const Jet<T, N>& f) { const T tan_a = tan(f.a); const T tmp = T(1.0) + tan_a * tan_a; return Jet<T, N>(tan_a, tmp * f.v); } // atan(a + h) ~= atan(a) + 1 / (1 + a^2) h -template <typename T, int N> inline -Jet<T, N> atan(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> atan(const Jet<T, N>& f) { const T tmp = T(1.0) / (T(1.0) + f.a * f.a); return Jet<T, N>(atan(f.a), tmp * f.v); } // sinh(a + h) ~= sinh(a) + cosh(a) h -template <typename T, int N> inline -Jet<T, N> sinh(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> sinh(const Jet<T, N>& f) { return Jet<T, N>(sinh(f.a), cosh(f.a) * f.v); } // cosh(a + h) ~= cosh(a) + sinh(a) h -template <typename T, int N> inline -Jet<T, N> cosh(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> cosh(const Jet<T, N>& f) { return Jet<T, N>(cosh(f.a), sinh(f.a) * f.v); } // tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h -template <typename T, int N> inline -Jet<T, N> tanh(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> tanh(const Jet<T, N>& f) { const T tanh_a = tanh(f.a); const T tmp = T(1.0) - tanh_a * tanh_a; return Jet<T, N>(tanh_a, tmp * f.v); @@ -491,8 +510,8 @@ Jet<T, N> tanh(const Jet<T, N>& f) { // result in a zero derivative which provides no information to the solver. // // floor(a + h) ~= floor(a) + 0 -template <typename T, int N> inline -Jet<T, N> floor(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> floor(const Jet<T, N>& f) { return Jet<T, N>(floor(f.a)); } @@ -500,11 +519,60 @@ Jet<T, N> floor(const Jet<T, N>& f) { // result in a zero derivative which provides no information to the solver. // // ceil(a + h) ~= ceil(a) + 0 -template <typename T, int N> inline -Jet<T, N> ceil(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> ceil(const Jet<T, N>& f) { return Jet<T, N>(ceil(f.a)); } +// 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 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 Jet<T, N> fmin(const Jet<T, N>& x, const Jet<T, N>& y) { + return y < x ? y : x; +} + // 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 @@ -512,21 +580,21 @@ Jet<T, N> ceil(const Jet<T, N>& f) { // function errors in client code (the specific warning is suppressed when // Ceres itself is built). inline double BesselJ0(double x) { -#if defined(_MSC_VER) && defined(_j0) +#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS) return _j0(x); #else return j0(x); #endif } inline double BesselJ1(double x) { -#if defined(_MSC_VER) && defined(_j1) +#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS) return _j1(x); #else return j1(x); #endif } inline double BesselJn(int n, double x) { -#if defined(_MSC_VER) && defined(_jn) +#if defined(CERES_MSVC_USE_UNDERSCORE_PREFIXED_BESSEL_FUNCTIONS) return _jn(n, x); #else return jn(n, x); @@ -541,32 +609,32 @@ inline double BesselJn(int n, double x) { // See formula http://dlmf.nist.gov/10.6#E3 // j0(a + h) ~= j0(a) - j1(a) h -template <typename T, int N> inline -Jet<T, N> BesselJ0(const Jet<T, N>& f) { - return Jet<T, N>(BesselJ0(f.a), - -BesselJ1(f.a) * f.v); +template <typename T, int N> +inline Jet<T, N> BesselJ0(const Jet<T, N>& f) { + return Jet<T, N>(BesselJ0(f.a), -BesselJ1(f.a) * f.v); } // See formula http://dlmf.nist.gov/10.6#E1 // j1(a + h) ~= j1(a) + 0.5 ( j0(a) - j2(a) ) h -template <typename T, int N> inline -Jet<T, N> BesselJ1(const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> BesselJ1(const Jet<T, N>& f) { return Jet<T, N>(BesselJ1(f.a), T(0.5) * (BesselJ0(f.a) - BesselJn(2, f.a)) * f.v); } // See formula http://dlmf.nist.gov/10.6#E1 // j_n(a + h) ~= j_n(a) + 0.5 ( j_{n-1}(a) - j_{n+1}(a) ) h -template <typename T, int N> inline -Jet<T, N> BesselJn(int n, const Jet<T, N>& f) { - return Jet<T, N>(BesselJn(n, f.a), - T(0.5) * (BesselJn(n - 1, f.a) - BesselJn(n + 1, f.a)) * f.v); +template <typename T, int N> +inline Jet<T, N> BesselJn(int n, const Jet<T, N>& f) { + return Jet<T, N>( + BesselJn(n, f.a), + T(0.5) * (BesselJn(n - 1, f.a) - BesselJn(n + 1, f.a)) * f.v); } // Jet Classification. It is not clear what the appropriate semantics are for -// these classifications. This picks that IsFinite and isnormal are "all" -// operations, i.e. all elements of the jet must be finite for the jet itself -// to be finite (or normal). For IsNaN and IsInfinite, the answer is less +// these classifications. This picks that std::isfinite and std::isnormal are +// "all" operations, i.e. all elements of the jet must be finite for the jet +// itself to be finite (or normal). For IsNaN and IsInfinite, the answer is less // clear. This takes a "any" approach for IsNaN and IsInfinite such that if any // part of a jet is nan or inf, then the entire jet is nan or inf. This leads // to strange situations like a jet can be both IsInfinite and IsNaN, but in @@ -574,81 +642,88 @@ Jet<T, N> BesselJn(int n, const Jet<T, N>& f) { // derivatives are sane. // The jet is finite if all parts of the jet are finite. -template <typename T, int N> inline -bool IsFinite(const Jet<T, N>& f) { - if (!IsFinite(f.a)) { - return false; - } +template <typename T, int N> +inline bool isfinite(const Jet<T, N>& f) { + // Branchless implementation. This is more efficient for the false-case and + // works with the codegen system. + auto result = isfinite(f.a); for (int i = 0; i < N; ++i) { - if (!IsFinite(f.v[i])) { - return false; - } + result = result & isfinite(f.v[i]); } - return true; + return result; } -// The jet is infinite if any part of the jet is infinite. -template <typename T, int N> inline -bool IsInfinite(const Jet<T, N>& f) { - if (IsInfinite(f.a)) { - return true; - } - for (int i = 0; i < N; i++) { - if (IsInfinite(f.v[i])) { - return true; - } +// The jet is infinite if any part of the Jet is infinite. +template <typename T, int N> +inline bool isinf(const Jet<T, N>& f) { + auto result = isinf(f.a); + for (int i = 0; i < N; ++i) { + result = result | isinf(f.v[i]); } - return false; + return result; } // The jet is NaN if any part of the jet is NaN. -template <typename T, int N> inline -bool IsNaN(const Jet<T, N>& f) { - if (IsNaN(f.a)) { - return true; - } +template <typename T, int N> +inline bool isnan(const Jet<T, N>& f) { + auto result = isnan(f.a); for (int i = 0; i < N; ++i) { - if (IsNaN(f.v[i])) { - return true; - } + result = result | isnan(f.v[i]); } - return false; + return result; } // The jet is normal if all parts of the jet are normal. -template <typename T, int N> inline -bool IsNormal(const Jet<T, N>& f) { - if (!IsNormal(f.a)) { - return false; - } +template <typename T, int N> +inline bool isnormal(const Jet<T, N>& f) { + auto result = isnormal(f.a); for (int i = 0; i < N; ++i) { - if (!IsNormal(f.v[i])) { - return false; - } + result = result & isnormal(f.v[i]); } - return true; + return result; +} + +// Legacy functions from the pre-C++11 days. +template <typename T, int N> +inline bool IsFinite(const Jet<T, N>& f) { + return isfinite(f); +} + +template <typename T, int N> +inline bool IsNaN(const Jet<T, N>& f) { + return isnan(f); +} + +template <typename T, int N> +inline bool IsNormal(const Jet<T, N>& f) { + return isnormal(f); +} + +// The jet is infinite if any part of the jet is infinite. +template <typename T, int N> +inline bool IsInfinite(const Jet<T, N>& f) { + return isinf(f); } // atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2) // // In words: the rate of change of theta is 1/r times the rate of // change of (x, y) in the positive angular direction. -template <typename T, int N> inline -Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) { +template <typename T, int N> +inline Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) { // Note order of arguments: // // f = a + da // g = b + db T const tmp = T(1.0) / (f.a * f.a + g.a * g.a); - return Jet<T, N>(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v)); + return Jet<T, N>(atan2(g.a, f.a), tmp * (-g.a * f.v + f.a * g.v)); } - // pow -- base is a differentiable function, exponent is a constant. // (a+da)^p ~= a^p + p*a^(p-1) da -template <typename T, int N> inline -Jet<T, N> pow(const Jet<T, N>& f, double g) { +template <typename T, int N> +inline Jet<T, N> pow(const Jet<T, N>& f, double g) { T const tmp = g * pow(f.a, g - T(1.0)); return Jet<T, N>(pow(f.a, g), tmp * f.v); } @@ -664,26 +739,30 @@ Jet<T, N> pow(const Jet<T, N>& f, double g) { // 3. For f < 0 and integer g we have: (f)^(g + dg) ~= f^g but if dg // != 0, the derivatives are not defined and we return NaN. -template <typename T, int N> inline -Jet<T, N> pow(double f, const Jet<T, N>& g) { - if (f == 0 && g.a > 0) { +template <typename T, int N> +inline Jet<T, N> pow(T f, const Jet<T, N>& g) { + Jet<T, N> result; + + if (f == T(0) && g.a > T(0)) { // Handle case 2. - return Jet<T, N>(T(0.0)); - } - if (f < 0 && g.a == floor(g.a)) { - // Handle case 3. - Jet<T, N> ret(pow(f, g.a)); - for (int i = 0; i < N; i++) { - if (g.v[i] != T(0.0)) { - // Return a NaN when g.v != 0. - ret.v[i] = std::numeric_limits<T>::quiet_NaN(); + result = Jet<T, N>(T(0.0)); + } else { + if (f < 0 && g.a == floor(g.a)) { // Handle case 3. + result = Jet<T, N>(pow(f, g.a)); + for (int i = 0; i < N; i++) { + if (g.v[i] != T(0.0)) { + // Return a NaN when g.v != 0. + result.v[i] = std::numeric_limits<T>::quiet_NaN(); + } } + } else { + // Handle case 1. + T const tmp = pow(f, g.a); + result = Jet<T, N>(tmp, log(f) * tmp * g.v); } - return ret; } - // Handle case 1. - T const tmp = pow(f, g.a); - return Jet<T, N>(tmp, log(f) * tmp * g.v); + + return result; } // pow -- both base and exponent are differentiable functions. This has a @@ -722,73 +801,48 @@ Jet<T, N> pow(double f, const Jet<T, N>& g) { // // 9. For f < 0, g noninteger: The value and derivatives of f^g are not finite. -template <typename T, int N> inline -Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) { - if (f.a == 0 && g.a >= 1) { +template <typename T, int N> +inline Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) { + Jet<T, N> result; + + if (f.a == T(0) && g.a >= T(1)) { // Handle cases 2 and 3. - if (g.a > 1) { - return Jet<T, N>(T(0.0)); + if (g.a > T(1)) { + result = Jet<T, N>(T(0.0)); + } else { + result = f; } - return f; - } - if (f.a < 0 && g.a == floor(g.a)) { - // Handle cases 7 and 8. - T const tmp = g.a * pow(f.a, g.a - T(1.0)); - Jet<T, N> ret(pow(f.a, g.a), tmp * f.v); - for (int i = 0; i < N; i++) { - if (g.v[i] != T(0.0)) { - // Return a NaN when g.v != 0. - ret.v[i] = std::numeric_limits<T>::quiet_NaN(); + + } else { + if (f.a < T(0) && g.a == floor(g.a)) { + // Handle cases 7 and 8. + T const tmp = g.a * pow(f.a, g.a - T(1.0)); + result = Jet<T, N>(pow(f.a, g.a), tmp * f.v); + for (int i = 0; i < N; i++) { + if (g.v[i] != T(0.0)) { + // Return a NaN when g.v != 0. + result.v[i] = T(std::numeric_limits<double>::quiet_NaN()); + } } + } else { + // Handle the remaining cases. For cases 4,5,6,9 we allow the log() + // function to generate -HUGE_VAL or NaN, since those cases result in a + // nonfinite derivative. + T const tmp1 = pow(f.a, g.a); + T const tmp2 = g.a * pow(f.a, g.a - T(1.0)); + T const tmp3 = tmp1 * log(f.a); + result = Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v); } - return ret; } - // Handle the remaining cases. For cases 4,5,6,9 we allow the log() function - // to generate -HUGE_VAL or NaN, since those cases result in a nonfinite - // derivative. - T const tmp1 = pow(f.a, g.a); - T const tmp2 = g.a * pow(f.a, g.a - T(1.0)); - T const tmp3 = tmp1 * log(f.a); - return Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v); -} - -// Define the helper functions Eigen needs to embed Jet types. -// -// NOTE(keir): machine_epsilon() and precision() are missing, because they don't -// work with nested template types (e.g. where the scalar is itself templated). -// Among other things, this means that decompositions of Jet's does not work, -// for example -// -// Matrix<Jet<T, N> ... > A, x, b; -// ... -// A.solve(b, &x) -// -// does not work and will fail with a strange compiler error. -// -// TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we -// switch to 3.0, also add the rest of the specialization functionality. -template<typename T, int N> inline const Jet<T, N>& ei_conj(const Jet<T, N>& x) { return x; } // NOLINT -template<typename T, int N> inline const Jet<T, N>& ei_real(const Jet<T, N>& x) { return x; } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_imag(const Jet<T, N>& ) { return Jet<T, N>(0.0); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_abs (const Jet<T, N>& x) { return fabs(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_abs2(const Jet<T, N>& x) { return x * x; } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_sqrt(const Jet<T, N>& x) { return sqrt(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_exp (const Jet<T, N>& x) { return exp(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_log (const Jet<T, N>& x) { return log(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_sin (const Jet<T, N>& x) { return sin(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_cos (const Jet<T, N>& x) { return cos(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_tan (const Jet<T, N>& x) { return tan(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_atan(const Jet<T, N>& x) { return atan(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_sinh(const Jet<T, N>& x) { return sinh(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_cosh(const Jet<T, N>& x) { return cosh(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_tanh(const Jet<T, N>& x) { return tanh(x); } // NOLINT -template<typename T, int N> inline Jet<T, N> ei_pow (const Jet<T, N>& x, Jet<T, N> y) { return pow(x, y); } // NOLINT + + return result; +} // Note: This has to be in the ceres namespace for argument dependent lookup to // function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with // strange compile errors. template <typename T, int N> -inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) { +inline std::ostream& operator<<(std::ostream& s, const Jet<T, N>& z) { s << "[" << z.a << " ; "; for (int i = 0; i < N; ++i) { s << z.v[i]; @@ -799,15 +853,78 @@ inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) { s << "]"; return s; } - } // namespace ceres +namespace std { +template <typename T, int N> +struct numeric_limits<ceres::Jet<T, N>> { + static constexpr bool is_specialized = true; + static constexpr bool is_signed = std::numeric_limits<T>::is_signed; + static constexpr bool is_integer = std::numeric_limits<T>::is_integer; + static constexpr bool is_exact = std::numeric_limits<T>::is_exact; + static constexpr bool has_infinity = std::numeric_limits<T>::has_infinity; + static constexpr bool has_quiet_NaN = std::numeric_limits<T>::has_quiet_NaN; + static constexpr bool has_signaling_NaN = + std::numeric_limits<T>::has_signaling_NaN; + static constexpr bool is_iec559 = std::numeric_limits<T>::is_iec559; + static constexpr bool is_bounded = std::numeric_limits<T>::is_bounded; + static constexpr bool is_modulo = std::numeric_limits<T>::is_modulo; + + static constexpr std::float_denorm_style has_denorm = + std::numeric_limits<T>::has_denorm; + static constexpr std::float_round_style round_style = + std::numeric_limits<T>::round_style; + + static constexpr int digits = std::numeric_limits<T>::digits; + static constexpr int digits10 = std::numeric_limits<T>::digits10; + static constexpr int max_digits10 = std::numeric_limits<T>::max_digits10; + static constexpr int radix = std::numeric_limits<T>::radix; + static constexpr int min_exponent = std::numeric_limits<T>::min_exponent; + static constexpr int min_exponent10 = std::numeric_limits<T>::max_exponent10; + static constexpr int max_exponent = std::numeric_limits<T>::max_exponent; + static constexpr int max_exponent10 = std::numeric_limits<T>::max_exponent10; + static constexpr bool traps = std::numeric_limits<T>::traps; + static constexpr bool tinyness_before = + std::numeric_limits<T>::tinyness_before; + + static constexpr ceres::Jet<T, N> min() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::min()); + } + static constexpr ceres::Jet<T, N> lowest() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::lowest()); + } + static constexpr ceres::Jet<T, N> epsilon() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::epsilon()); + } + static constexpr ceres::Jet<T, N> round_error() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::round_error()); + } + static constexpr ceres::Jet<T, N> infinity() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::infinity()); + } + static constexpr ceres::Jet<T, N> quiet_NaN() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::quiet_NaN()); + } + static constexpr ceres::Jet<T, N> signaling_NaN() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::signaling_NaN()); + } + static constexpr ceres::Jet<T, N> denorm_min() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::denorm_min()); + } + + static constexpr ceres::Jet<T, N> max() noexcept { + return ceres::Jet<T, N>(std::numeric_limits<T>::max()); + } +}; + +} // namespace std + namespace Eigen { // Creating a specialization of NumTraits enables placing Jet objects inside // Eigen arrays, getting all the goodness of Eigen combined with autodiff. -template<typename T, int N> -struct NumTraits<ceres::Jet<T, N> > { +template <typename T, int N> +struct NumTraits<ceres::Jet<T, N>> { typedef ceres::Jet<T, N> Real; typedef ceres::Jet<T, N> NonInteger; typedef ceres::Jet<T, N> Nested; @@ -821,6 +938,8 @@ struct NumTraits<ceres::Jet<T, N> > { return Real(std::numeric_limits<T>::epsilon()); } + static inline int digits10() { return NumTraits<T>::digits10(); } + enum { IsComplex = 0, IsInteger = 0, @@ -833,7 +952,7 @@ struct NumTraits<ceres::Jet<T, N> > { RequireInitialization = 1 }; - template<bool Vectorized> + template <bool Vectorized> struct Div { enum { #if defined(EIGEN_VECTORIZE_AVX) @@ -847,6 +966,24 @@ struct NumTraits<ceres::Jet<T, N> > { Cost = 3 }; }; + + static inline Real highest() { return Real(std::numeric_limits<T>::max()); } + static inline Real lowest() { return Real(-std::numeric_limits<T>::max()); } +}; + +// Specifying the return type of binary operations between Jets and scalar types +// allows you to perform matrix/array operations with Eigen matrices and arrays +// such as addition, subtraction, multiplication, and division where one Eigen +// matrix/array is of type Jet and the other is a scalar type. This improves +// performance by using the optimized scalar-to-Jet binary operations but +// is only available on Eigen versions >= 3.3 +template <typename BinaryOp, typename T, int N> +struct ScalarBinaryOpTraits<ceres::Jet<T, N>, T, BinaryOp> { + typedef ceres::Jet<T, N> ReturnType; +}; +template <typename BinaryOp, typename T, int N> +struct ScalarBinaryOpTraits<T, ceres::Jet<T, N>, BinaryOp> { + typedef ceres::Jet<T, N> ReturnType; }; } // namespace Eigen |