include/ceres/internal/autodiff.h

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   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
 151 namespace ceres {
 152 namespace 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.
 168 template <typename JetT, typename T, int N>
 169 inline 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] = T(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.
 181 template <typename JetT, typename T>
 182 inline 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.
 191 template <typename JetT, typename T, int N0, int N>
 192 inline 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.
 205 template <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>
 208 struct 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) ||                                   // NOLINT
 219            ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||                              // NOLINT
 220            ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) ||                         // NOLINT
 221            ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) ||                    // NOLINT
 222            ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) ||               // NOLINT
 223            ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) ||          // NOLINT
 224            ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) ||     // NOLINT
 225            ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0)))  // NOLINT
 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     // Invalidate the output Jets, so that we can detect if the user
 263     // did not assign values to all of them.
 264     for (int i = 0; i < num_outputs; ++i) {
 265       output[i].a = kImpossibleValue;
 266       output[i].v.setConstant(kImpossibleValue);
 267     }
 268
 269 #define CERES_MAKE_1ST_ORDER_PERTURBATION(i)                            \
 270     if (N ## i) {                                                       \
 271       internal::Make1stOrderPerturbation<JetT, T, N ## i>(              \
 272           jet ## i,                                                     \
 273           parameters[i],                                                \
 274           x.get() + jet ## i);                                          \
 275     }
 276     CERES_MAKE_1ST_ORDER_PERTURBATION(0);
 277     CERES_MAKE_1ST_ORDER_PERTURBATION(1);
 278     CERES_MAKE_1ST_ORDER_PERTURBATION(2);
 279     CERES_MAKE_1ST_ORDER_PERTURBATION(3);
 280     CERES_MAKE_1ST_ORDER_PERTURBATION(4);
 281     CERES_MAKE_1ST_ORDER_PERTURBATION(5);
 282     CERES_MAKE_1ST_ORDER_PERTURBATION(6);
 283     CERES_MAKE_1ST_ORDER_PERTURBATION(7);
 284     CERES_MAKE_1ST_ORDER_PERTURBATION(8);
 285     CERES_MAKE_1ST_ORDER_PERTURBATION(9);
 286 #undef CERES_MAKE_1ST_ORDER_PERTURBATION
 287
 288     if (!VariadicEvaluate<Functor, JetT,
 289                           N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call(
 290         functor, unpacked_parameters, output)) {
 291       return false;
 292     }
 293
 294     internal::Take0thOrderPart(num_outputs, output, function_value);
 295
 296 #define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \
 297     if (N ## i) { \
 298       if (jacobians[i]) { \
 299         internal::Take1stOrderPart<JetT, T, \
 300                                    jet ## i, \
 301                                    N ## i>(num_outputs, \
 302                                            output, \
 303                                            jacobians[i]); \
 304       } \
 305     }
 306     CERES_TAKE_1ST_ORDER_PERTURBATION(0);
 307     CERES_TAKE_1ST_ORDER_PERTURBATION(1);
 308     CERES_TAKE_1ST_ORDER_PERTURBATION(2);
 309     CERES_TAKE_1ST_ORDER_PERTURBATION(3);
 310     CERES_TAKE_1ST_ORDER_PERTURBATION(4);
 311     CERES_TAKE_1ST_ORDER_PERTURBATION(5);
 312     CERES_TAKE_1ST_ORDER_PERTURBATION(6);
 313     CERES_TAKE_1ST_ORDER_PERTURBATION(7);
 314     CERES_TAKE_1ST_ORDER_PERTURBATION(8);
 315     CERES_TAKE_1ST_ORDER_PERTURBATION(9);
 316 #undef CERES_TAKE_1ST_ORDER_PERTURBATION
 317     return true;
 318   }
 319 };
 320
 321 }  // namespace internal
 322 }  // namespace ceres
 323
 324 #endif  // CERES_PUBLIC_INTERNAL_AUTODIFF_H_