1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3 // http://code.google.com/p/ceres-solver/ 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 (at) 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 // LG << "df/dx = " << z.a[0] 110 // << "df/dy = " << z.a[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 <string> 163 164 #include "Eigen/Core" 165 #include "ceres/fpclassify.h" 166 167 namespace ceres { 168 169 template <typename T, int N> 170 struct Jet { 171 enum { DIMENSION = N }; 172 173 // Default-construct "a" because otherwise this can lead to false errors about 174 // uninitialized uses when other classes relying on default constructed T 175 // (where T is a Jet<T, N>). This usually only happens in opt mode. Note that 176 // the C++ standard mandates that e.g. default constructed doubles are 177 // initialized to 0.0; see sections 8.5 of the C++03 standard. 178 Jet() : a() { 179 v.setZero(); 180 } 181 182 // Constructor from scalar: a + 0. 183 explicit Jet(const T& value) { 184 a = value; 185 v.setZero(); 186 } 187 188 // Constructor from scalar plus variable: a + t_i. 189 Jet(const T& value, int k) { 190 a = value; 191 v.setZero(); 192 v[k] = T(1.0); 193 } 194 195 // Compound operators 196 Jet<T, N>& operator+=(const Jet<T, N> &y) { 197 *this = *this + y; 198 return *this; 199 } 200 201 Jet<T, N>& operator-=(const Jet<T, N> &y) { 202 *this = *this - y; 203 return *this; 204 } 205 206 Jet<T, N>& operator*=(const Jet<T, N> &y) { 207 *this = *this * y; 208 return *this; 209 } 210 211 Jet<T, N>& operator/=(const Jet<T, N> &y) { 212 *this = *this / y; 213 return *this; 214 } 215 216 // The scalar part. 217 T a; 218 219 // The infinitesimal part. 220 // 221 // Note the Eigen::DontAlign bit is needed here because this object 222 // gets allocated on the stack and as part of other arrays and 223 // structs. Forcing the right alignment there is the source of much 224 // pain and suffering. Even if that works, passing Jets around to 225 // functions by value has problems because the C++ ABI does not 226 // guarantee alignment for function arguments. 227 // 228 // Setting the DontAlign bit prevents Eigen from using SSE for the 229 // various operations on Jets. This is a small performance penalty 230 // since the AutoDiff code will still expose much of the code as 231 // statically sized loops to the compiler. But given the subtle 232 // issues that arise due to alignment, especially when dealing with 233 // multiple platforms, it seems to be a trade off worth making. 234 Eigen::Matrix<T, N, 1, Eigen::DontAlign> v; 235 }; 236 237 // Unary + 238 template<typename T, int N> inline 239 Jet<T, N> const& operator+(const Jet<T, N>& f) { 240 return f; 241 } 242 243 // TODO(keir): Try adding __attribute__((always_inline)) to these functions to 244 // see if it causes a performance increase. 245 246 // Unary - 247 template<typename T, int N> inline 248 Jet<T, N> operator-(const Jet<T, N>&f) { 249 Jet<T, N> g; 250 g.a = -f.a; 251 g.v = -f.v; 252 return g; 253 } 254 255 // Binary + 256 template<typename T, int N> inline 257 Jet<T, N> operator+(const Jet<T, N>& f, 258 const Jet<T, N>& g) { 259 Jet<T, N> h; 260 h.a = f.a + g.a; 261 h.v = f.v + g.v; 262 return h; 263 } 264 265 // Binary + with a scalar: x + s 266 template<typename T, int N> inline 267 Jet<T, N> operator+(const Jet<T, N>& f, T s) { 268 Jet<T, N> h; 269 h.a = f.a + s; 270 h.v = f.v; 271 return h; 272 } 273 274 // Binary + with a scalar: s + x 275 template<typename T, int N> inline 276 Jet<T, N> operator+(T s, const Jet<T, N>& f) { 277 Jet<T, N> h; 278 h.a = f.a + s; 279 h.v = f.v; 280 return h; 281 } 282 283 // Binary - 284 template<typename T, int N> inline 285 Jet<T, N> operator-(const Jet<T, N>& f, 286 const Jet<T, N>& g) { 287 Jet<T, N> h; 288 h.a = f.a - g.a; 289 h.v = f.v - g.v; 290 return h; 291 } 292 293 // Binary - with a scalar: x - s 294 template<typename T, int N> inline 295 Jet<T, N> operator-(const Jet<T, N>& f, T s) { 296 Jet<T, N> h; 297 h.a = f.a - s; 298 h.v = f.v; 299 return h; 300 } 301 302 // Binary - with a scalar: s - x 303 template<typename T, int N> inline 304 Jet<T, N> operator-(T s, const Jet<T, N>& f) { 305 Jet<T, N> h; 306 h.a = s - f.a; 307 h.v = -f.v; 308 return h; 309 } 310 311 // Binary * 312 template<typename T, int N> inline 313 Jet<T, N> operator*(const Jet<T, N>& f, 314 const Jet<T, N>& g) { 315 Jet<T, N> h; 316 h.a = f.a * g.a; 317 h.v = f.a * g.v + f.v * g.a; 318 return h; 319 } 320 321 // Binary * with a scalar: x * s 322 template<typename T, int N> inline 323 Jet<T, N> operator*(const Jet<T, N>& f, T s) { 324 Jet<T, N> h; 325 h.a = f.a * s; 326 h.v = f.v * s; 327 return h; 328 } 329 330 // Binary * with a scalar: s * x 331 template<typename T, int N> inline 332 Jet<T, N> operator*(T s, const Jet<T, N>& f) { 333 Jet<T, N> h; 334 h.a = f.a * s; 335 h.v = f.v * s; 336 return h; 337 } 338 339 // Binary / 340 template<typename T, int N> inline 341 Jet<T, N> operator/(const Jet<T, N>& f, 342 const Jet<T, N>& g) { 343 Jet<T, N> h; 344 // This uses: 345 // 346 // a + u (a + u)(b - v) (a + u)(b - v) 347 // ----- = -------------- = -------------- 348 // b + v (b + v)(b - v) b^2 349 // 350 // which holds because v*v = 0. 351 const T g_a_inverse = T(1.0) / g.a; 352 h.a = f.a * g_a_inverse; 353 const T f_a_by_g_a = f.a * g_a_inverse; 354 for (int i = 0; i < N; ++i) { 355 h.v[i] = (f.v[i] - f_a_by_g_a * g.v[i]) * g_a_inverse; 356 } 357 return h; 358 } 359 360 // Binary / with a scalar: s / x 361 template<typename T, int N> inline 362 Jet<T, N> operator/(T s, const Jet<T, N>& g) { 363 Jet<T, N> h; 364 h.a = s / g.a; 365 const T minus_s_g_a_inverse2 = -s / (g.a * g.a); 366 h.v = g.v * minus_s_g_a_inverse2; 367 return h; 368 } 369 370 // Binary / with a scalar: x / s 371 template<typename T, int N> inline 372 Jet<T, N> operator/(const Jet<T, N>& f, T s) { 373 Jet<T, N> h; 374 const T s_inverse = 1.0 / s; 375 h.a = f.a * s_inverse; 376 h.v = f.v * s_inverse; 377 return h; 378 } 379 380 // Binary comparison operators for both scalars and jets. 381 #define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ 382 template<typename T, int N> inline \ 383 bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \ 384 return f.a op g.a; \ 385 } \ 386 template<typename T, int N> inline \ 387 bool operator op(const T& s, const Jet<T, N>& g) { \ 388 return s op g.a; \ 389 } \ 390 template<typename T, int N> inline \ 391 bool operator op(const Jet<T, N>& f, const T& s) { \ 392 return f.a op s; \ 393 } 394 CERES_DEFINE_JET_COMPARISON_OPERATOR( < ) // NOLINT 395 CERES_DEFINE_JET_COMPARISON_OPERATOR( <= ) // NOLINT 396 CERES_DEFINE_JET_COMPARISON_OPERATOR( > ) // NOLINT 397 CERES_DEFINE_JET_COMPARISON_OPERATOR( >= ) // NOLINT 398 CERES_DEFINE_JET_COMPARISON_OPERATOR( == ) // NOLINT 399 CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT 400 #undef CERES_DEFINE_JET_COMPARISON_OPERATOR 401 402 // Pull some functions from namespace std. 403 // 404 // This is necessary because we want to use the same name (e.g. 'sqrt') for 405 // double-valued and Jet-valued functions, but we are not allowed to put 406 // Jet-valued functions inside namespace std. 407 // 408 // TODO(keir): Switch to "using". 409 inline double abs (double x) { return std::abs(x); } 410 inline double log (double x) { return std::log(x); } 411 inline double exp (double x) { return std::exp(x); } 412 inline double sqrt (double x) { return std::sqrt(x); } 413 inline double cos (double x) { return std::cos(x); } 414 inline double acos (double x) { return std::acos(x); } 415 inline double sin (double x) { return std::sin(x); } 416 inline double asin (double x) { return std::asin(x); } 417 inline double tan (double x) { return std::tan(x); } 418 inline double atan (double x) { return std::atan(x); } 419 inline double sinh (double x) { return std::sinh(x); } 420 inline double cosh (double x) { return std::cosh(x); } 421 inline double tanh (double x) { return std::tanh(x); } 422 inline double pow (double x, double y) { return std::pow(x, y); } 423 inline double atan2(double y, double x) { return std::atan2(y, x); } 424 425 // In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule. 426 427 // abs(x + h) ~= x + h or -(x + h) 428 template <typename T, int N> inline 429 Jet<T, N> abs(const Jet<T, N>& f) { 430 return f.a < T(0.0) ? -f : f; 431 } 432 433 // log(a + h) ~= log(a) + h / a 434 template <typename T, int N> inline 435 Jet<T, N> log(const Jet<T, N>& f) { 436 Jet<T, N> g; 437 g.a = log(f.a); 438 const T a_inverse = T(1.0) / f.a; 439 g.v = f.v * a_inverse; 440 return g; 441 } 442 443 // exp(a + h) ~= exp(a) + exp(a) h 444 template <typename T, int N> inline 445 Jet<T, N> exp(const Jet<T, N>& f) { 446 Jet<T, N> g; 447 g.a = exp(f.a); 448 g.v = g.a * f.v; 449 return g; 450 } 451 452 // sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a)) 453 template <typename T, int N> inline 454 Jet<T, N> sqrt(const Jet<T, N>& f) { 455 Jet<T, N> g; 456 g.a = sqrt(f.a); 457 const T two_a_inverse = T(1.0) / (T(2.0) * g.a); 458 g.v = f.v * two_a_inverse; 459 return g; 460 } 461 462 // cos(a + h) ~= cos(a) - sin(a) h 463 template <typename T, int N> inline 464 Jet<T, N> cos(const Jet<T, N>& f) { 465 Jet<T, N> g; 466 g.a = cos(f.a); 467 const T sin_a = sin(f.a); 468 g.v = - sin_a * f.v; 469 return g; 470 } 471 472 // acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h 473 template <typename T, int N> inline 474 Jet<T, N> acos(const Jet<T, N>& f) { 475 Jet<T, N> g; 476 g.a = acos(f.a); 477 const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a); 478 g.v = tmp * f.v; 479 return g; 480 } 481 482 // sin(a + h) ~= sin(a) + cos(a) h 483 template <typename T, int N> inline 484 Jet<T, N> sin(const Jet<T, N>& f) { 485 Jet<T, N> g; 486 g.a = sin(f.a); 487 const T cos_a = cos(f.a); 488 g.v = cos_a * f.v; 489 return g; 490 } 491 492 // asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h 493 template <typename T, int N> inline 494 Jet<T, N> asin(const Jet<T, N>& f) { 495 Jet<T, N> g; 496 g.a = asin(f.a); 497 const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a); 498 g.v = tmp * f.v; 499 return g; 500 } 501 502 // tan(a + h) ~= tan(a) + (1 + tan(a)^2) h 503 template <typename T, int N> inline 504 Jet<T, N> tan(const Jet<T, N>& f) { 505 Jet<T, N> g; 506 g.a = tan(f.a); 507 double tan_a = tan(f.a); 508 const T tmp = T(1.0) + tan_a * tan_a; 509 g.v = tmp * f.v; 510 return g; 511 } 512 513 // atan(a + h) ~= atan(a) + 1 / (1 + a^2) h 514 template <typename T, int N> inline 515 Jet<T, N> atan(const Jet<T, N>& f) { 516 Jet<T, N> g; 517 g.a = atan(f.a); 518 const T tmp = T(1.0) / (T(1.0) + f.a * f.a); 519 g.v = tmp * f.v; 520 return g; 521 } 522 523 // sinh(a + h) ~= sinh(a) + cosh(a) h 524 template <typename T, int N> inline 525 Jet<T, N> sinh(const Jet<T, N>& f) { 526 Jet<T, N> g; 527 g.a = sinh(f.a); 528 const T cosh_a = cosh(f.a); 529 g.v = cosh_a * f.v; 530 return g; 531 } 532 533 // cosh(a + h) ~= cosh(a) + sinh(a) h 534 template <typename T, int N> inline 535 Jet<T, N> cosh(const Jet<T, N>& f) { 536 Jet<T, N> g; 537 g.a = cosh(f.a); 538 const T sinh_a = sinh(f.a); 539 g.v = sinh_a * f.v; 540 return g; 541 } 542 543 // tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h 544 template <typename T, int N> inline 545 Jet<T, N> tanh(const Jet<T, N>& f) { 546 Jet<T, N> g; 547 g.a = tanh(f.a); 548 double tanh_a = tanh(f.a); 549 const T tmp = T(1.0) - tanh_a * tanh_a; 550 g.v = tmp * f.v; 551 return g; 552 } 553 554 // Jet Classification. It is not clear what the appropriate semantics are for 555 // these classifications. This picks that IsFinite and isnormal are "all" 556 // operations, i.e. all elements of the jet must be finite for the jet itself 557 // to be finite (or normal). For IsNaN and IsInfinite, the answer is less 558 // clear. This takes a "any" approach for IsNaN and IsInfinite such that if any 559 // part of a jet is nan or inf, then the entire jet is nan or inf. This leads 560 // to strange situations like a jet can be both IsInfinite and IsNaN, but in 561 // practice the "any" semantics are the most useful for e.g. checking that 562 // derivatives are sane. 563 564 // The jet is finite if all parts of the jet are finite. 565 template <typename T, int N> inline 566 bool IsFinite(const Jet<T, N>& f) { 567 if (!IsFinite(f.a)) { 568 return false; 569 } 570 for (int i = 0; i < N; ++i) { 571 if (!IsFinite(f.v[i])) { 572 return false; 573 } 574 } 575 return true; 576 } 577 578 // The jet is infinite if any part of the jet is infinite. 579 template <typename T, int N> inline 580 bool IsInfinite(const Jet<T, N>& f) { 581 if (IsInfinite(f.a)) { 582 return true; 583 } 584 for (int i = 0; i < N; i++) { 585 if (IsInfinite(f.v[i])) { 586 return true; 587 } 588 } 589 return false; 590 } 591 592 // The jet is NaN if any part of the jet is NaN. 593 template <typename T, int N> inline 594 bool IsNaN(const Jet<T, N>& f) { 595 if (IsNaN(f.a)) { 596 return true; 597 } 598 for (int i = 0; i < N; ++i) { 599 if (IsNaN(f.v[i])) { 600 return true; 601 } 602 } 603 return false; 604 } 605 606 // The jet is normal if all parts of the jet are normal. 607 template <typename T, int N> inline 608 bool IsNormal(const Jet<T, N>& f) { 609 if (!IsNormal(f.a)) { 610 return false; 611 } 612 for (int i = 0; i < N; ++i) { 613 if (!IsNormal(f.v[i])) { 614 return false; 615 } 616 } 617 return true; 618 } 619 620 // atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2) 621 // 622 // In words: the rate of change of theta is 1/r times the rate of 623 // change of (x, y) in the positive angular direction. 624 template <typename T, int N> inline 625 Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) { 626 // Note order of arguments: 627 // 628 // f = a + da 629 // g = b + db 630 631 Jet<T, N> out; 632 633 out.a = atan2(g.a, f.a); 634 635 T const temp = T(1.0) / (f.a * f.a + g.a * g.a); 636 out.v = temp * (- g.a * f.v + f.a * g.v); 637 return out; 638 } 639 640 641 // pow -- base is a differentiatble function, exponent is a constant. 642 // (a+da)^p ~= a^p + p*a^(p-1) da 643 template <typename T, int N> inline 644 Jet<T, N> pow(const Jet<T, N>& f, double g) { 645 Jet<T, N> out; 646 out.a = pow(f.a, g); 647 T const temp = g * pow(f.a, g - T(1.0)); 648 out.v = temp * f.v; 649 return out; 650 } 651 652 // pow -- base is a constant, exponent is a differentiable function. 653 // (a)^(p+dp) ~= a^p + a^p log(a) dp 654 template <typename T, int N> inline 655 Jet<T, N> pow(double f, const Jet<T, N>& g) { 656 Jet<T, N> out; 657 out.a = pow(f, g.a); 658 T const temp = log(f) * out.a; 659 out.v = temp * g.v; 660 return out; 661 } 662 663 664 // pow -- both base and exponent are differentiable functions. 665 // (a+da)^(b+db) ~= a^b + b * a^(b-1) da + a^b log(a) * db 666 template <typename T, int N> inline 667 Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) { 668 Jet<T, N> out; 669 670 T const temp1 = pow(f.a, g.a); 671 T const temp2 = g.a * pow(f.a, g.a - T(1.0)); 672 T const temp3 = temp1 * log(f.a); 673 674 out.a = temp1; 675 out.v = temp2 * f.v + temp3 * g.v; 676 return out; 677 } 678 679 // Define the helper functions Eigen needs to embed Jet types. 680 // 681 // NOTE(keir): machine_epsilon() and precision() are missing, because they don't 682 // work with nested template types (e.g. where the scalar is itself templated). 683 // Among other things, this means that decompositions of Jet's does not work, 684 // for example 685 // 686 // Matrix<Jet<T, N> ... > A, x, b; 687 // ... 688 // A.solve(b, &x) 689 // 690 // does not work and will fail with a strange compiler error. 691 // 692 // TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we 693 // switch to 3.0, also add the rest of the specialization functionality. 694 template<typename T, int N> inline const Jet<T, N>& ei_conj(const Jet<T, N>& x) { return x; } // NOLINT 695 template<typename T, int N> inline const Jet<T, N>& ei_real(const Jet<T, N>& x) { return x; } // NOLINT 696 template<typename T, int N> inline Jet<T, N> ei_imag(const Jet<T, N>& ) { return Jet<T, N>(0.0); } // NOLINT 697 template<typename T, int N> inline Jet<T, N> ei_abs (const Jet<T, N>& x) { return fabs(x); } // NOLINT 698 template<typename T, int N> inline Jet<T, N> ei_abs2(const Jet<T, N>& x) { return x * x; } // NOLINT 699 template<typename T, int N> inline Jet<T, N> ei_sqrt(const Jet<T, N>& x) { return sqrt(x); } // NOLINT 700 template<typename T, int N> inline Jet<T, N> ei_exp (const Jet<T, N>& x) { return exp(x); } // NOLINT 701 template<typename T, int N> inline Jet<T, N> ei_log (const Jet<T, N>& x) { return log(x); } // NOLINT 702 template<typename T, int N> inline Jet<T, N> ei_sin (const Jet<T, N>& x) { return sin(x); } // NOLINT 703 template<typename T, int N> inline Jet<T, N> ei_cos (const Jet<T, N>& x) { return cos(x); } // NOLINT 704 template<typename T, int N> inline Jet<T, N> ei_tan (const Jet<T, N>& x) { return tan(x); } // NOLINT 705 template<typename T, int N> inline Jet<T, N> ei_atan(const Jet<T, N>& x) { return atan(x); } // NOLINT 706 template<typename T, int N> inline Jet<T, N> ei_sinh(const Jet<T, N>& x) { return sinh(x); } // NOLINT 707 template<typename T, int N> inline Jet<T, N> ei_cosh(const Jet<T, N>& x) { return cosh(x); } // NOLINT 708 template<typename T, int N> inline Jet<T, N> ei_tanh(const Jet<T, N>& x) { return tanh(x); } // NOLINT 709 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 710 711 // Note: This has to be in the ceres namespace for argument dependent lookup to 712 // function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with 713 // strange compile errors. 714 template <typename T, int N> 715 inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) { 716 return s << "[" << z.a << " ; " << z.v.transpose() << "]"; 717 } 718 719 } // namespace ceres 720 721 namespace Eigen { 722 723 // Creating a specialization of NumTraits enables placing Jet objects inside 724 // Eigen arrays, getting all the goodness of Eigen combined with autodiff. 725 template<typename T, int N> 726 struct NumTraits<ceres::Jet<T, N> > { 727 typedef ceres::Jet<T, N> Real; 728 typedef ceres::Jet<T, N> NonInteger; 729 typedef ceres::Jet<T, N> Nested; 730 731 static typename ceres::Jet<T, N> dummy_precision() { 732 return ceres::Jet<T, N>(1e-12); 733 } 734 735 enum { 736 IsComplex = 0, 737 IsInteger = 0, 738 IsSigned, 739 ReadCost = 1, 740 AddCost = 1, 741 // For Jet types, multiplication is more expensive than addition. 742 MulCost = 3, 743 HasFloatingPoint = 1 744 }; 745 }; 746 747 } // namespace Eigen 748 749 #endif // CERES_PUBLIC_JET_H_ 750