Ceres: Update to the latest actual version
[blender.git] / extern / ceres / include / ceres / jet.h
1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 //   this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 //   this list of conditions and the following disclaimer in the documentation
12 //   and/or other materials provided with the distribution.
13 // * Neither the name of Google Inc. nor the names of its contributors may be
14 //   used to endorse or promote products derived from this software without
15 //   specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 // Author: keir@google.com (Keir Mierle)
30 //
31 // A simple implementation of N-dimensional dual numbers, for automatically
32 // computing exact derivatives of functions.
33 //
34 // While a complete treatment of the mechanics of automatic differentation is
35 // beyond the scope of this header (see
36 // http://en.wikipedia.org/wiki/Automatic_differentiation for details), the
37 // basic idea is to extend normal arithmetic with an extra element, "e," often
38 // denoted with the greek symbol epsilon, such that e != 0 but e^2 = 0. Dual
39 // numbers are extensions of the real numbers analogous to complex numbers:
40 // whereas complex numbers augment the reals by introducing an imaginary unit i
41 // such that i^2 = -1, dual numbers introduce an "infinitesimal" unit e such
42 // that e^2 = 0. Dual numbers have two components: the "real" component and the
43 // "infinitesimal" component, generally written as x + y*e. Surprisingly, this
44 // leads to a convenient method for computing exact derivatives without needing
45 // to manipulate complicated symbolic expressions.
46 //
47 // For example, consider the function
48 //
49 //   f(x) = x^2 ,
50 //
51 // evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20.
52 // Next, augument 10 with an infinitesimal to get:
53 //
54 //   f(10 + e) = (10 + e)^2
55 //             = 100 + 2 * 10 * e + e^2
56 //             = 100 + 20 * e       -+-
57 //                     --            |
58 //                     |             +--- This is zero, since e^2 = 0
59 //                     |
60 //                     +----------------- This is df/dx!
61 //
62 // Note that the derivative of f with respect to x is simply the infinitesimal
63 // component of the value of f(x + e). So, in order to take the derivative of
64 // any function, it is only necessary to replace the numeric "object" used in
65 // the function with one extended with infinitesimals. The class Jet, defined in
66 // this header, is one such example of this, where substitution is done with
67 // templates.
68 //
69 // To handle derivatives of functions taking multiple arguments, different
70 // infinitesimals are used, one for each variable to take the derivative of. For
71 // example, consider a scalar function of two scalar parameters x and y:
72 //
73 //   f(x, y) = x^2 + x * y
74 //
75 // Following the technique above, to compute the derivatives df/dx and df/dy for
76 // f(1, 3) involves doing two evaluations of f, the first time replacing x with
77 // x + e, the second time replacing y with y + e.
78 //
79 // For df/dx:
80 //
81 //   f(1 + e, y) = (1 + e)^2 + (1 + e) * 3
82 //               = 1 + 2 * e + 3 + 3 * e
83 //               = 4 + 5 * e
84 //
85 //               --> df/dx = 5
86 //
87 // For df/dy:
88 //
89 //   f(1, 3 + e) = 1^2 + 1 * (3 + e)
90 //               = 1 + 3 + e
91 //               = 4 + e
92 //
93 //               --> df/dy = 1
94 //
95 // To take the gradient of f with the implementation of dual numbers ("jets") in
96 // this file, it is necessary to create a single jet type which has components
97 // for the derivative in x and y, and passing them to a templated version of f:
98 //
99 //   template<typename T>
100 //   T f(const T &x, const T &y) {
101 //     return x * x + x * y;
102 //   }
103 //
104 //   // The "2" means there should be 2 dual number components.
105 //   Jet<double, 2> x(0);  // Pick the 0th dual number for x.
106 //   Jet<double, 2> y(1);  // Pick the 1st dual number for y.
107 //   Jet<double, 2> z = f(x, y);
108 //
109 //   LOG(INFO) << "df/dx = " << z.v[0]
110 //             << "df/dy = " << z.v[1];
111 //
112 // Most users should not use Jet objects directly; a wrapper around Jet objects,
113 // which makes computing the derivative, gradient, or jacobian of templated
114 // functors simple, is in autodiff.h. Even autodiff.h should not be used
115 // directly; instead autodiff_cost_function.h is typically the file of interest.
116 //
117 // For the more mathematically inclined, this file implements first-order
118 // "jets". A 1st order jet is an element of the ring
119 //
120 //   T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2
121 //
122 // which essentially means that each jet consists of a "scalar" value 'a' from T
123 // and a 1st order perturbation vector 'v' of length N:
124 //
125 //   x = a + \sum_i v[i] t_i
126 //
127 // A shorthand is to write an element as x = a + u, where u is the pertubation.
128 // Then, the main point about the arithmetic of jets is that the product of
129 // perturbations is zero:
130 //
131 //   (a + u) * (b + v) = ab + av + bu + uv
132 //                     = ab + (av + bu) + 0
133 //
134 // which is what operator* implements below. Addition is simpler:
135 //
136 //   (a + u) + (b + v) = (a + b) + (u + v).
137 //
138 // The only remaining question is how to evaluate the function of a jet, for
139 // which we use the chain rule:
140 //
141 //   f(a + u) = f(a) + f'(a) u
142 //
143 // where f'(a) is the (scalar) derivative of f at a.
144 //
145 // By pushing these things through sufficiently and suitably templated
146 // functions, we can do automatic differentiation. Just be sure to turn on
147 // function inlining and common-subexpression elimination, or it will be very
148 // slow!
149 //
150 // WARNING: Most Ceres users should not directly include this file or know the
151 // details of how jets work. Instead the suggested method for automatic
152 // derivatives is to use autodiff_cost_function.h, which is a wrapper around
153 // both jets.h and autodiff.h to make taking derivatives of cost functions for
154 // use in Ceres easier.
155
156 #ifndef CERES_PUBLIC_JET_H_
157 #define CERES_PUBLIC_JET_H_
158
159 #include <cmath>
160 #include <iosfwd>
161 #include <iostream>  // NOLINT
162 #include <limits>
163 #include <string>
164
165 #include "Eigen/Core"
166 #include "ceres/fpclassify.h"
167 #include "ceres/internal/port.h"
168
169 namespace ceres {
170
171 template <typename T, int N>
172 struct Jet {
173   enum { DIMENSION = N };
174
175   // Default-construct "a" because otherwise this can lead to false errors about
176   // uninitialized uses when other classes relying on default constructed T
177   // (where T is a Jet<T, N>). This usually only happens in opt mode. Note that
178   // the C++ standard mandates that e.g. default constructed doubles are
179   // initialized to 0.0; see sections 8.5 of the C++03 standard.
180   Jet() : a() {
181     v.setZero();
182   }
183
184   // Constructor from scalar: a + 0.
185   explicit Jet(const T& value) {
186     a = value;
187     v.setZero();
188   }
189
190   // Constructor from scalar plus variable: a + t_i.
191   Jet(const T& value, int k) {
192     a = value;
193     v.setZero();
194     v[k] = T(1.0);
195   }
196
197   // Constructor from scalar and vector part
198   // The use of Eigen::DenseBase allows Eigen expressions
199   // to be passed in without being fully evaluated until
200   // they are assigned to v
201   template<typename Derived>
202   EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase<Derived> &v)
203       : a(a), v(v) {
204   }
205
206   // Compound operators
207   Jet<T, N>& operator+=(const Jet<T, N> &y) {
208     *this = *this + y;
209     return *this;
210   }
211
212   Jet<T, N>& operator-=(const Jet<T, N> &y) {
213     *this = *this - y;
214     return *this;
215   }
216
217   Jet<T, N>& operator*=(const Jet<T, N> &y) {
218     *this = *this * y;
219     return *this;
220   }
221
222   Jet<T, N>& operator/=(const Jet<T, N> &y) {
223     *this = *this / y;
224     return *this;
225   }
226
227   // The scalar part.
228   T a;
229
230   // The infinitesimal part.
231
232   // We allocate Jets on the stack and other places they
233   // might not be aligned to 16-byte boundaries.  If we have C++11, we
234   // can specify their alignment anyway, and thus can safely enable
235   // vectorization on those matrices; in C++99, we are out of luck.  Figure out
236   // what case we're in and do the right thing.
237 #ifndef CERES_USE_CXX11
238   // fall back to safe version:
239   Eigen::Matrix<T, N, 1, Eigen::DontAlign> v;
240 #else
241   static constexpr bool kShouldAlignMatrix =
242       16 <= ::ceres::port_constants::kMaxAlignBytes;
243   static constexpr int kAlignHint = kShouldAlignMatrix ?
244       Eigen::AutoAlign : Eigen::DontAlign;
245   static constexpr size_t kAlignment = kShouldAlignMatrix ? 16 : 1;
246   alignas(kAlignment) Eigen::Matrix<T, N, 1, kAlignHint> v;
247 #endif
248 };
249
250 // Unary +
251 template<typename T, int N> inline
252 Jet<T, N> const& operator+(const Jet<T, N>& f) {
253   return f;
254 }
255
256 // TODO(keir): Try adding __attribute__((always_inline)) to these functions to
257 // see if it causes a performance increase.
258
259 // Unary -
260 template<typename T, int N> inline
261 Jet<T, N> operator-(const Jet<T, N>&f) {
262   return Jet<T, N>(-f.a, -f.v);
263 }
264
265 // Binary +
266 template<typename T, int N> inline
267 Jet<T, N> operator+(const Jet<T, N>& f,
268                     const Jet<T, N>& g) {
269   return Jet<T, N>(f.a + g.a, f.v + g.v);
270 }
271
272 // Binary + with a scalar: x + s
273 template<typename T, int N> inline
274 Jet<T, N> operator+(const Jet<T, N>& f, T s) {
275   return Jet<T, N>(f.a + s, f.v);
276 }
277
278 // Binary + with a scalar: s + x
279 template<typename T, int N> inline
280 Jet<T, N> operator+(T s, const Jet<T, N>& f) {
281   return Jet<T, N>(f.a + s, f.v);
282 }
283
284 // Binary -
285 template<typename T, int N> inline
286 Jet<T, N> operator-(const Jet<T, N>& f,
287                     const Jet<T, N>& g) {
288   return Jet<T, N>(f.a - g.a, f.v - g.v);
289 }
290
291 // Binary - with a scalar: x - s
292 template<typename T, int N> inline
293 Jet<T, N> operator-(const Jet<T, N>& f, T s) {
294   return Jet<T, N>(f.a - s, f.v);
295 }
296
297 // Binary - with a scalar: s - x
298 template<typename T, int N> inline
299 Jet<T, N> operator-(T s, const Jet<T, N>& f) {
300   return Jet<T, N>(s - f.a, -f.v);
301 }
302
303 // Binary *
304 template<typename T, int N> inline
305 Jet<T, N> operator*(const Jet<T, N>& f,
306                     const Jet<T, N>& g) {
307   return Jet<T, N>(f.a * g.a, f.a * g.v + f.v * g.a);
308 }
309
310 // Binary * with a scalar: x * s
311 template<typename T, int N> inline
312 Jet<T, N> operator*(const Jet<T, N>& f, T s) {
313   return Jet<T, N>(f.a * s, f.v * s);
314 }
315
316 // Binary * with a scalar: s * x
317 template<typename T, int N> inline
318 Jet<T, N> operator*(T s, const Jet<T, N>& f) {
319   return Jet<T, N>(f.a * s, f.v * s);
320 }
321
322 // Binary /
323 template<typename T, int N> inline
324 Jet<T, N> operator/(const Jet<T, N>& f,
325                     const Jet<T, N>& g) {
326   // This uses:
327   //
328   //   a + u   (a + u)(b - v)   (a + u)(b - v)
329   //   ----- = -------------- = --------------
330   //   b + v   (b + v)(b - v)        b^2
331   //
332   // which holds because v*v = 0.
333   const T g_a_inverse = T(1.0) / g.a;
334   const T f_a_by_g_a = f.a * g_a_inverse;
335   return Jet<T, N>(f.a * g_a_inverse, (f.v - f_a_by_g_a * g.v) * g_a_inverse);
336 }
337
338 // Binary / with a scalar: s / x
339 template<typename T, int N> inline
340 Jet<T, N> operator/(T s, const Jet<T, N>& g) {
341   const T minus_s_g_a_inverse2 = -s / (g.a * g.a);
342   return Jet<T, N>(s / g.a, g.v * minus_s_g_a_inverse2);
343 }
344
345 // Binary / with a scalar: x / s
346 template<typename T, int N> inline
347 Jet<T, N> operator/(const Jet<T, N>& f, T s) {
348   const T s_inverse = 1.0 / s;
349   return Jet<T, N>(f.a * s_inverse, f.v * s_inverse);
350 }
351
352 // Binary comparison operators for both scalars and jets.
353 #define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \
354 template<typename T, int N> inline \
355 bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \
356   return f.a op g.a; \
357 } \
358 template<typename T, int N> inline \
359 bool operator op(const T& s, const Jet<T, N>& g) { \
360   return s op g.a; \
361 } \
362 template<typename T, int N> inline \
363 bool operator op(const Jet<T, N>& f, const T& s) { \
364   return f.a op s; \
365 }
366 CERES_DEFINE_JET_COMPARISON_OPERATOR( <  )  // NOLINT
367 CERES_DEFINE_JET_COMPARISON_OPERATOR( <= )  // NOLINT
368 CERES_DEFINE_JET_COMPARISON_OPERATOR( >  )  // NOLINT
369 CERES_DEFINE_JET_COMPARISON_OPERATOR( >= )  // NOLINT
370 CERES_DEFINE_JET_COMPARISON_OPERATOR( == )  // NOLINT
371 CERES_DEFINE_JET_COMPARISON_OPERATOR( != )  // NOLINT
372 #undef CERES_DEFINE_JET_COMPARISON_OPERATOR
373
374 // Pull some functions from namespace std.
375 //
376 // This is necessary because we want to use the same name (e.g. 'sqrt') for
377 // double-valued and Jet-valued functions, but we are not allowed to put
378 // Jet-valued functions inside namespace std.
379 //
380 // TODO(keir): Switch to "using".
381 inline double abs     (double x) { return std::abs(x);      }
382 inline double log     (double x) { return std::log(x);      }
383 inline double exp     (double x) { return std::exp(x);      }
384 inline double sqrt    (double x) { return std::sqrt(x);     }
385 inline double cos     (double x) { return std::cos(x);      }
386 inline double acos    (double x) { return std::acos(x);     }
387 inline double sin     (double x) { return std::sin(x);      }
388 inline double asin    (double x) { return std::asin(x);     }
389 inline double tan     (double x) { return std::tan(x);      }
390 inline double atan    (double x) { return std::atan(x);     }
391 inline double sinh    (double x) { return std::sinh(x);     }
392 inline double cosh    (double x) { return std::cosh(x);     }
393 inline double tanh    (double x) { return std::tanh(x);     }
394 inline double floor   (double x) { return std::floor(x);    }
395 inline double ceil    (double x) { return std::ceil(x);     }
396 inline double pow  (double x, double y) { return std::pow(x, y);   }
397 inline double atan2(double y, double x) { return std::atan2(y, x); }
398
399 // In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule.
400
401 // abs(x + h) ~= x + h or -(x + h)
402 template <typename T, int N> inline
403 Jet<T, N> abs(const Jet<T, N>& f) {
404   return f.a < T(0.0) ? -f : f;
405 }
406
407 // log(a + h) ~= log(a) + h / a
408 template <typename T, int N> inline
409 Jet<T, N> log(const Jet<T, N>& f) {
410   const T a_inverse = T(1.0) / f.a;
411   return Jet<T, N>(log(f.a), f.v * a_inverse);
412 }
413
414 // exp(a + h) ~= exp(a) + exp(a) h
415 template <typename T, int N> inline
416 Jet<T, N> exp(const Jet<T, N>& f) {
417   const T tmp = exp(f.a);
418   return Jet<T, N>(tmp, tmp * f.v);
419 }
420
421 // sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a))
422 template <typename T, int N> inline
423 Jet<T, N> sqrt(const Jet<T, N>& f) {
424   const T tmp = sqrt(f.a);
425   const T two_a_inverse = T(1.0) / (T(2.0) * tmp);
426   return Jet<T, N>(tmp, f.v * two_a_inverse);
427 }
428
429 // cos(a + h) ~= cos(a) - sin(a) h
430 template <typename T, int N> inline
431 Jet<T, N> cos(const Jet<T, N>& f) {
432   return Jet<T, N>(cos(f.a), - sin(f.a) * f.v);
433 }
434
435 // acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h
436 template <typename T, int N> inline
437 Jet<T, N> acos(const Jet<T, N>& f) {
438   const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a);
439   return Jet<T, N>(acos(f.a), tmp * f.v);
440 }
441
442 // sin(a + h) ~= sin(a) + cos(a) h
443 template <typename T, int N> inline
444 Jet<T, N> sin(const Jet<T, N>& f) {
445   return Jet<T, N>(sin(f.a), cos(f.a) * f.v);
446 }
447
448 // asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h
449 template <typename T, int N> inline
450 Jet<T, N> asin(const Jet<T, N>& f) {
451   const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a);
452   return Jet<T, N>(asin(f.a), tmp * f.v);
453 }
454
455 // tan(a + h) ~= tan(a) + (1 + tan(a)^2) h
456 template <typename T, int N> inline
457 Jet<T, N> tan(const Jet<T, N>& f) {
458   const T tan_a = tan(f.a);
459   const T tmp = T(1.0) + tan_a * tan_a;
460   return Jet<T, N>(tan_a, tmp * f.v);
461 }
462
463 // atan(a + h) ~= atan(a) + 1 / (1 + a^2) h
464 template <typename T, int N> inline
465 Jet<T, N> atan(const Jet<T, N>& f) {
466   const T tmp = T(1.0) / (T(1.0) + f.a * f.a);
467   return Jet<T, N>(atan(f.a), tmp * f.v);
468 }
469
470 // sinh(a + h) ~= sinh(a) + cosh(a) h
471 template <typename T, int N> inline
472 Jet<T, N> sinh(const Jet<T, N>& f) {
473   return Jet<T, N>(sinh(f.a), cosh(f.a) * f.v);
474 }
475
476 // cosh(a + h) ~= cosh(a) + sinh(a) h
477 template <typename T, int N> inline
478 Jet<T, N> cosh(const Jet<T, N>& f) {
479   return Jet<T, N>(cosh(f.a), sinh(f.a) * f.v);
480 }
481
482 // tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h
483 template <typename T, int N> inline
484 Jet<T, N> tanh(const Jet<T, N>& f) {
485   const T tanh_a = tanh(f.a);
486   const T tmp = T(1.0) - tanh_a * tanh_a;
487   return Jet<T, N>(tanh_a, tmp * f.v);
488 }
489
490 // The floor function should be used with extreme care as this operation will
491 // result in a zero derivative which provides no information to the solver.
492 //
493 // floor(a + h) ~= floor(a) + 0
494 template <typename T, int N> inline
495 Jet<T, N> floor(const Jet<T, N>& f) {
496   return Jet<T, N>(floor(f.a));
497 }
498
499 // The ceil function should be used with extreme care as this operation will
500 // result in a zero derivative which provides no information to the solver.
501 //
502 // ceil(a + h) ~= ceil(a) + 0
503 template <typename T, int N> inline
504 Jet<T, N> ceil(const Jet<T, N>& f) {
505   return Jet<T, N>(ceil(f.a));
506 }
507
508 // Bessel functions of the first kind with integer order equal to 0, 1, n.
509 //
510 // Microsoft has deprecated the j[0,1,n]() POSIX Bessel functions in favour of
511 // _j[0,1,n]().  Where available on MSVC, use _j[0,1,n]() to avoid deprecated
512 // function errors in client code (the specific warning is suppressed when
513 // Ceres itself is built).
514 inline double BesselJ0(double x) {
515 #if defined(_MSC_VER) && defined(_j0)
516   return _j0(x);
517 #else
518   return j0(x);
519 #endif
520 }
521 inline double BesselJ1(double x) {
522 #if defined(_MSC_VER) && defined(_j1)
523   return _j1(x);
524 #else
525   return j1(x);
526 #endif
527 }
528 inline double BesselJn(int n, double x) {
529 #if defined(_MSC_VER) && defined(_jn)
530   return _jn(n, x);
531 #else
532   return jn(n, x);
533 #endif
534 }
535
536 // For the formulae of the derivatives of the Bessel functions see the book:
537 // Olver, Lozier, Boisvert, Clark, NIST Handbook of Mathematical Functions,
538 // Cambridge University Press 2010.
539 //
540 // Formulae are also available at http://dlmf.nist.gov
541
542 // See formula http://dlmf.nist.gov/10.6#E3
543 // j0(a + h) ~= j0(a) - j1(a) h
544 template <typename T, int N> inline
545 Jet<T, N> BesselJ0(const Jet<T, N>& f) {
546   return Jet<T, N>(BesselJ0(f.a),
547                    -BesselJ1(f.a) * f.v);
548 }
549
550 // See formula http://dlmf.nist.gov/10.6#E1
551 // j1(a + h) ~= j1(a) + 0.5 ( j0(a) - j2(a) ) h
552 template <typename T, int N> inline
553 Jet<T, N> BesselJ1(const Jet<T, N>& f) {
554   return Jet<T, N>(BesselJ1(f.a),
555                    T(0.5) * (BesselJ0(f.a) - BesselJn(2, f.a)) * f.v);
556 }
557
558 // See formula http://dlmf.nist.gov/10.6#E1
559 // j_n(a + h) ~= j_n(a) + 0.5 ( j_{n-1}(a) - j_{n+1}(a) ) h
560 template <typename T, int N> inline
561 Jet<T, N> BesselJn(int n, const Jet<T, N>& f) {
562   return Jet<T, N>(BesselJn(n, f.a),
563                    T(0.5) * (BesselJn(n - 1, f.a) - BesselJn(n + 1, f.a)) * f.v);
564 }
565
566 // Jet Classification. It is not clear what the appropriate semantics are for
567 // these classifications. This picks that IsFinite and isnormal are "all"
568 // operations, i.e. all elements of the jet must be finite for the jet itself
569 // to be finite (or normal). For IsNaN and IsInfinite, the answer is less
570 // clear. This takes a "any" approach for IsNaN and IsInfinite such that if any
571 // part of a jet is nan or inf, then the entire jet is nan or inf. This leads
572 // to strange situations like a jet can be both IsInfinite and IsNaN, but in
573 // practice the "any" semantics are the most useful for e.g. checking that
574 // derivatives are sane.
575
576 // The jet is finite if all parts of the jet are finite.
577 template <typename T, int N> inline
578 bool IsFinite(const Jet<T, N>& f) {
579   if (!IsFinite(f.a)) {
580     return false;
581   }
582   for (int i = 0; i < N; ++i) {
583     if (!IsFinite(f.v[i])) {
584       return false;
585     }
586   }
587   return true;
588 }
589
590 // The jet is infinite if any part of the jet is infinite.
591 template <typename T, int N> inline
592 bool IsInfinite(const Jet<T, N>& f) {
593   if (IsInfinite(f.a)) {
594     return true;
595   }
596   for (int i = 0; i < N; i++) {
597     if (IsInfinite(f.v[i])) {
598       return true;
599     }
600   }
601   return false;
602 }
603
604 // The jet is NaN if any part of the jet is NaN.
605 template <typename T, int N> inline
606 bool IsNaN(const Jet<T, N>& f) {
607   if (IsNaN(f.a)) {
608     return true;
609   }
610   for (int i = 0; i < N; ++i) {
611     if (IsNaN(f.v[i])) {
612       return true;
613     }
614   }
615   return false;
616 }
617
618 // The jet is normal if all parts of the jet are normal.
619 template <typename T, int N> inline
620 bool IsNormal(const Jet<T, N>& f) {
621   if (!IsNormal(f.a)) {
622     return false;
623   }
624   for (int i = 0; i < N; ++i) {
625     if (!IsNormal(f.v[i])) {
626       return false;
627     }
628   }
629   return true;
630 }
631
632 // atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2)
633 //
634 // In words: the rate of change of theta is 1/r times the rate of
635 // change of (x, y) in the positive angular direction.
636 template <typename T, int N> inline
637 Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) {
638   // Note order of arguments:
639   //
640   //   f = a + da
641   //   g = b + db
642
643   T const tmp = T(1.0) / (f.a * f.a + g.a * g.a);
644   return Jet<T, N>(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v));
645 }
646
647
648 // pow -- base is a differentiable function, exponent is a constant.
649 // (a+da)^p ~= a^p + p*a^(p-1) da
650 template <typename T, int N> inline
651 Jet<T, N> pow(const Jet<T, N>& f, double g) {
652   T const tmp = g * pow(f.a, g - T(1.0));
653   return Jet<T, N>(pow(f.a, g), tmp * f.v);
654 }
655
656 // pow -- base is a constant, exponent is a differentiable function.
657 // We have various special cases, see the comment for pow(Jet, Jet) for
658 // analysis:
659 //
660 // 1. For f > 0 we have: (f)^(g + dg) ~= f^g + f^g log(f) dg
661 //
662 // 2. For f == 0 and g > 0 we have: (f)^(g + dg) ~= f^g
663 //
664 // 3. For f < 0 and integer g we have: (f)^(g + dg) ~= f^g but if dg
665 // != 0, the derivatives are not defined and we return NaN.
666
667 template <typename T, int N> inline
668 Jet<T, N> pow(double f, const Jet<T, N>& g) {
669   if (f == 0 && g.a > 0) {
670     // Handle case 2.
671     return Jet<T, N>(T(0.0));
672   }
673   if (f < 0 && g.a == floor(g.a)) {
674     // Handle case 3.
675     Jet<T, N> ret(pow(f, g.a));
676     for (int i = 0; i < N; i++) {
677       if (g.v[i] != T(0.0)) {
678         // Return a NaN when g.v != 0.
679         ret.v[i] = std::numeric_limits<T>::quiet_NaN();
680       }
681     }
682     return ret;
683   }
684   // Handle case 1.
685   T const tmp = pow(f, g.a);
686   return Jet<T, N>(tmp, log(f) * tmp * g.v);
687 }
688
689 // pow -- both base and exponent are differentiable functions. This has a
690 // variety of special cases that require careful handling.
691 //
692 // 1. For f > 0:
693 //    (f + df)^(g + dg) ~= f^g + f^(g - 1) * (g * df + f * log(f) * dg)
694 //    The numerical evaluation of f * log(f) for f > 0 is well behaved, even for
695 //    extremely small values (e.g. 1e-99).
696 //
697 // 2. For f == 0 and g > 1: (f + df)^(g + dg) ~= 0
698 //    This cases is needed because log(0) can not be evaluated in the f > 0
699 //    expression. However the function f*log(f) is well behaved around f == 0
700 //    and its limit as f-->0 is zero.
701 //
702 // 3. For f == 0 and g == 1: (f + df)^(g + dg) ~= 0 + df
703 //
704 // 4. For f == 0 and 0 < g < 1: The value is finite but the derivatives are not.
705 //
706 // 5. For f == 0 and g < 0: The value and derivatives of f^g are not finite.
707 //
708 // 6. For f == 0 and g == 0: The C standard incorrectly defines 0^0 to be 1
709 //    "because there are applications that can exploit this definition". We
710 //    (arbitrarily) decree that derivatives here will be nonfinite, since that
711 //    is consistent with the behavior for f == 0, g < 0 and 0 < g < 1.
712 //    Practically any definition could have been justified because mathematical
713 //    consistency has been lost at this point.
714 //
715 // 7. For f < 0, g integer, dg == 0: (f + df)^(g + dg) ~= f^g + g * f^(g - 1) df
716 //    This is equivalent to the case where f is a differentiable function and g
717 //    is a constant (to first order).
718 //
719 // 8. For f < 0, g integer, dg != 0: The value is finite but the derivatives are
720 //    not, because any change in the value of g moves us away from the point
721 //    with a real-valued answer into the region with complex-valued answers.
722 //
723 // 9. For f < 0, g noninteger: The value and derivatives of f^g are not finite.
724
725 template <typename T, int N> inline
726 Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) {
727   if (f.a == 0 && g.a >= 1) {
728     // Handle cases 2 and 3.
729     if (g.a > 1) {
730       return Jet<T, N>(T(0.0));
731     }
732     return f;
733   }
734   if (f.a < 0 && g.a == floor(g.a)) {
735     // Handle cases 7 and 8.
736     T const tmp = g.a * pow(f.a, g.a - T(1.0));
737     Jet<T, N> ret(pow(f.a, g.a), tmp * f.v);
738     for (int i = 0; i < N; i++) {
739       if (g.v[i] != T(0.0)) {
740         // Return a NaN when g.v != 0.
741         ret.v[i] = std::numeric_limits<T>::quiet_NaN();
742       }
743     }
744     return ret;
745   }
746   // Handle the remaining cases. For cases 4,5,6,9 we allow the log() function
747   // to generate -HUGE_VAL or NaN, since those cases result in a nonfinite
748   // derivative.
749   T const tmp1 = pow(f.a, g.a);
750   T const tmp2 = g.a * pow(f.a, g.a - T(1.0));
751   T const tmp3 = tmp1 * log(f.a);
752   return Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v);
753 }
754
755 // Define the helper functions Eigen needs to embed Jet types.
756 //
757 // NOTE(keir): machine_epsilon() and precision() are missing, because they don't
758 // work with nested template types (e.g. where the scalar is itself templated).
759 // Among other things, this means that decompositions of Jet's does not work,
760 // for example
761 //
762 //   Matrix<Jet<T, N> ... > A, x, b;
763 //   ...
764 //   A.solve(b, &x)
765 //
766 // does not work and will fail with a strange compiler error.
767 //
768 // TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we
769 // switch to 3.0, also add the rest of the specialization functionality.
770 template<typename T, int N> inline const Jet<T, N>& ei_conj(const Jet<T, N>& x) { return x;              }  // NOLINT
771 template<typename T, int N> inline const Jet<T, N>& ei_real(const Jet<T, N>& x) { return x;              }  // NOLINT
772 template<typename T, int N> inline       Jet<T, N>  ei_imag(const Jet<T, N>&  ) { return Jet<T, N>(0.0); }  // NOLINT
773 template<typename T, int N> inline       Jet<T, N>  ei_abs (const Jet<T, N>& x) { return fabs(x);        }  // NOLINT
774 template<typename T, int N> inline       Jet<T, N>  ei_abs2(const Jet<T, N>& x) { return x * x;          }  // NOLINT
775 template<typename T, int N> inline       Jet<T, N>  ei_sqrt(const Jet<T, N>& x) { return sqrt(x);        }  // NOLINT
776 template<typename T, int N> inline       Jet<T, N>  ei_exp (const Jet<T, N>& x) { return exp(x);         }  // NOLINT
777 template<typename T, int N> inline       Jet<T, N>  ei_log (const Jet<T, N>& x) { return log(x);         }  // NOLINT
778 template<typename T, int N> inline       Jet<T, N>  ei_sin (const Jet<T, N>& x) { return sin(x);         }  // NOLINT
779 template<typename T, int N> inline       Jet<T, N>  ei_cos (const Jet<T, N>& x) { return cos(x);         }  // NOLINT
780 template<typename T, int N> inline       Jet<T, N>  ei_tan (const Jet<T, N>& x) { return tan(x);         }  // NOLINT
781 template<typename T, int N> inline       Jet<T, N>  ei_atan(const Jet<T, N>& x) { return atan(x);        }  // NOLINT
782 template<typename T, int N> inline       Jet<T, N>  ei_sinh(const Jet<T, N>& x) { return sinh(x);        }  // NOLINT
783 template<typename T, int N> inline       Jet<T, N>  ei_cosh(const Jet<T, N>& x) { return cosh(x);        }  // NOLINT
784 template<typename T, int N> inline       Jet<T, N>  ei_tanh(const Jet<T, N>& x) { return tanh(x);        }  // NOLINT
785 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
786
787 // Note: This has to be in the ceres namespace for argument dependent lookup to
788 // function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with
789 // strange compile errors.
790 template <typename T, int N>
791 inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) {
792   s << "[" << z.a << " ; ";
793   for (int i = 0; i < N; ++i) {
794     s << z.v[i];
795     if (i != N - 1) {
796       s << ", ";
797     }
798   }
799   s << "]";
800   return s;
801 }
802
803 }  // namespace ceres
804
805 namespace Eigen {
806
807 // Creating a specialization of NumTraits enables placing Jet objects inside
808 // Eigen arrays, getting all the goodness of Eigen combined with autodiff.
809 template<typename T, int N>
810 struct NumTraits<ceres::Jet<T, N> > {
811   typedef ceres::Jet<T, N> Real;
812   typedef ceres::Jet<T, N> NonInteger;
813   typedef ceres::Jet<T, N> Nested;
814   typedef ceres::Jet<T, N> Literal;
815
816   static typename ceres::Jet<T, N> dummy_precision() {
817     return ceres::Jet<T, N>(1e-12);
818   }
819
820   static inline Real epsilon() {
821     return Real(std::numeric_limits<T>::epsilon());
822   }
823
824   enum {
825     IsComplex = 0,
826     IsInteger = 0,
827     IsSigned,
828     ReadCost = 1,
829     AddCost = 1,
830     // For Jet types, multiplication is more expensive than addition.
831     MulCost = 3,
832     HasFloatingPoint = 1,
833     RequireInitialization = 1
834   };
835
836   template<bool Vectorized>
837   struct Div {
838     enum {
839 #if defined(EIGEN_VECTORIZE_AVX)
840       AVX = true,
841 #else
842       AVX = false,
843 #endif
844
845       // Assuming that for Jets, division is as expensive as
846       // multiplication.
847       Cost = 3
848     };
849   };
850 };
851
852 }  // namespace Eigen
853
854 #endif  // CERES_PUBLIC_JET_H_