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 145#include "ceres/jet.h" 146#include "ceres/internal/eigen.h" 147#include "ceres/internal/fixed_array.h" 148#include "ceres/internal/variadic_evaluate.h" 149#include "glog/logging.h" 150 151namespace ceres { 152namespace internal { 153 154// Extends src by a 1st order pertubation for every dimension and puts it in 155// dst. The size of src is N. Since this is also used for perturbations in 156// blocked arrays, offset is used to shift which part of the jet the 157// perturbation occurs. This is used to set up the extended x augmented by an 158// identity matrix. The JetT type should be a Jet type, and T should be a 159// numeric type (e.g. double). For example, 160// 161// 0 1 2 3 4 5 6 7 8 162// dst[0] [ * | . . | 1 0 0 | . . . ] 163// dst[1] [ * | . . | 0 1 0 | . . . ] 164// dst[2] [ * | . . | 0 0 1 | . . . ] 165// 166// is what would get put in dst if N was 3, offset was 3, and the jet type JetT 167// was 8-dimensional. 168template <typename JetT, typename T, int N> 169inline void Make1stOrderPerturbation(int offset, const T* src, JetT* dst) { 170 DCHECK(src); 171 DCHECK(dst); 172 for (int j = 0; j < N; ++j) { 173 dst[j].a = src[j]; 174 dst[j].v.setZero(); 175 dst[j].v[offset + j] = 1.0; 176 } 177} 178 179// Takes the 0th order part of src, assumed to be a Jet type, and puts it in 180// dst. This is used to pick out the "vector" part of the extended y. 181template <typename JetT, typename T> 182inline void Take0thOrderPart(int M, const JetT *src, T dst) { 183 DCHECK(src); 184 for (int i = 0; i < M; ++i) { 185 dst[i] = src[i].a; 186 } 187} 188 189// Takes N 1st order parts, starting at index N0, and puts them in the M x N 190// matrix 'dst'. This is used to pick out the "matrix" parts of the extended y. 191template <typename JetT, typename T, int N0, int N> 192inline void Take1stOrderPart(const int M, const JetT *src, T *dst) { 193 DCHECK(src); 194 DCHECK(dst); 195 for (int i = 0; i < M; ++i) { 196 Eigen::Map<Eigen::Matrix<T, N, 1> >(dst + N * i, N) = 197 src[i].v.template segment<N>(N0); 198 } 199} 200 201// This is in a struct because default template parameters on a 202// function are not supported in C++03 (though it is available in 203// C++0x). N0 through N5 are the dimension of the input arguments to 204// the user supplied functor. 205template <typename Functor, typename T, 206 int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, 207 int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0> 208struct AutoDiff { 209 static bool Differentiate(const Functor& functor, 210 T const *const *parameters, 211 int num_outputs, 212 T *function_value, 213 T **jacobians) { 214 // This block breaks the 80 column rule to keep it somewhat readable. 215 DCHECK_GT(num_outputs, 0); 216 DCHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || 217 ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || 218 ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || 219 ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) || 220 ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) || 221 ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) || 222 ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) || 223 ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) || 224 ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) || 225 ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0))) 226 << "Zero block cannot precede a non-zero block. Block sizes are " 227 << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " 228 << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", " 229 << N8 << ", " << N9; 230 231 typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9> JetT; 232 FixedArray<JetT, (256 * 7) / sizeof(JetT)> x( 233 N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs); 234 235 // These are the positions of the respective jets in the fixed array x. 236 const int jet0 = 0; 237 const int jet1 = N0; 238 const int jet2 = N0 + N1; 239 const int jet3 = N0 + N1 + N2; 240 const int jet4 = N0 + N1 + N2 + N3; 241 const int jet5 = N0 + N1 + N2 + N3 + N4; 242 const int jet6 = N0 + N1 + N2 + N3 + N4 + N5; 243 const int jet7 = N0 + N1 + N2 + N3 + N4 + N5 + N6; 244 const int jet8 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7; 245 const int jet9 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8; 246 247 const JetT *unpacked_parameters[10] = { 248 x.get() + jet0, 249 x.get() + jet1, 250 x.get() + jet2, 251 x.get() + jet3, 252 x.get() + jet4, 253 x.get() + jet5, 254 x.get() + jet6, 255 x.get() + jet7, 256 x.get() + jet8, 257 x.get() + jet9, 258 }; 259 260 JetT* output = x.get() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9; 261 262#define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \ 263 if (N ## i) { \ 264 internal::Make1stOrderPerturbation<JetT, T, N ## i>( \ 265 jet ## i, \ 266 parameters[i], \ 267 x.get() + jet ## i); \ 268 } 269 CERES_MAKE_1ST_ORDER_PERTURBATION(0); 270 CERES_MAKE_1ST_ORDER_PERTURBATION(1); 271 CERES_MAKE_1ST_ORDER_PERTURBATION(2); 272 CERES_MAKE_1ST_ORDER_PERTURBATION(3); 273 CERES_MAKE_1ST_ORDER_PERTURBATION(4); 274 CERES_MAKE_1ST_ORDER_PERTURBATION(5); 275 CERES_MAKE_1ST_ORDER_PERTURBATION(6); 276 CERES_MAKE_1ST_ORDER_PERTURBATION(7); 277 CERES_MAKE_1ST_ORDER_PERTURBATION(8); 278 CERES_MAKE_1ST_ORDER_PERTURBATION(9); 279#undef CERES_MAKE_1ST_ORDER_PERTURBATION 280 281 if (!VariadicEvaluate<Functor, JetT, 282 N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call( 283 functor, unpacked_parameters, output)) { 284 return false; 285 } 286 287 internal::Take0thOrderPart(num_outputs, output, function_value); 288 289#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \ 290 if (N ## i) { \ 291 if (jacobians[i]) { \ 292 internal::Take1stOrderPart<JetT, T, \ 293 jet ## i, \ 294 N ## i>(num_outputs, \ 295 output, \ 296 jacobians[i]); \ 297 } \ 298 } 299 CERES_TAKE_1ST_ORDER_PERTURBATION(0); 300 CERES_TAKE_1ST_ORDER_PERTURBATION(1); 301 CERES_TAKE_1ST_ORDER_PERTURBATION(2); 302 CERES_TAKE_1ST_ORDER_PERTURBATION(3); 303 CERES_TAKE_1ST_ORDER_PERTURBATION(4); 304 CERES_TAKE_1ST_ORDER_PERTURBATION(5); 305 CERES_TAKE_1ST_ORDER_PERTURBATION(6); 306 CERES_TAKE_1ST_ORDER_PERTURBATION(7); 307 CERES_TAKE_1ST_ORDER_PERTURBATION(8); 308 CERES_TAKE_1ST_ORDER_PERTURBATION(9); 309#undef CERES_TAKE_1ST_ORDER_PERTURBATION 310 return true; 311 } 312}; 313 314} // namespace internal 315} // namespace ceres 316 317#endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_ 318