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 // 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 145 #include <glog/logging.h> 146 #include "ceres/jet.h" 147 #include "ceres/internal/eigen.h" 148 #include "ceres/internal/fixed_array.h" 149 150 namespace ceres { 151 namespace internal { 152 153 // Extends src by a 1st order pertubation for every dimension and puts it in 154 // dst. The size of src is N. Since this is also used for perturbations in 155 // blocked arrays, offset is used to shift which part of the jet the 156 // perturbation occurs. This is used to set up the extended x augmented by an 157 // identity matrix. The JetT type should be a Jet type, and T should be a 158 // numeric type (e.g. double). For example, 159 // 160 // 0 1 2 3 4 5 6 7 8 161 // dst[0] [ * | . . | 1 0 0 | . . . ] 162 // dst[1] [ * | . . | 0 1 0 | . . . ] 163 // dst[2] [ * | . . | 0 0 1 | . . . ] 164 // 165 // is what would get put in dst if N was 3, offset was 3, and the jet type JetT 166 // was 8-dimensional. 167 template <typename JetT, typename T> 168 inline void Make1stOrderPerturbation(int offset, int N, const T *src, 169 JetT *dst) { 170 DCHECK(src); 171 DCHECK(dst); 172 for (int j = 0; j < N; ++j) { 173 dst[j] = JetT(src[j], offset + j); 174 } 175 } 176 177 // Takes the 0th order part of src, assumed to be a Jet type, and puts it in 178 // dst. This is used to pick out the "vector" part of the extended y. 179 template <typename JetT, typename T> 180 inline void Take0thOrderPart(int M, const JetT *src, T dst) { 181 DCHECK(src); 182 for (int i = 0; i < M; ++i) { 183 dst[i] = src[i].a; 184 } 185 } 186 187 // Takes N 1st order parts, starting at index N0, and puts them in the M x N 188 // matrix 'dst'. This is used to pick out the "matrix" parts of the extended y. 189 template <typename JetT, typename T, int N0, int N> 190 inline void Take1stOrderPart(const int M, const JetT *src, T *dst) { 191 DCHECK(src); 192 DCHECK(dst); 193 for (int i = 0; i < M; ++i) { 194 Eigen::Map<Eigen::Matrix<T, N, 1> >(dst + N * i, N) = src[i].v.template segment<N>(N0); 195 } 196 } 197 198 // This block of quasi-repeated code calls the user-supplied functor, which may 199 // take a variable number of arguments. This is accomplished by specializing the 200 // struct based on the size of the trailing parameters; parameters with 0 size 201 // are assumed missing. 202 // 203 // Supporting variadic functions is the primary source of complexity in the 204 // autodiff implementation. 205 206 template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, 207 int N5, int N6, int N7, int N8, int N9> 208 struct VariadicEvaluate { 209 static bool Call(const Functor& functor, T const *const *input, T* output) { 210 return functor(input[0], 211 input[1], 212 input[2], 213 input[3], 214 input[4], 215 input[5], 216 input[6], 217 input[7], 218 input[8], 219 input[9], 220 output); 221 } 222 }; 223 224 template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, 225 int N5, int N6, int N7, int N8> 226 struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, N8, 0> { 227 static bool Call(const Functor& functor, T const *const *input, T* output) { 228 return functor(input[0], 229 input[1], 230 input[2], 231 input[3], 232 input[4], 233 input[5], 234 input[6], 235 input[7], 236 input[8], 237 output); 238 } 239 }; 240 241 template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, 242 int N5, int N6, int N7> 243 struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, 0, 0> { 244 static bool Call(const Functor& functor, T const *const *input, T* output) { 245 return functor(input[0], 246 input[1], 247 input[2], 248 input[3], 249 input[4], 250 input[5], 251 input[6], 252 input[7], 253 output); 254 } 255 }; 256 257 template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, 258 int N5, int N6> 259 struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, 0, 0, 0> { 260 static bool Call(const Functor& functor, T const *const *input, T* output) { 261 return functor(input[0], 262 input[1], 263 input[2], 264 input[3], 265 input[4], 266 input[5], 267 input[6], 268 output); 269 } 270 }; 271 272 template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4, 273 int N5> 274 struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, 0, 0, 0, 0> { 275 static bool Call(const Functor& functor, T const *const *input, T* output) { 276 return functor(input[0], 277 input[1], 278 input[2], 279 input[3], 280 input[4], 281 input[5], 282 output); 283 } 284 }; 285 286 template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4> 287 struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0, 0, 0, 0, 0> { 288 static bool Call(const Functor& functor, T const *const *input, T* output) { 289 return functor(input[0], 290 input[1], 291 input[2], 292 input[3], 293 input[4], 294 output); 295 } 296 }; 297 298 template<typename Functor, typename T, int N0, int N1, int N2, int N3> 299 struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, 0, 0, 0, 0, 0, 0> { 300 static bool Call(const Functor& functor, T const *const *input, T* output) { 301 return functor(input[0], 302 input[1], 303 input[2], 304 input[3], 305 output); 306 } 307 }; 308 309 template<typename Functor, typename T, int N0, int N1, int N2> 310 struct VariadicEvaluate<Functor, T, N0, N1, N2, 0, 0, 0, 0, 0, 0, 0> { 311 static bool Call(const Functor& functor, T const *const *input, T* output) { 312 return functor(input[0], 313 input[1], 314 input[2], 315 output); 316 } 317 }; 318 319 template<typename Functor, typename T, int N0, int N1> 320 struct VariadicEvaluate<Functor, T, N0, N1, 0, 0, 0, 0, 0, 0, 0, 0> { 321 static bool Call(const Functor& functor, T const *const *input, T* output) { 322 return functor(input[0], 323 input[1], 324 output); 325 } 326 }; 327 328 template<typename Functor, typename T, int N0> 329 struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0, 0, 0, 0, 0> { 330 static bool Call(const Functor& functor, T const *const *input, T* output) { 331 return functor(input[0], 332 output); 333 } 334 }; 335 336 // This is in a struct because default template parameters on a function are not 337 // supported in C++03 (though it is available in C++0x). N0 through N5 are the 338 // dimension of the input arguments to the user supplied functor. 339 template <typename Functor, typename T, 340 int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, 341 int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0> 342 struct AutoDiff { 343 static bool Differentiate(const Functor& functor, 344 T const *const *parameters, 345 int num_outputs, 346 T *function_value, 347 T **jacobians) { 348 // This block breaks the 80 column rule to keep it somewhat readable. 349 DCHECK_GT(num_outputs, 0); 350 CHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || 351 ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || 352 ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || 353 ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || 354 ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || 355 ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || 356 ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || 357 ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || 358 ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || 359 ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) 360 << "Zero block cannot precede a non-zero block. Block sizes are " 361 << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " 362 << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", " 363 << N8 << ", " << N9; 364 365 typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9> JetT; 366 FixedArray<JetT, (256 * 7) / sizeof(JetT)> x( 367 N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs); 368 369 // These are the positions of the respective jets in the fixed array x. 370 const int jet0 = 0; 371 const int jet1 = N0; 372 const int jet2 = N0 + N1; 373 const int jet3 = N0 + N1 + N2; 374 const int jet4 = N0 + N1 + N2 + N3; 375 const int jet5 = N0 + N1 + N2 + N3 + N4; 376 const int jet6 = N0 + N1 + N2 + N3 + N4 + N5; 377 const int jet7 = N0 + N1 + N2 + N3 + N4 + N5 + N6; 378 const int jet8 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7; 379 const int jet9 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8; 380 381 const JetT *unpacked_parameters[10] = { 382 x.get() + jet0, 383 x.get() + jet1, 384 x.get() + jet2, 385 x.get() + jet3, 386 x.get() + jet4, 387 x.get() + jet5, 388 x.get() + jet6, 389 x.get() + jet7, 390 x.get() + jet8, 391 x.get() + jet9, 392 }; 393 JetT *output = x.get() + jet6; 394 395 #define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \ 396 if (N ## i) { \ 397 internal::Make1stOrderPerturbation(jet ## i, \ 398 N ## i, \ 399 parameters[i], \ 400 x.get() + jet ## i); \ 401 } 402 CERES_MAKE_1ST_ORDER_PERTURBATION(0); 403 CERES_MAKE_1ST_ORDER_PERTURBATION(1); 404 CERES_MAKE_1ST_ORDER_PERTURBATION(2); 405 CERES_MAKE_1ST_ORDER_PERTURBATION(3); 406 CERES_MAKE_1ST_ORDER_PERTURBATION(4); 407 CERES_MAKE_1ST_ORDER_PERTURBATION(5); 408 CERES_MAKE_1ST_ORDER_PERTURBATION(6); 409 CERES_MAKE_1ST_ORDER_PERTURBATION(7); 410 CERES_MAKE_1ST_ORDER_PERTURBATION(8); 411 CERES_MAKE_1ST_ORDER_PERTURBATION(9); 412 #undef CERES_MAKE_1ST_ORDER_PERTURBATION 413 414 if (!VariadicEvaluate<Functor, JetT, 415 N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call( 416 functor, unpacked_parameters, output)) { 417 return false; 418 } 419 420 internal::Take0thOrderPart(num_outputs, output, function_value); 421 422 #define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \ 423 if (N ## i) { \ 424 if (jacobians[i]) { \ 425 internal::Take1stOrderPart<JetT, T, \ 426 jet ## i, \ 427 N ## i>(num_outputs, \ 428 output, \ 429 jacobians[i]); \ 430 } \ 431 } 432 CERES_TAKE_1ST_ORDER_PERTURBATION(0); 433 CERES_TAKE_1ST_ORDER_PERTURBATION(1); 434 CERES_TAKE_1ST_ORDER_PERTURBATION(2); 435 CERES_TAKE_1ST_ORDER_PERTURBATION(3); 436 CERES_TAKE_1ST_ORDER_PERTURBATION(4); 437 CERES_TAKE_1ST_ORDER_PERTURBATION(5); 438 CERES_TAKE_1ST_ORDER_PERTURBATION(6); 439 CERES_TAKE_1ST_ORDER_PERTURBATION(7); 440 CERES_TAKE_1ST_ORDER_PERTURBATION(8); 441 CERES_TAKE_1ST_ORDER_PERTURBATION(9); 442 #undef CERES_TAKE_1ST_ORDER_PERTURBATION 443 return true; 444 } 445 }; 446 447 } // namespace internal 448 } // namespace ceres 449 450 #endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_ 451
