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 // LOG(INFO) << "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 // Constructor from scalar and vector part 196 // The use of Eigen::DenseBase allows Eigen expressions 197 // to be passed in without being fully evaluated until 198 // they are assigned to v 199 template<typename Derived> 200 Jet(const T& value, const Eigen::DenseBase<Derived> &vIn) 201 : a(value), 202 v(vIn) 203 { 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 // Note the Eigen::DontAlign bit is needed here because this object 233 // gets allocated on the stack and as part of other arrays and 234 // structs. Forcing the right alignment there is the source of much 235 // pain and suffering. Even if that works, passing Jets around to 236 // functions by value has problems because the C++ ABI does not 237 // guarantee alignment for function arguments. 238 // 239 // Setting the DontAlign bit prevents Eigen from using SSE for the 240 // various operations on Jets. This is a small performance penalty 241 // since the AutoDiff code will still expose much of the code as 242 // statically sized loops to the compiler. But given the subtle 243 // issues that arise due to alignment, especially when dealing with 244 // multiple platforms, it seems to be a trade off worth making. 245 Eigen::Matrix<T, N, 1, Eigen::DontAlign> v; 246 }; 247 248 // Unary + 249 template<typename T, int N> inline 250 Jet<T, N> const& operator+(const Jet<T, N>& f) { 251 return f; 252 } 253 254 // TODO(keir): Try adding __attribute__((always_inline)) to these functions to 255 // see if it causes a performance increase. 256 257 // Unary - 258 template<typename T, int N> inline 259 Jet<T, N> operator-(const Jet<T, N>&f) { 260 return Jet<T, N>(-f.a, -f.v); 261 } 262 263 // Binary + 264 template<typename T, int N> inline 265 Jet<T, N> operator+(const Jet<T, N>& f, 266 const Jet<T, N>& g) { 267 return Jet<T, N>(f.a + g.a, f.v + g.v); 268 } 269 270 // Binary + with a scalar: x + s 271 template<typename T, int N> inline 272 Jet<T, N> operator+(const Jet<T, N>& f, T s) { 273 return Jet<T, N>(f.a + s, f.v); 274 } 275 276 // Binary + with a scalar: s + x 277 template<typename T, int N> inline 278 Jet<T, N> operator+(T s, const Jet<T, N>& f) { 279 return Jet<T, N>(f.a + s, f.v); 280 } 281 282 // Binary - 283 template<typename T, int N> inline 284 Jet<T, N> operator-(const Jet<T, N>& f, 285 const Jet<T, N>& g) { 286 return Jet<T, N>(f.a - g.a, f.v - g.v); 287 } 288 289 // Binary - with a scalar: x - s 290 template<typename T, int N> inline 291 Jet<T, N> operator-(const Jet<T, N>& f, T s) { 292 return Jet<T, N>(f.a - s, f.v); 293 } 294 295 // Binary - with a scalar: s - x 296 template<typename T, int N> inline 297 Jet<T, N> operator-(T s, const Jet<T, N>& f) { 298 return Jet<T, N>(s - f.a, -f.v); 299 } 300 301 // Binary * 302 template<typename T, int N> inline 303 Jet<T, N> operator*(const Jet<T, N>& f, 304 const Jet<T, N>& g) { 305 return Jet<T, N>(f.a * g.a, f.a * g.v + f.v * g.a); 306 } 307 308 // Binary * with a scalar: x * s 309 template<typename T, int N> inline 310 Jet<T, N> operator*(const Jet<T, N>& f, T s) { 311 return Jet<T, N>(f.a * s, f.v * s); 312 } 313 314 // Binary * with a scalar: s * x 315 template<typename T, int N> inline 316 Jet<T, N> operator*(T s, const Jet<T, N>& f) { 317 return Jet<T, N>(f.a * s, f.v * s); 318 } 319 320 // Binary / 321 template<typename T, int N> inline 322 Jet<T, N> operator/(const Jet<T, N>& f, 323 const Jet<T, N>& g) { 324 // This uses: 325 // 326 // a + u (a + u)(b - v) (a + u)(b - v) 327 // ----- = -------------- = -------------- 328 // b + v (b + v)(b - v) b^2 329 // 330 // which holds because v*v = 0. 331 const T g_a_inverse = T(1.0) / g.a; 332 const T f_a_by_g_a = f.a * g_a_inverse; 333 return Jet<T, N>(f.a * g_a_inverse, (f.v - f_a_by_g_a * g.v) * g_a_inverse); 334 } 335 336 // Binary / with a scalar: s / x 337 template<typename T, int N> inline 338 Jet<T, N> operator/(T s, const Jet<T, N>& g) { 339 const T minus_s_g_a_inverse2 = -s / (g.a * g.a); 340 return Jet<T, N>(s / g.a, g.v * minus_s_g_a_inverse2); 341 } 342 343 // Binary / with a scalar: x / s 344 template<typename T, int N> inline 345 Jet<T, N> operator/(const Jet<T, N>& f, T s) { 346 const T s_inverse = 1.0 / s; 347 return Jet<T, N>(f.a * s_inverse, f.v * s_inverse); 348 } 349 350 // Binary comparison operators for both scalars and jets. 351 #define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ 352 template<typename T, int N> inline \ 353 bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \ 354 return f.a op g.a; \ 355 } \ 356 template<typename T, int N> inline \ 357 bool operator op(const T& s, const Jet<T, N>& g) { \ 358 return s op g.a; \ 359 } \ 360 template<typename T, int N> inline \ 361 bool operator op(const Jet<T, N>& f, const T& s) { \ 362 return f.a op s; \ 363 } 364 CERES_DEFINE_JET_COMPARISON_OPERATOR( < ) // NOLINT 365 CERES_DEFINE_JET_COMPARISON_OPERATOR( <= ) // NOLINT 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 #undef CERES_DEFINE_JET_COMPARISON_OPERATOR 371 372 // Pull some functions from namespace std. 373 // 374 // This is necessary because we want to use the same name (e.g. 'sqrt') for 375 // double-valued and Jet-valued functions, but we are not allowed to put 376 // Jet-valued functions inside namespace std. 377 // 378 // TODO(keir): Switch to "using". 379 inline double abs (double x) { return std::abs(x); } 380 inline double log (double x) { return std::log(x); } 381 inline double exp (double x) { return std::exp(x); } 382 inline double sqrt (double x) { return std::sqrt(x); } 383 inline double cos (double x) { return std::cos(x); } 384 inline double acos (double x) { return std::acos(x); } 385 inline double sin (double x) { return std::sin(x); } 386 inline double asin (double x) { return std::asin(x); } 387 inline double tan (double x) { return std::tan(x); } 388 inline double atan (double x) { return std::atan(x); } 389 inline double sinh (double x) { return std::sinh(x); } 390 inline double cosh (double x) { return std::cosh(x); } 391 inline double tanh (double x) { return std::tanh(x); } 392 inline double pow (double x, double y) { return std::pow(x, y); } 393 inline double atan2(double y, double x) { return std::atan2(y, x); } 394 395 // In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule. 396 397 // abs(x + h) ~= x + h or -(x + h) 398 template <typename T, int N> inline 399 Jet<T, N> abs(const Jet<T, N>& f) { 400 return f.a < T(0.0) ? -f : f; 401 } 402 403 // log(a + h) ~= log(a) + h / a 404 template <typename T, int N> inline 405 Jet<T, N> log(const Jet<T, N>& f) { 406 const T a_inverse = T(1.0) / f.a; 407 return Jet<T, N>(log(f.a), f.v * a_inverse); 408 } 409 410 // exp(a + h) ~= exp(a) + exp(a) h 411 template <typename T, int N> inline 412 Jet<T, N> exp(const Jet<T, N>& f) { 413 const T tmp = exp(f.a); 414 return Jet<T, N>(tmp, tmp * f.v); 415 } 416 417 // sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a)) 418 template <typename T, int N> inline 419 Jet<T, N> sqrt(const Jet<T, N>& f) { 420 const T tmp = sqrt(f.a); 421 const T two_a_inverse = T(1.0) / (T(2.0) * tmp); 422 return Jet<T, N>(tmp, f.v * two_a_inverse); 423 } 424 425 // cos(a + h) ~= cos(a) - sin(a) h 426 template <typename T, int N> inline 427 Jet<T, N> cos(const Jet<T, N>& f) { 428 return Jet<T, N>(cos(f.a), - sin(f.a) * f.v); 429 } 430 431 // acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h 432 template <typename T, int N> inline 433 Jet<T, N> acos(const Jet<T, N>& f) { 434 const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a); 435 return Jet<T, N>(acos(f.a), tmp * f.v); 436 } 437 438 // sin(a + h) ~= sin(a) + cos(a) h 439 template <typename T, int N> inline 440 Jet<T, N> sin(const Jet<T, N>& f) { 441 return Jet<T, N>(sin(f.a), cos(f.a) * f.v); 442 } 443 444 // asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h 445 template <typename T, int N> inline 446 Jet<T, N> asin(const Jet<T, N>& f) { 447 const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a); 448 return Jet<T, N>(asin(f.a), tmp * f.v); 449 } 450 451 // tan(a + h) ~= tan(a) + (1 + tan(a)^2) h 452 template <typename T, int N> inline 453 Jet<T, N> tan(const Jet<T, N>& f) { 454 const T tan_a = tan(f.a); 455 const T tmp = T(1.0) + tan_a * tan_a; 456 return Jet<T, N>(tan_a, tmp * f.v); 457 } 458 459 // atan(a + h) ~= atan(a) + 1 / (1 + a^2) h 460 template <typename T, int N> inline 461 Jet<T, N> atan(const Jet<T, N>& f) { 462 const T tmp = T(1.0) / (T(1.0) + f.a * f.a); 463 return Jet<T, N>(atan(f.a), tmp * f.v); 464 } 465 466 // sinh(a + h) ~= sinh(a) + cosh(a) h 467 template <typename T, int N> inline 468 Jet<T, N> sinh(const Jet<T, N>& f) { 469 return Jet<T, N>(sinh(f.a), cosh(f.a) * f.v); 470 } 471 472 // cosh(a + h) ~= cosh(a) + sinh(a) h 473 template <typename T, int N> inline 474 Jet<T, N> cosh(const Jet<T, N>& f) { 475 return Jet<T, N>(cosh(f.a), sinh(f.a) * f.v); 476 } 477 478 // tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h 479 template <typename T, int N> inline 480 Jet<T, N> tanh(const Jet<T, N>& f) { 481 const T tanh_a = tanh(f.a); 482 const T tmp = T(1.0) - tanh_a * tanh_a; 483 return Jet<T, N>(tanh_a, tmp * f.v); 484 } 485 486 // Jet Classification. It is not clear what the appropriate semantics are for 487 // these classifications. This picks that IsFinite and isnormal are "all" 488 // operations, i.e. all elements of the jet must be finite for the jet itself 489 // to be finite (or normal). For IsNaN and IsInfinite, the answer is less 490 // clear. This takes a "any" approach for IsNaN and IsInfinite such that if any 491 // part of a jet is nan or inf, then the entire jet is nan or inf. This leads 492 // to strange situations like a jet can be both IsInfinite and IsNaN, but in 493 // practice the "any" semantics are the most useful for e.g. checking that 494 // derivatives are sane. 495 496 // The jet is finite if all parts of the jet are finite. 497 template <typename T, int N> inline 498 bool IsFinite(const Jet<T, N>& f) { 499 if (!IsFinite(f.a)) { 500 return false; 501 } 502 for (int i = 0; i < N; ++i) { 503 if (!IsFinite(f.v[i])) { 504 return false; 505 } 506 } 507 return true; 508 } 509 510 // The jet is infinite if any part of the jet is infinite. 511 template <typename T, int N> inline 512 bool IsInfinite(const Jet<T, N>& f) { 513 if (IsInfinite(f.a)) { 514 return true; 515 } 516 for (int i = 0; i < N; i++) { 517 if (IsInfinite(f.v[i])) { 518 return true; 519 } 520 } 521 return false; 522 } 523 524 // The jet is NaN if any part of the jet is NaN. 525 template <typename T, int N> inline 526 bool IsNaN(const Jet<T, N>& f) { 527 if (IsNaN(f.a)) { 528 return true; 529 } 530 for (int i = 0; i < N; ++i) { 531 if (IsNaN(f.v[i])) { 532 return true; 533 } 534 } 535 return false; 536 } 537 538 // The jet is normal if all parts of the jet are normal. 539 template <typename T, int N> inline 540 bool IsNormal(const Jet<T, N>& f) { 541 if (!IsNormal(f.a)) { 542 return false; 543 } 544 for (int i = 0; i < N; ++i) { 545 if (!IsNormal(f.v[i])) { 546 return false; 547 } 548 } 549 return true; 550 } 551 552 // atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2) 553 // 554 // In words: the rate of change of theta is 1/r times the rate of 555 // change of (x, y) in the positive angular direction. 556 template <typename T, int N> inline 557 Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) { 558 // Note order of arguments: 559 // 560 // f = a + da 561 // g = b + db 562 563 T const tmp = T(1.0) / (f.a * f.a + g.a * g.a); 564 return Jet<T, N>(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v)); 565 } 566 567 568 // pow -- base is a differentiable function, exponent is a constant. 569 // (a+da)^p ~= a^p + p*a^(p-1) da 570 template <typename T, int N> inline 571 Jet<T, N> pow(const Jet<T, N>& f, double g) { 572 T const tmp = g * pow(f.a, g - T(1.0)); 573 return Jet<T, N>(pow(f.a, g), tmp * f.v); 574 } 575 576 // pow -- base is a constant, exponent is a differentiable function. 577 // (a)^(p+dp) ~= a^p + a^p log(a) dp 578 template <typename T, int N> inline 579 Jet<T, N> pow(double f, const Jet<T, N>& g) { 580 T const tmp = pow(f, g.a); 581 return Jet<T, N>(tmp, log(f) * tmp * g.v); 582 } 583 584 585 // pow -- both base and exponent are differentiable functions. 586 // (a+da)^(b+db) ~= a^b + b * a^(b-1) da + a^b log(a) * db 587 template <typename T, int N> inline 588 Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) { 589 T const tmp1 = pow(f.a, g.a); 590 T const tmp2 = g.a * pow(f.a, g.a - T(1.0)); 591 T const tmp3 = tmp1 * log(f.a); 592 593 return Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v); 594 } 595 596 // Define the helper functions Eigen needs to embed Jet types. 597 // 598 // NOTE(keir): machine_epsilon() and precision() are missing, because they don't 599 // work with nested template types (e.g. where the scalar is itself templated). 600 // Among other things, this means that decompositions of Jet's does not work, 601 // for example 602 // 603 // Matrix<Jet<T, N> ... > A, x, b; 604 // ... 605 // A.solve(b, &x) 606 // 607 // does not work and will fail with a strange compiler error. 608 // 609 // TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we 610 // switch to 3.0, also add the rest of the specialization functionality. 611 template<typename T, int N> inline const Jet<T, N>& ei_conj(const Jet<T, N>& x) { return x; } // NOLINT 612 template<typename T, int N> inline const Jet<T, N>& ei_real(const Jet<T, N>& x) { return x; } // NOLINT 613 template<typename T, int N> inline Jet<T, N> ei_imag(const Jet<T, N>& ) { return Jet<T, N>(0.0); } // NOLINT 614 template<typename T, int N> inline Jet<T, N> ei_abs (const Jet<T, N>& x) { return fabs(x); } // NOLINT 615 template<typename T, int N> inline Jet<T, N> ei_abs2(const Jet<T, N>& x) { return x * x; } // NOLINT 616 template<typename T, int N> inline Jet<T, N> ei_sqrt(const Jet<T, N>& x) { return sqrt(x); } // NOLINT 617 template<typename T, int N> inline Jet<T, N> ei_exp (const Jet<T, N>& x) { return exp(x); } // NOLINT 618 template<typename T, int N> inline Jet<T, N> ei_log (const Jet<T, N>& x) { return log(x); } // NOLINT 619 template<typename T, int N> inline Jet<T, N> ei_sin (const Jet<T, N>& x) { return sin(x); } // NOLINT 620 template<typename T, int N> inline Jet<T, N> ei_cos (const Jet<T, N>& x) { return cos(x); } // NOLINT 621 template<typename T, int N> inline Jet<T, N> ei_tan (const Jet<T, N>& x) { return tan(x); } // NOLINT 622 template<typename T, int N> inline Jet<T, N> ei_atan(const Jet<T, N>& x) { return atan(x); } // NOLINT 623 template<typename T, int N> inline Jet<T, N> ei_sinh(const Jet<T, N>& x) { return sinh(x); } // NOLINT 624 template<typename T, int N> inline Jet<T, N> ei_cosh(const Jet<T, N>& x) { return cosh(x); } // NOLINT 625 template<typename T, int N> inline Jet<T, N> ei_tanh(const Jet<T, N>& x) { return tanh(x); } // NOLINT 626 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 627 628 // Note: This has to be in the ceres namespace for argument dependent lookup to 629 // function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with 630 // strange compile errors. 631 template <typename T, int N> 632 inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) { 633 return s << "[" << z.a << " ; " << z.v.transpose() << "]"; 634 } 635 636 } // namespace ceres 637 638 namespace Eigen { 639 640 // Creating a specialization of NumTraits enables placing Jet objects inside 641 // Eigen arrays, getting all the goodness of Eigen combined with autodiff. 642 template<typename T, int N> 643 struct NumTraits<ceres::Jet<T, N> > { 644 typedef ceres::Jet<T, N> Real; 645 typedef ceres::Jet<T, N> NonInteger; 646 typedef ceres::Jet<T, N> Nested; 647 648 static typename ceres::Jet<T, N> dummy_precision() { 649 return ceres::Jet<T, N>(1e-12); 650 } 651 652 static inline Real epsilon() { return Real(std::numeric_limits<T>::epsilon()); } 653 654 enum { 655 IsComplex = 0, 656 IsInteger = 0, 657 IsSigned, 658 ReadCost = 1, 659 AddCost = 1, 660 // For Jet types, multiplication is more expensive than addition. 661 MulCost = 3, 662 HasFloatingPoint = 1, 663 RequireInitialization = 1 664 }; 665 }; 666 667 } // namespace Eigen 668 669 #endif // CERES_PUBLIC_JET_H_ 670