include/ceres/dynamic_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 //         mierle@gmail.com (Keir Mierle)
  31
  32 #ifndef CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_
  33 #define CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_
  34
  35 #include <cmath>
  36 #include <numeric>
  37 #include <vector>
  38
  39 #include "ceres/dynamic_cost_function.h"
  40 #include "ceres/internal/scoped_ptr.h"
  41 #include "ceres/jet.h"
  42 #include "glog/logging.h"
  43
  44 namespace ceres {
  45
  46 // This autodiff implementation differs from the one found in
  47 // autodiff_cost_function.h by supporting autodiff on cost functions
  48 // with variable numbers of parameters with variable sizes. With the
  49 // other implementation, all the sizes (both the number of parameter
  50 // blocks and the size of each block) must be fixed at compile time.
  51 //
  52 // The functor API differs slightly from the API for fixed size
  53 // autodiff; the expected interface for the cost functors is:
  54 //
  55 //   struct MyCostFunctor {
  56 //     template<typename T>
  57 //     bool operator()(T const* const* parameters, T* residuals) const {
  58 //       // Use parameters[i] to access the i'th parameter block.
  59 //     }
  60 //   }
  61 //
  62 // Since the sizing of the parameters is done at runtime, you must
  63 // also specify the sizes after creating the dynamic autodiff cost
  64 // function. For example:
  65 //
  66 //   DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function(
  67 //       new MyCostFunctor());
  68 //   cost_function.AddParameterBlock(5);
  69 //   cost_function.AddParameterBlock(10);
  70 //   cost_function.SetNumResiduals(21);
  71 //
  72 // Under the hood, the implementation evaluates the cost function
  73 // multiple times, computing a small set of the derivatives (four by
  74 // default, controlled by the Stride template parameter) with each
  75 // pass. There is a tradeoff with the size of the passes; you may want
  76 // to experiment with the stride.
  77 template <typename CostFunctor, int Stride = 4>
  78 class DynamicAutoDiffCostFunction : public DynamicCostFunction {
  79  public:
  80   explicit DynamicAutoDiffCostFunction(CostFunctor* functor)
  81     : functor_(functor) {}
  82
  83   virtual ~DynamicAutoDiffCostFunction() {}
  84
  85   virtual bool Evaluate(double const* const* parameters,
  86                         double* residuals,
  87                         double** jacobians) const {
  88     CHECK_GT(num_residuals(), 0)
  89         << "You must call DynamicAutoDiffCostFunction::SetNumResiduals() "
  90         << "before DynamicAutoDiffCostFunction::Evaluate().";
  91
  92     if (jacobians == NULL) {
  93       return (*functor_)(parameters, residuals);
  94     }
  95
  96     // The difficulty with Jets, as implemented in Ceres, is that they were
  97     // originally designed for strictly compile-sized use. At this point, there
  98     // is a large body of code that assumes inside a cost functor it is
  99     // acceptable to do e.g. T(1.5) and get an appropriately sized jet back.
 100     //
 101     // Unfortunately, it is impossible to communicate the expected size of a
 102     // dynamically sized jet to the static instantiations that existing code
 103     // depends on.
 104     //
 105     // To work around this issue, the solution here is to evaluate the
 106     // jacobians in a series of passes, each one computing Stripe *
 107     // num_residuals() derivatives. This is done with small, fixed-size jets.
 108     const int num_parameter_blocks = parameter_block_sizes().size();
 109     const int num_parameters = std::accumulate(parameter_block_sizes().begin(),
 110                                                parameter_block_sizes().end(),
 111                                                0);
 112
 113     // Allocate scratch space for the strided evaluation.
 114     std::vector<Jet<double, Stride> > input_jets(num_parameters);
 115     std::vector<Jet<double, Stride> > output_jets(num_residuals());
 116
 117     // Make the parameter pack that is sent to the functor (reused).
 118     std::vector<Jet<double, Stride>* > jet_parameters(num_parameter_blocks,
 119         static_cast<Jet<double, Stride>* >(NULL));
 120     int num_active_parameters = 0;
 121
 122     // To handle constant parameters between non-constant parameter blocks, the
 123     // start position --- a raw parameter index --- of each contiguous block of
 124     // non-constant parameters is recorded in start_derivative_section.
 125     std::vector<int> start_derivative_section;
 126     bool in_derivative_section = false;
 127     int parameter_cursor = 0;
 128
 129     // Discover the derivative sections and set the parameter values.
 130     for (int i = 0; i < num_parameter_blocks; ++i) {
 131       jet_parameters[i] = &input_jets[parameter_cursor];
 132
 133       const int parameter_block_size = parameter_block_sizes()[i];
 134       if (jacobians[i] != NULL) {
 135         if (!in_derivative_section) {
 136           start_derivative_section.push_back(parameter_cursor);
 137           in_derivative_section = true;
 138         }
 139
 140         num_active_parameters += parameter_block_size;
 141       } else {
 142         in_derivative_section = false;
 143       }
 144
 145       for (int j = 0; j < parameter_block_size; ++j, parameter_cursor++) {
 146         input_jets[parameter_cursor].a = parameters[i][j];
 147       }
 148     }
 149
 150     // When `num_active_parameters % Stride != 0` then it can be the case
 151     // that `active_parameter_count < Stride` while parameter_cursor is less
 152     // than the total number of parameters and with no remaining non-constant
 153     // parameter blocks. Pushing parameter_cursor (the total number of
 154     // parameters) as a final entry to start_derivative_section is required
 155     // because if a constant parameter block is encountered after the
 156     // last non-constant block then current_derivative_section is incremented
 157     // and would otherwise index an invalid position in
 158     // start_derivative_section. Setting the final element to the total number
 159     // of parameters means that this can only happen at most once in the loop
 160     // below.
 161     start_derivative_section.push_back(parameter_cursor);
 162
 163     // Evaluate all of the strides. Each stride is a chunk of the derivative to
 164     // evaluate, typically some size proportional to the size of the SIMD
 165     // registers of the CPU.
 166     int num_strides = static_cast<int>(ceil(num_active_parameters /
 167                                             static_cast<float>(Stride)));
 168
 169     int current_derivative_section = 0;
 170     int current_derivative_section_cursor = 0;
 171
 172     for (int pass = 0; pass < num_strides; ++pass) {
 173       // Set most of the jet components to zero, except for
 174       // non-constant #Stride parameters.
 175       const int initial_derivative_section = current_derivative_section;
 176       const int initial_derivative_section_cursor =
 177         current_derivative_section_cursor;
 178
 179       int active_parameter_count = 0;
 180       parameter_cursor = 0;
 181
 182       for (int i = 0; i < num_parameter_blocks; ++i) {
 183         for (int j = 0; j < parameter_block_sizes()[i];
 184              ++j, parameter_cursor++) {
 185           input_jets[parameter_cursor].v.setZero();
 186           if (active_parameter_count < Stride &&
 187               parameter_cursor >= (
 188                 start_derivative_section[current_derivative_section] +
 189                 current_derivative_section_cursor)) {
 190             if (jacobians[i] != NULL) {
 191               input_jets[parameter_cursor].v[active_parameter_count] = 1.0;
 192               ++active_parameter_count;
 193               ++current_derivative_section_cursor;
 194             } else {
 195               ++current_derivative_section;
 196               current_derivative_section_cursor = 0;
 197             }
 198           }
 199         }
 200       }
 201
 202       if (!(*functor_)(&jet_parameters[0], &output_jets[0])) {
 203         return false;
 204       }
 205
 206       // Copy the pieces of the jacobians into their final place.
 207       active_parameter_count = 0;
 208
 209       current_derivative_section = initial_derivative_section;
 210       current_derivative_section_cursor = initial_derivative_section_cursor;
 211
 212       for (int i = 0, parameter_cursor = 0; i < num_parameter_blocks; ++i) {
 213         for (int j = 0; j < parameter_block_sizes()[i];
 214              ++j, parameter_cursor++) {
 215           if (active_parameter_count < Stride &&
 216               parameter_cursor >= (
 217                 start_derivative_section[current_derivative_section] +
 218                 current_derivative_section_cursor)) {
 219             if (jacobians[i] != NULL) {
 220               for (int k = 0; k < num_residuals(); ++k) {
 221                 jacobians[i][k * parameter_block_sizes()[i] + j] =
 222                     output_jets[k].v[active_parameter_count];
 223               }
 224               ++active_parameter_count;
 225               ++current_derivative_section_cursor;
 226             } else {
 227               ++current_derivative_section;
 228               current_derivative_section_cursor = 0;
 229             }
 230           }
 231         }
 232       }
 233
 234       // Only copy the residuals over once (even though we compute them on
 235       // every loop).
 236       if (pass == num_strides - 1) {
 237         for (int k = 0; k < num_residuals(); ++k) {
 238           residuals[k] = output_jets[k].a;
 239         }
 240       }
 241     }
 242     return true;
 243   }
 244
 245  private:
 246   internal::scoped_ptr<CostFunctor> functor_;
 247 };
 248
 249 }  // namespace ceres
 250
 251 #endif  // CERES_PUBLIC_DYNAMIC_AUTODIFF_COST_FUNCTION_H_