jet.h revision 79397c21138f54fcff6ec067b44b847f1f7e0e98
10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: keir@google.com (Keir Mierle) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// A simple implementation of N-dimensional dual numbers, for automatically 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// computing exact derivatives of functions. 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// While a complete treatment of the mechanics of automatic differentation is 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// beyond the scope of this header (see 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://en.wikipedia.org/wiki/Automatic_differentiation for details), the 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// basic idea is to extend normal arithmetic with an extra element, "e," often 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// denoted with the greek symbol epsilon, such that e != 0 but e^2 = 0. Dual 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// numbers are extensions of the real numbers analogous to complex numbers: 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// whereas complex numbers augment the reals by introducing an imaginary unit i 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// such that i^2 = -1, dual numbers introduce an "infinitesimal" unit e such 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// that e^2 = 0. Dual numbers have two components: the "real" component and the 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// "infinitesimal" component, generally written as x + y*e. Surprisingly, this 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// leads to a convenient method for computing exact derivatives without needing 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// to manipulate complicated symbolic expressions. 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// For example, consider the function 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// f(x) = x^2 , 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20. 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Next, augument 10 with an infinitesimal to get: 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// f(10 + e) = (10 + e)^2 550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// = 100 + 2 * 10 * e + e^2 560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// = 100 + 20 * e -+- 570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// -- | 580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// | +--- This is zero, since e^2 = 0 590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// | 600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// +----------------- This is df/dx! 610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Note that the derivative of f with respect to x is simply the infinitesimal 630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// component of the value of f(x + e). So, in order to take the derivative of 640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// any function, it is only necessary to replace the numeric "object" used in 650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// the function with one extended with infinitesimals. The class Jet, defined in 660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this header, is one such example of this, where substitution is done with 670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// templates. 680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// To handle derivatives of functions taking multiple arguments, different 700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// infinitesimals are used, one for each variable to take the derivative of. For 710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// example, consider a scalar function of two scalar parameters x and y: 720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// f(x, y) = x^2 + x * y 740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Following the technique above, to compute the derivatives df/dx and df/dy for 760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// f(1, 3) involves doing two evaluations of f, the first time replacing x with 770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// x + e, the second time replacing y with y + e. 780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// For df/dx: 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// f(1 + e, y) = (1 + e)^2 + (1 + e) * 3 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// = 1 + 2 * e + 3 + 3 * e 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// = 4 + 5 * e 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// --> df/dx = 5 860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// For df/dy: 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// f(1, 3 + e) = 1^2 + 1 * (3 + e) 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// = 1 + 3 + e 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// = 4 + e 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// --> df/dy = 1 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// To take the gradient of f with the implementation of dual numbers ("jets") in 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this file, it is necessary to create a single jet type which has components 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// for the derivative in x and y, and passing them to a templated version of f: 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// template<typename T> 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// T f(const T &x, const T &y) { 1010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// return x * x + x * y; 1020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// } 1030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// // The "2" means there should be 2 dual number components. 1050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Jet<double, 2> x(0); // Pick the 0th dual number for x. 1060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Jet<double, 2> y(1); // Pick the 1st dual number for y. 1070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Jet<double, 2> z = f(x, y); 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 10979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// LOG(INFO) << "df/dx = " << z.a[0] 11079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// << "df/dy = " << z.a[1]; 1110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Most users should not use Jet objects directly; a wrapper around Jet objects, 1130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// which makes computing the derivative, gradient, or jacobian of templated 1140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// functors simple, is in autodiff.h. Even autodiff.h should not be used 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// directly; instead autodiff_cost_function.h is typically the file of interest. 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// For the more mathematically inclined, this file implements first-order 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// "jets". A 1st order jet is an element of the ring 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// which essentially means that each jet consists of a "scalar" value 'a' from T 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and a 1st order perturbation vector 'v' of length N: 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// x = a + \sum_i v[i] t_i 1260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// A shorthand is to write an element as x = a + u, where u is the pertubation. 1280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Then, the main point about the arithmetic of jets is that the product of 1290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// perturbations is zero: 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// (a + u) * (b + v) = ab + av + bu + uv 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// = ab + (av + bu) + 0 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// which is what operator* implements below. Addition is simpler: 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// (a + u) + (b + v) = (a + b) + (u + v). 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The only remaining question is how to evaluate the function of a jet, for 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// which we use the chain rule: 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// f(a + u) = f(a) + f'(a) u 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// where f'(a) is the (scalar) derivative of f at a. 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// By pushing these things through sufficiently and suitably templated 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// functions, we can do automatic differentiation. Just be sure to turn on 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// function inlining and common-subexpression elimination, or it will be very 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// slow! 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// WARNING: Most Ceres users should not directly include this file or know the 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// details of how jets work. Instead the suggested method for automatic 1520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// derivatives is to use autodiff_cost_function.h, which is a wrapper around 1530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// both jets.h and autodiff.h to make taking derivatives of cost functions for 1540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// use in Ceres easier. 1550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_PUBLIC_JET_H_ 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#define CERES_PUBLIC_JET_H_ 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cmath> 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <iosfwd> 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <iostream> // NOLINT 1620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <string> 1630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "Eigen/Core" 1650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "ceres/fpclassify.h" 1660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstruct Jet { 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong enum { DIMENSION = N }; 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Default-construct "a" because otherwise this can lead to false errors about 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // uninitialized uses when other classes relying on default constructed T 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // (where T is a Jet<T, N>). This usually only happens in opt mode. Note that 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the C++ standard mandates that e.g. default constructed doubles are 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // initialized to 0.0; see sections 8.5 of the C++03 standard. 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Jet() : a() { 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong v.setZero(); 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Constructor from scalar: a + 0. 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong explicit Jet(const T& value) { 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong a = value; 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong v.setZero(); 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Constructor from scalar plus variable: a + t_i. 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Jet(const T& value, int k) { 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong a = value; 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong v.setZero(); 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong v[k] = T(1.0); 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 19579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Constructor from scalar and vector part 19679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // The use of Eigen::DenseBase allows Eigen expressions 19779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // to be passed in without being fully evaluated until 19879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // they are assigned to v 19979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez template<typename Derived> 20079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez Jet(const T& value, const Eigen::DenseBase<Derived> &vIn) 20179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez : a(value), 20279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez v(vIn) 20379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez { 20479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez } 20579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 2060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Compound operators 2070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Jet<T, N>& operator+=(const Jet<T, N> &y) { 2080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *this = *this + y; 2090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return *this; 2100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Jet<T, N>& operator-=(const Jet<T, N> &y) { 2130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *this = *this - y; 2140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return *this; 2150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Jet<T, N>& operator*=(const Jet<T, N> &y) { 2180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *this = *this * y; 2190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return *this; 2200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Jet<T, N>& operator/=(const Jet<T, N> &y) { 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong *this = *this / y; 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return *this; 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The scalar part. 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T a; 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // The infinitesimal part. 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Note the Eigen::DontAlign bit is needed here because this object 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // gets allocated on the stack and as part of other arrays and 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // structs. Forcing the right alignment there is the source of much 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // pain and suffering. Even if that works, passing Jets around to 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // functions by value has problems because the C++ ABI does not 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // guarantee alignment for function arguments. 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Setting the DontAlign bit prevents Eigen from using SSE for the 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // various operations on Jets. This is a small performance penalty 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // since the AutoDiff code will still expose much of the code as 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // statically sized loops to the compiler. But given the subtle 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // issues that arise due to alignment, especially when dealing with 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // multiple platforms, it seems to be a trade off worth making. 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong Eigen::Matrix<T, N, 1, Eigen::DontAlign> v; 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Unary + 2490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> const& operator+(const Jet<T, N>& f) { 2510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return f; 2520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// TODO(keir): Try adding __attribute__((always_inline)) to these functions to 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// see if it causes a performance increase. 2560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Unary - 2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline 2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> operator-(const Jet<T, N>&f) { 26079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(-f.a, -f.v); 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Binary + 2640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline 2650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> operator+(const Jet<T, N>& f, 2660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Jet<T, N>& g) { 26779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(f.a + g.a, f.v + g.v); 2680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Binary + with a scalar: x + s 2710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline 2720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> operator+(const Jet<T, N>& f, T s) { 27379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(f.a + s, f.v); 2740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Binary + with a scalar: s + x 2770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline 2780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> operator+(T s, const Jet<T, N>& f) { 27979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(f.a + s, f.v); 2800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Binary - 2830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline 2840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> operator-(const Jet<T, N>& f, 2850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Jet<T, N>& g) { 28679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(f.a - g.a, f.v - g.v); 2870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Binary - with a scalar: x - s 2900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline 2910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> operator-(const Jet<T, N>& f, T s) { 29279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(f.a - s, f.v); 2930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Binary - with a scalar: s - x 2960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline 2970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> operator-(T s, const Jet<T, N>& f) { 29879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(s - f.a, -f.v); 2990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Binary * 3020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline 3030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> operator*(const Jet<T, N>& f, 3040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Jet<T, N>& g) { 30579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(f.a * g.a, f.a * g.v + f.v * g.a); 3060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Binary * with a scalar: x * s 3090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline 3100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> operator*(const Jet<T, N>& f, T s) { 31179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(f.a * s, f.v * s); 3120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Binary * with a scalar: s * x 3150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline 3160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> operator*(T s, const Jet<T, N>& f) { 31779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(f.a * s, f.v * s); 3180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Binary / 3210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline 3220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> operator/(const Jet<T, N>& f, 3230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const Jet<T, N>& g) { 3240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // This uses: 3250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 3260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // a + u (a + u)(b - v) (a + u)(b - v) 3270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // ----- = -------------- = -------------- 3280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // b + v (b + v)(b - v) b^2 3290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 3300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // which holds because v*v = 0. 3311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T g_a_inverse = T(1.0) / g.a; 3321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T f_a_by_g_a = f.a * g_a_inverse; 33379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(f.a * g_a_inverse, (f.v - f_a_by_g_a * g.v) * g_a_inverse); 3340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Binary / with a scalar: s / x 3370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline 3380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> operator/(T s, const Jet<T, N>& g) { 3391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T minus_s_g_a_inverse2 = -s / (g.a * g.a); 34079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(s / g.a, g.v * minus_s_g_a_inverse2); 3410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Binary / with a scalar: x / s 3440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline 3450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> operator/(const Jet<T, N>& f, T s) { 3461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T s_inverse = 1.0 / s; 34779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(f.a * s_inverse, f.v * s_inverse); 3480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Binary comparison operators for both scalars and jets. 3510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ 3520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline \ 3530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \ 3540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return f.a op g.a; \ 3550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} \ 3560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline \ 3570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool operator op(const T& s, const Jet<T, N>& g) { \ 3580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return s op g.a; \ 3590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} \ 3600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline \ 3610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool operator op(const Jet<T, N>& f, const T& s) { \ 3620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return f.a op s; \ 3630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongCERES_DEFINE_JET_COMPARISON_OPERATOR( < ) // NOLINT 3650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongCERES_DEFINE_JET_COMPARISON_OPERATOR( <= ) // NOLINT 3660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongCERES_DEFINE_JET_COMPARISON_OPERATOR( > ) // NOLINT 3670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongCERES_DEFINE_JET_COMPARISON_OPERATOR( >= ) // NOLINT 3680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongCERES_DEFINE_JET_COMPARISON_OPERATOR( == ) // NOLINT 3690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongCERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT 3700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#undef CERES_DEFINE_JET_COMPARISON_OPERATOR 3710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Pull some functions from namespace std. 3730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 3740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// This is necessary because we want to use the same name (e.g. 'sqrt') for 3750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// double-valued and Jet-valued functions, but we are not allowed to put 3760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Jet-valued functions inside namespace std. 3770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 3780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// TODO(keir): Switch to "using". 3790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline double abs (double x) { return std::abs(x); } 3800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline double log (double x) { return std::log(x); } 3810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline double exp (double x) { return std::exp(x); } 3820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline double sqrt (double x) { return std::sqrt(x); } 3830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline double cos (double x) { return std::cos(x); } 3840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline double acos (double x) { return std::acos(x); } 3850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline double sin (double x) { return std::sin(x); } 3860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline double asin (double x) { return std::asin(x); } 3871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlinginline double tan (double x) { return std::tan(x); } 3881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlinginline double atan (double x) { return std::atan(x); } 3891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlinginline double sinh (double x) { return std::sinh(x); } 3901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlinginline double cosh (double x) { return std::cosh(x); } 3911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlinginline double tanh (double x) { return std::tanh(x); } 3920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline double pow (double x, double y) { return std::pow(x, y); } 3930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline double atan2(double y, double x) { return std::atan2(y, x); } 3940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule. 3960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// abs(x + h) ~= x + h or -(x + h) 3980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 3990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> abs(const Jet<T, N>& f) { 4000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return f.a < T(0.0) ? -f : f; 4010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// log(a + h) ~= log(a) + h / a 4040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 4050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> log(const Jet<T, N>& f) { 4061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T a_inverse = T(1.0) / f.a; 40779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(log(f.a), f.v * a_inverse); 4080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// exp(a + h) ~= exp(a) + exp(a) h 4110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 4120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> exp(const Jet<T, N>& f) { 41379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const T tmp = exp(f.a); 41479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(tmp, tmp * f.v); 4150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a)) 4180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 4190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> sqrt(const Jet<T, N>& f) { 42079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const T tmp = sqrt(f.a); 42179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const T two_a_inverse = T(1.0) / (T(2.0) * tmp); 42279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(tmp, f.v * two_a_inverse); 4230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// cos(a + h) ~= cos(a) - sin(a) h 4260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 4270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> cos(const Jet<T, N>& f) { 42879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(cos(f.a), - sin(f.a) * f.v); 4290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h 4320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 4330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> acos(const Jet<T, N>& f) { 4341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a); 43579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(acos(f.a), tmp * f.v); 4360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// sin(a + h) ~= sin(a) + cos(a) h 4390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 4400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> sin(const Jet<T, N>& f) { 44179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(sin(f.a), cos(f.a) * f.v); 4420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h 4450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 4460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> asin(const Jet<T, N>& f) { 4471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a); 44879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(asin(f.a), tmp * f.v); 4491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// tan(a + h) ~= tan(a) + (1 + tan(a)^2) h 4521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int N> inline 4531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingJet<T, N> tan(const Jet<T, N>& f) { 45479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const T tan_a = tan(f.a); 4551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T tmp = T(1.0) + tan_a * tan_a; 45679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(tan_a, tmp * f.v); 4571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// atan(a + h) ~= atan(a) + 1 / (1 + a^2) h 4601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int N> inline 4611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingJet<T, N> atan(const Jet<T, N>& f) { 4621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T tmp = T(1.0) / (T(1.0) + f.a * f.a); 46379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(atan(f.a), tmp * f.v); 4641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// sinh(a + h) ~= sinh(a) + cosh(a) h 4671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int N> inline 4681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingJet<T, N> sinh(const Jet<T, N>& f) { 46979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(sinh(f.a), cosh(f.a) * f.v); 4701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// cosh(a + h) ~= cosh(a) + sinh(a) h 4731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int N> inline 4741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingJet<T, N> cosh(const Jet<T, N>& f) { 47579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(cosh(f.a), sinh(f.a) * f.v); 4761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h 4791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int N> inline 4801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingJet<T, N> tanh(const Jet<T, N>& f) { 48179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const T tanh_a = tanh(f.a); 4821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T tmp = T(1.0) - tanh_a * tanh_a; 48379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(tanh_a, tmp * f.v); 4840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Jet Classification. It is not clear what the appropriate semantics are for 4870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// these classifications. This picks that IsFinite and isnormal are "all" 4880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// operations, i.e. all elements of the jet must be finite for the jet itself 4890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// to be finite (or normal). For IsNaN and IsInfinite, the answer is less 4900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// clear. This takes a "any" approach for IsNaN and IsInfinite such that if any 4910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// part of a jet is nan or inf, then the entire jet is nan or inf. This leads 4920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// to strange situations like a jet can be both IsInfinite and IsNaN, but in 4930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// practice the "any" semantics are the most useful for e.g. checking that 4940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// derivatives are sane. 4950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The jet is finite if all parts of the jet are finite. 4970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 4980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool IsFinite(const Jet<T, N>& f) { 4990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!IsFinite(f.a)) { 5000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 5010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < N; ++i) { 5030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!IsFinite(f.v[i])) { 5040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 5050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 5080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The jet is infinite if any part of the jet is infinite. 5110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 5120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool IsInfinite(const Jet<T, N>& f) { 5130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (IsInfinite(f.a)) { 5140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 5150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < N; i++) { 5170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (IsInfinite(f.v[i])) { 5180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 5190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 5220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The jet is NaN if any part of the jet is NaN. 5250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 5260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool IsNaN(const Jet<T, N>& f) { 5270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (IsNaN(f.a)) { 5280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 5290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < N; ++i) { 5310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (IsNaN(f.v[i])) { 5320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 5330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 5360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The jet is normal if all parts of the jet are normal. 5390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 5400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongbool IsNormal(const Jet<T, N>& f) { 5410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!IsNormal(f.a)) { 5420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 5430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < N; ++i) { 5450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (!IsNormal(f.v[i])) { 5460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return false; 5470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return true; 5500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2) 5530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 5540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// In words: the rate of change of theta is 1/r times the rate of 5550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// change of (x, y) in the positive angular direction. 5560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 5570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) { 5580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Note order of arguments: 5590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 5600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // f = a + da 5610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // g = b + db 5620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 56379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez T const tmp = T(1.0) / (f.a * f.a + g.a * g.a); 56479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v)); 5650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 56879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez// pow -- base is a differentiable function, exponent is a constant. 5690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// (a+da)^p ~= a^p + p*a^(p-1) da 5700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 5710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> pow(const Jet<T, N>& f, double g) { 57279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez T const tmp = g * pow(f.a, g - T(1.0)); 57379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(pow(f.a, g), tmp * f.v); 5740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// pow -- base is a constant, exponent is a differentiable function. 5770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// (a)^(p+dp) ~= a^p + a^p log(a) dp 5780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 5790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> pow(double f, const Jet<T, N>& g) { 58079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez T const tmp = pow(f, g.a); 58179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(tmp, log(f) * tmp * g.v); 5820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// pow -- both base and exponent are differentiable functions. 5860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// (a+da)^(b+db) ~= a^b + b * a^(b-1) da + a^b log(a) * db 5870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> inline 5880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongJet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) { 58979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez T const tmp1 = pow(f.a, g.a); 59079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez T const tmp2 = g.a * pow(f.a, g.a - T(1.0)); 59179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez T const tmp3 = tmp1 * log(f.a); 5920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 59379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez return Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v); 5940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Define the helper functions Eigen needs to embed Jet types. 5970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 5980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// NOTE(keir): machine_epsilon() and precision() are missing, because they don't 5990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// work with nested template types (e.g. where the scalar is itself templated). 6000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Among other things, this means that decompositions of Jet's does not work, 6010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// for example 6020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 6030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Matrix<Jet<T, N> ... > A, x, b; 6040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ... 6050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// A.solve(b, &x) 6060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 6070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// does not work and will fail with a strange compiler error. 6080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 6090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we 6100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// switch to 3.0, also add the rest of the specialization functionality. 6110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline const Jet<T, N>& ei_conj(const Jet<T, N>& x) { return x; } // NOLINT 6120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline const Jet<T, N>& ei_real(const Jet<T, N>& x) { return x; } // NOLINT 6130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline Jet<T, N> ei_imag(const Jet<T, N>& ) { return Jet<T, N>(0.0); } // NOLINT 6140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline Jet<T, N> ei_abs (const Jet<T, N>& x) { return fabs(x); } // NOLINT 6150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline Jet<T, N> ei_abs2(const Jet<T, N>& x) { return x * x; } // NOLINT 6160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline Jet<T, N> ei_sqrt(const Jet<T, N>& x) { return sqrt(x); } // NOLINT 6170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline Jet<T, N> ei_exp (const Jet<T, N>& x) { return exp(x); } // NOLINT 6180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline Jet<T, N> ei_log (const Jet<T, N>& x) { return log(x); } // NOLINT 6190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline Jet<T, N> ei_sin (const Jet<T, N>& x) { return sin(x); } // NOLINT 6200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline Jet<T, N> ei_cos (const Jet<T, N>& x) { return cos(x); } // NOLINT 6211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate<typename T, int N> inline Jet<T, N> ei_tan (const Jet<T, N>& x) { return tan(x); } // NOLINT 6221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate<typename T, int N> inline Jet<T, N> ei_atan(const Jet<T, N>& x) { return atan(x); } // NOLINT 6231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate<typename T, int N> inline Jet<T, N> ei_sinh(const Jet<T, N>& x) { return sinh(x); } // NOLINT 6241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate<typename T, int N> inline Jet<T, N> ei_cosh(const Jet<T, N>& x) { return cosh(x); } // NOLINT 6251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate<typename T, int N> inline Jet<T, N> ei_tanh(const Jet<T, N>& x) { return tanh(x); } // NOLINT 6260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> inline Jet<T, N> ei_pow (const Jet<T, N>& x, Jet<T, N> y) { return pow(x, y); } // NOLINT 6270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Note: This has to be in the ceres namespace for argument dependent lookup to 6290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with 6300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// strange compile errors. 6310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T, int N> 6320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) { 6330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return s << "[" << z.a << " ; " << z.v.transpose() << "]"; 6340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 6350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 6370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace Eigen { 6390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Creating a specialization of NumTraits enables placing Jet objects inside 6410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Eigen arrays, getting all the goodness of Eigen combined with autodiff. 6420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T, int N> 6430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongstruct NumTraits<ceres::Jet<T, N> > { 6440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef ceres::Jet<T, N> Real; 6450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef ceres::Jet<T, N> NonInteger; 6460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong typedef ceres::Jet<T, N> Nested; 6470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static typename ceres::Jet<T, N> dummy_precision() { 6490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return ceres::Jet<T, N>(1e-12); 6500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 6510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 65279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez static inline Real epsilon() { return Real(std::numeric_limits<T>::epsilon()); } 65379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 6540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong enum { 6550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong IsComplex = 0, 6560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong IsInteger = 0, 6570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong IsSigned, 6580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ReadCost = 1, 6590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong AddCost = 1, 6600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // For Jet types, multiplication is more expensive than addition. 6610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong MulCost = 3, 66279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez HasFloatingPoint = 1, 66379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez RequireInitialization = 1 6640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 6650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong}; 6660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace Eigen 6680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_PUBLIC_JET_H_ 670