autodiff.h
Go to the documentation of this file.
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@google.com (Keir Mierle)
30 //
31 // Computation of the Jacobian matrix for vector-valued functions of multiple
32 // variables, using automatic differentiation based on the implementation of
33 // dual numbers in jet.h. Before reading the rest of this file, it is adivsable
34 // to read jet.h's header comment in detail.
35 //
36 // The helper wrapper AutoDiff::Differentiate() computes the jacobian of
37 // functors with templated operator() taking this form:
38 //
39 // struct F {
40 // template<typename T>
41 // bool operator()(const T *x, const T *y, ..., T *z) {
42 // // Compute z[] based on x[], y[], ...
43 // // return true if computation succeeded, false otherwise.
44 // }
45 // };
46 //
47 // All inputs and outputs may be vector-valued.
48 //
49 // To understand how jets are used to compute the jacobian, a
50 // picture may help. Consider a vector-valued function, F, returning 3
51 // dimensions and taking a vector-valued parameter of 4 dimensions:
52 //
53 // y x
54 // [ * ] F [ * ]
55 // [ * ] <--- [ * ]
56 // [ * ] [ * ]
57 // [ * ]
58 //
59 // Similar to the 2-parameter example for f described in jet.h, computing the
60 // jacobian dy/dx is done by substutiting a suitable jet object for x and all
61 // intermediate steps of the computation of F. Since x is has 4 dimensions, use
62 // a Jet<double, 4>.
63 //
64 // Before substituting a jet object for x, the dual components are set
65 // appropriately for each dimension of x:
66 //
67 // y x
68 // [ * | * * * * ] f [ * | 1 0 0 0 ] x0
69 // [ * | * * * * ] <--- [ * | 0 1 0 0 ] x1
70 // [ * | * * * * ] [ * | 0 0 1 0 ] x2
71 // ---+--- [ * | 0 0 0 1 ] x3
72 // | ^ ^ ^ ^
73 // dy/dx | | | +----- infinitesimal for x3
74 // | | +------- infinitesimal for x2
75 // | +--------- infinitesimal for x1
76 // +----------- infinitesimal for x0
77 //
78 // The reason to set the internal 4x4 submatrix to the identity is that we wish
79 // to take the derivative of y separately with respect to each dimension of x.
80 // Each column of the 4x4 identity is therefore for a single component of the
81 // independent variable x.
82 //
83 // Then the jacobian of the mapping, dy/dx, is the 3x4 sub-matrix of the
84 // extended y vector, indicated in the above diagram.
85 //
86 // Functors with multiple parameters
87 // ---------------------------------
88 // In practice, it is often convenient to use a function f of two or more
89 // vector-valued parameters, for example, x[3] and z[6]. Unfortunately, the jet
90 // framework is designed for a single-parameter vector-valued input. The wrapper
91 // in this file addresses this issue adding support for functions with one or
92 // more parameter vectors.
93 //
94 // To support multiple parameters, all the parameter vectors are concatenated
95 // into one and treated as a single parameter vector, except that since the
96 // functor expects different inputs, we need to construct the jets as if they
97 // were part of a single parameter vector. The extended jets are passed
98 // separately for each parameter.
99 //
100 // For example, consider a functor F taking two vector parameters, p[2] and
101 // q[3], and producing an output y[4]:
102 //
103 // struct F {
104 // template<typename T>
105 // bool operator()(const T *p, const T *q, T *z) {
106 // // ...
107 // }
108 // };
109 //
110 // In this case, the necessary jet type is Jet<double, 5>. Here is a
111 // visualization of the jet objects in this case:
112 //
113 // Dual components for p ----+
114 // |
115 // -+-
116 // y [ * | 1 0 | 0 0 0 ] --- p[0]
117 // [ * | 0 1 | 0 0 0 ] --- p[1]
118 // [ * | . . | + + + ] |
119 // [ * | . . | + + + ] v
120 // [ * | . . | + + + ] <--- F(p, q)
121 // [ * | . . | + + + ] ^
122 // ^^^ ^^^^^ |
123 // dy/dp dy/dq [ * | 0 0 | 1 0 0 ] --- q[0]
124 // [ * | 0 0 | 0 1 0 ] --- q[1]
125 // [ * | 0 0 | 0 0 1 ] --- q[2]
126 // --+--
127 // |
128 // Dual components for q --------------+
129 //
130 // where the 4x2 submatrix (marked with ".") and 4x3 submatrix (marked with "+"
131 // of y in the above diagram are the derivatives of y with respect to p and q
132 // respectively. This is how autodiff works for functors taking multiple vector
133 // valued arguments (up to 6).
134 //
135 // Jacobian NULL pointers
136 // ----------------------
137 // In general, the functions below will accept NULL pointers for all or some of
138 // the Jacobian parameters, meaning that those Jacobians will not be computed.
139 
140 #ifndef CERES_PUBLIC_INTERNAL_AUTODIFF_H_
141 #define CERES_PUBLIC_INTERNAL_AUTODIFF_H_
142 
143 #include <stddef.h>
144 
149 #define DCHECK assert
150 #define DCHECK_GT(a,b) assert((a)>(b))
151 
152 namespace ceres {
153 namespace internal {
154 
155 // Extends src by a 1st order pertubation for every dimension and puts it in
156 // dst. The size of src is N. Since this is also used for perturbations in
157 // blocked arrays, offset is used to shift which part of the jet the
158 // perturbation occurs. This is used to set up the extended x augmented by an
159 // identity matrix. The JetT type should be a Jet type, and T should be a
160 // numeric type (e.g. double). For example,
161 //
162 // 0 1 2 3 4 5 6 7 8
163 // dst[0] [ * | . . | 1 0 0 | . . . ]
164 // dst[1] [ * | . . | 0 1 0 | . . . ]
165 // dst[2] [ * | . . | 0 0 1 | . . . ]
166 //
167 // is what would get put in dst if N was 3, offset was 3, and the jet type JetT
168 // was 8-dimensional.
169 template <typename JetT, typename T, int N>
170 inline void Make1stOrderPerturbation(int offset, const T* src, JetT* dst) {
171  DCHECK(src);
172  DCHECK(dst);
173  for (int j = 0; j < N; ++j) {
174  dst[j].a = src[j];
175  dst[j].v.setZero();
176  dst[j].v[offset + j] = T(1.0);
177  }
178 }
179 
180 // Takes the 0th order part of src, assumed to be a Jet type, and puts it in
181 // dst. This is used to pick out the "vector" part of the extended y.
182 template <typename JetT, typename T>
183 inline void Take0thOrderPart(int M, const JetT *src, T dst) {
184  DCHECK(src);
185  for (int i = 0; i < M; ++i) {
186  dst[i] = src[i].a;
187  }
188 }
189 
190 // Takes N 1st order parts, starting at index N0, and puts them in the M x N
191 // matrix 'dst'. This is used to pick out the "matrix" parts of the extended y.
192 template <typename JetT, typename T, int N0, int N>
193 inline void Take1stOrderPart(const int M, const JetT *src, T *dst) {
194  DCHECK(src);
195  DCHECK(dst);
196  for (int i = 0; i < M; ++i) {
197  Eigen::Map<Eigen::Matrix<T, N, 1> >(dst + N * i, N) =
198  src[i].v.template segment<N>(N0);
199  }
200 }
201 
202 // This is in a struct because default template parameters on a
203 // function are not supported in C++03 (though it is available in
204 // C++0x). N0 through N5 are the dimension of the input arguments to
205 // the user supplied functor.
206 template <typename Functor, typename T,
207  int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
208  int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
209 struct AutoDiff {
210  static bool Differentiate(const Functor& functor,
211  T const *const *parameters,
212  int num_outputs,
213  T *function_value,
214  T **jacobians) {
215  // This block breaks the 80 column rule to keep it somewhat readable.
216  DCHECK_GT(num_outputs, 0);
217  DCHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
218  ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
219  ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
220  ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
221  ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) ||
222  ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) ||
223  ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) ||
224  ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) ||
225  ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) ||
226  ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0)));
227 
229  FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(
230  N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs);
231 
232  // These are the positions of the respective jets in the fixed array x.
233  const int jet0 = 0;
234  const int jet1 = N0;
235  const int jet2 = N0 + N1;
236  const int jet3 = N0 + N1 + N2;
237  const int jet4 = N0 + N1 + N2 + N3;
238  const int jet5 = N0 + N1 + N2 + N3 + N4;
239  const int jet6 = N0 + N1 + N2 + N3 + N4 + N5;
240  const int jet7 = N0 + N1 + N2 + N3 + N4 + N5 + N6;
241  const int jet8 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7;
242  const int jet9 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8;
243 
244  const JetT *unpacked_parameters[10] = {
245  x.get() + jet0,
246  x.get() + jet1,
247  x.get() + jet2,
248  x.get() + jet3,
249  x.get() + jet4,
250  x.get() + jet5,
251  x.get() + jet6,
252  x.get() + jet7,
253  x.get() + jet8,
254  x.get() + jet9,
255  };
256 
257  JetT* output = x.get() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9;
258 
259 #define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \
260  if (N ## i) { \
261  internal::Make1stOrderPerturbation<JetT, T, N ## i>( \
262  jet ## i, \
263  parameters[i], \
264  x.get() + jet ## i); \
265  }
276 #undef CERES_MAKE_1ST_ORDER_PERTURBATION
277 
278  if (!VariadicEvaluate<Functor, JetT,
279  N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call(
280  functor, unpacked_parameters, output)) {
281  return false;
282  }
283 
284  internal::Take0thOrderPart(num_outputs, output, function_value);
285 
286 #define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \
287  if (N ## i) { \
288  if (jacobians[i]) { \
289  internal::Take1stOrderPart<JetT, T, \
290  jet ## i, \
291  N ## i>(num_outputs, \
292  output, \
293  jacobians[i]); \
294  } \
295  }
306 #undef CERES_TAKE_1ST_ORDER_PERTURBATION
307  return true;
308  }
309 };
310 
311 } // namespace internal
312 } // namespace ceres
313 
314 #endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_
Matrix< RealScalar, Dynamic, Dynamic > M
Definition: bench_gemm.cpp:51
#define CERES_MAKE_1ST_ORDER_PERTURBATION(i)
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate offset
void Take0thOrderPart(int M, const JetT *src, T dst)
Definition: autodiff.h:183
#define N
Definition: gksort.c:12
Array< int, Dynamic, 1 > v
void Take1stOrderPart(const int M, const JetT *src, T *dst)
Definition: autodiff.h:193
Eigen::Triplet< double > T
#define DCHECK_GT(a, b)
Definition: autodiff.h:150
static ConjugateGradientParameters parameters
#define CERES_TAKE_1ST_ORDER_PERTURBATION(i)
const T * get() const
Definition: fixed_array.h:109
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
void Make1stOrderPerturbation(int offset, const T *src, JetT *dst)
Definition: autodiff.h:170
std::ptrdiff_t j
static bool Differentiate(const Functor &functor, T const *const *parameters, int num_outputs, T *function_value, T **jacobians)
Definition: autodiff.h:210
#define DCHECK
Definition: autodiff.h:149


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:33:56