include/ceres/autodiff_cost_function.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: sameeragarwal@google.com (Sameer Agarwal)
  30 //
  31 // Create CostFunctions as needed by the least squares framework, with
  32 // Jacobians computed via automatic differentiation. For more
  33 // information on automatic differentation, see the wikipedia article
  34 // at http://en.wikipedia.org/wiki/Automatic_differentiation
  35 //
  36 // To get an auto differentiated cost function, you must define a class with a
  37 // templated operator() (a functor) that computes the cost function in terms of
  38 // the template parameter T. The autodiff framework substitutes appropriate
  39 // "jet" objects for T in order to compute the derivative when necessary, but
  40 // this is hidden, and you should write the function as if T were a scalar type
  41 // (e.g. a double-precision floating point number).
  42 //
  43 // The function must write the computed value in the last argument
  44 // (the only non-const one) and return true to indicate
  45 // success. Please see cost_function.h for details on how the return
  46 // value maybe used to impose simple constraints on the parameter
  47 // block.
  48 //
  49 // For example, consider a scalar error e = k - x'y, where both x and y are
  50 // two-dimensional column vector parameters, the prime sign indicates
  51 // transposition, and k is a constant. The form of this error, which is the
  52 // difference between a constant and an expression, is a common pattern in least
  53 // squares problems. For example, the value x'y might be the model expectation
  54 // for a series of measurements, where there is an instance of the cost function
  55 // for each measurement k.
  56 //
  57 // The actual cost added to the total problem is e^2, or (k - x'k)^2; however,
  58 // the squaring is implicitly done by the optimization framework.
  59 //
  60 // To write an auto-differentiable cost function for the above model, first
  61 // define the object
  62 //
  63 //   class MyScalarCostFunctor {
  64 //     MyScalarCostFunctor(double k): k_(k) {}
  65 //
  66 //     template <typename T>
  67 //     bool operator()(const T* const x , const T* const y, T* e) const {
  68 //       e[0] = T(k_) - x[0] * y[0] + x[1] * y[1];
  69 //       return true;
  70 //     }
  71 //
  72 //    private:
  73 //     double k_;
  74 //   };
  75 //
  76 // Note that in the declaration of operator() the input parameters x and y come
  77 // first, and are passed as const pointers to arrays of T. If there were three
  78 // input parameters, then the third input parameter would come after y. The
  79 // output is always the last parameter, and is also a pointer to an array. In
  80 // the example above, e is a scalar, so only e[0] is set.
  81 //
  82 // Then given this class definition, the auto differentiated cost function for
  83 // it can be constructed as follows.
  84 //
  85 //   CostFunction* cost_function
  86 //       = new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(
  87 //            new MyScalarCostFunctor(1.0));             ^  ^  ^
  88 //                                                       |  |  |
  89 //                            Dimension of residual -----+  |  |
  90 //                            Dimension of x ---------------+  |
  91 //                            Dimension of y ------------------+
  92 //
  93 // In this example, there is usually an instance for each measumerent of k.
  94 //
  95 // In the instantiation above, the template parameters following
  96 // "MyScalarCostFunctor", "1, 2, 2", describe the functor as computing a
  97 // 1-dimensional output from two arguments, both 2-dimensional.
  98 //
  99 // AutoDiffCostFunction also supports cost functions with a
 100 // runtime-determined number of residuals. For example:
 101 //
 102 //   CostFunction* cost_function
 103 //       = new AutoDiffCostFunction<MyScalarCostFunctor, DYNAMIC, 2, 2>(
 104 //           new CostFunctorWithDynamicNumResiduals(1.0),   ^     ^  ^
 105 //           runtime_number_of_residuals); <----+           |     |  |
 106 //                                              |           |     |  |
 107 //                                              |           |     |  |
 108 //             Actual number of residuals ------+           |     |  |
 109 //             Indicate dynamic number of residuals --------+     |  |
 110 //             Dimension of x ------------------------------------+  |
 111 //             Dimension of y ---------------------------------------+
 112 //
 113 // The framework can currently accommodate cost functions of up to 10
 114 // independent variables, and there is no limit on the dimensionality
 115 // of each of them.
 116 //
 117 // WARNING #1: Since the functor will get instantiated with different types for
 118 // T, you must to convert from other numeric types to T before mixing
 119 // computations with other variables of type T. In the example above, this is
 120 // seen where instead of using k_ directly, k_ is wrapped with T(k_).
 121 //
 122 // WARNING #2: A common beginner's error when first using autodiff cost
 123 // functions is to get the sizing wrong. In particular, there is a tendency to
 124 // set the template parameters to (dimension of residual, number of parameters)
 125 // instead of passing a dimension parameter for *every parameter*. In the
 126 // example above, that would be <MyScalarCostFunctor, 1, 2>, which is missing
 127 // the last '2' argument. Please be careful when setting the size parameters.
 128
 129 #ifndef CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_
 130 #define CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_
 131
 132 #include "ceres/internal/autodiff.h"
 133 #include "ceres/internal/scoped_ptr.h"
 134 #include "ceres/sized_cost_function.h"
 135 #include "ceres/types.h"
 136 #include "glog/logging.h"
 137
 138 namespace ceres {
 139
 140 // A cost function which computes the derivative of the cost with respect to
 141 // the parameters (a.k.a. the jacobian) using an autodifferentiation framework.
 142 // The first template argument is the functor object, described in the header
 143 // comment. The second argument is the dimension of the residual (or
 144 // ceres::DYNAMIC to indicate it will be set at runtime), and subsequent
 145 // arguments describe the size of the Nth parameter, one per parameter.
 146 //
 147 // The constructors take ownership of the cost functor.
 148 //
 149 // If the number of residuals (argument kNumResiduals below) is
 150 // ceres::DYNAMIC, then the two-argument constructor must be used. The
 151 // second constructor takes a number of residuals (in addition to the
 152 // templated number of residuals). This allows for varying the number
 153 // of residuals for a single autodiff cost function at runtime.
 154 template <typename CostFunctor,
 155           int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.
 156           int N0,       // Number of parameters in block 0.
 157           int N1 = 0,   // Number of parameters in block 1.
 158           int N2 = 0,   // Number of parameters in block 2.
 159           int N3 = 0,   // Number of parameters in block 3.
 160           int N4 = 0,   // Number of parameters in block 4.
 161           int N5 = 0,   // Number of parameters in block 5.
 162           int N6 = 0,   // Number of parameters in block 6.
 163           int N7 = 0,   // Number of parameters in block 7.
 164           int N8 = 0,   // Number of parameters in block 8.
 165           int N9 = 0>   // Number of parameters in block 9.
 166 class AutoDiffCostFunction : public SizedCostFunction<kNumResiduals,
 167                                                       N0, N1, N2, N3, N4,
 168                                                       N5, N6, N7, N8, N9> {
 169  public:
 170   // Takes ownership of functor. Uses the template-provided value for the
 171   // number of residuals ("kNumResiduals").
 172   explicit AutoDiffCostFunction(CostFunctor* functor)
 173       : functor_(functor) {
 174     CHECK_NE(kNumResiduals, DYNAMIC)
 175         << "Can't run the fixed-size constructor if the "
 176         << "number of residuals is set to ceres::DYNAMIC.";
 177   }
 178
 179   // Takes ownership of functor. Ignores the template-provided
 180   // kNumResiduals in favor of the "num_residuals" argument provided.
 181   //
 182   // This allows for having autodiff cost functions which return varying
 183   // numbers of residuals at runtime.
 184   AutoDiffCostFunction(CostFunctor* functor, int num_residuals)
 185       : functor_(functor) {
 186     CHECK_EQ(kNumResiduals, DYNAMIC)
 187         << "Can't run the dynamic-size constructor if the "
 188         << "number of residuals is not ceres::DYNAMIC.";
 189     SizedCostFunction<kNumResiduals,
 190                       N0, N1, N2, N3, N4,
 191                       N5, N6, N7, N8, N9>
 192         ::set_num_residuals(num_residuals);
 193   }
 194
 195   virtual ~AutoDiffCostFunction() {}
 196
 197   // Implementation details follow; clients of the autodiff cost function should
 198   // not have to examine below here.
 199   //
 200   // To handle varardic cost functions, some template magic is needed. It's
 201   // mostly hidden inside autodiff.h.
 202   virtual bool Evaluate(double const* const* parameters,
 203                         double* residuals,
 204                         double** jacobians) const {
 205     if (!jacobians) {
 206       return internal::VariadicEvaluate<
 207           CostFunctor, double, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>
 208           ::Call(*functor_, parameters, residuals);
 209     }
 210     return internal::AutoDiff<CostFunctor, double,
 211            N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Differentiate(
 212                *functor_,
 213                parameters,
 214                SizedCostFunction<kNumResiduals,
 215                                  N0, N1, N2, N3, N4,
 216                                  N5, N6, N7, N8, N9>::num_residuals(),
 217                residuals,
 218                jacobians);
 219   }
 220
 221  private:
 222   internal::scoped_ptr<CostFunctor> functor_;
 223 };
 224
 225 }  // namespace ceres
 226
 227 #endif  // CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_