include/ceres/dynamic_cost_function_to_functor.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 //         dgossow@google.com (David Gossow)
  31
  32 #ifndef CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_
  33 #define CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_
  34
  35 #include <numeric>
  36 #include <vector>
  37
  38 #include "ceres/dynamic_cost_function.h"
  39 #include "ceres/internal/fixed_array.h"
  40 #include "ceres/internal/port.h"
  41 #include "ceres/internal/scoped_ptr.h"
  42
  43 namespace ceres {
  44
  45 // DynamicCostFunctionToFunctor allows users to use CostFunction
  46 // objects in templated functors which are to be used for automatic
  47 // differentiation. It works similar to CostFunctionToFunctor, with the
  48 // difference that it allows you to wrap a cost function with dynamic numbers
  49 // of parameters and residuals.
  50 //
  51 // For example, let us assume that
  52 //
  53 //  class IntrinsicProjection : public CostFunction {
  54 //    public:
  55 //      IntrinsicProjection(const double* observation);
  56 //      virtual bool Evaluate(double const* const* parameters,
  57 //                            double* residuals,
  58 //                            double** jacobians) const;
  59 //  };
  60 //
  61 // is a cost function that implements the projection of a point in its
  62 // local coordinate system onto its image plane and subtracts it from
  63 // the observed point projection. It can compute its residual and
  64 // either via analytic or numerical differentiation can compute its
  65 // jacobians. The intrinsics are passed in as parameters[0] and the point as
  66 // parameters[1].
  67 //
  68 // Now we would like to compose the action of this CostFunction with
  69 // the action of camera extrinsics, i.e., rotation and
  70 // translation. Say we have a templated function
  71 //
  72 //   template<typename T>
  73 //   void RotateAndTranslatePoint(double const* const* parameters,
  74 //                                double* residuals);
  75 //
  76 // Then we can now do the following,
  77 //
  78 // struct CameraProjection {
  79 //   CameraProjection(const double* observation)
  80 //       : intrinsic_projection_.(new IntrinsicProjection(observation)) {
  81 //   }
  82 //   template <typename T>
  83 //   bool operator()(T const* const* parameters,
  84 //                   T* residual) const {
  85 //     const T* rotation = parameters[0];
  86 //     const T* translation = parameters[1];
  87 //     const T* intrinsics = parameters[2];
  88 //     const T* point = parameters[3];
  89 //     T transformed_point[3];
  90 //     RotateAndTranslatePoint(rotation, translation, point, transformed_point);
  91 //
  92 //     // Note that we call intrinsic_projection_, just like it was
  93 //     // any other templated functor.
  94 //     const T* projection_parameters[2];
  95 //     projection_parameters[0] = intrinsics;
  96 //     projection_parameters[1] = transformed_point;
  97 //     return intrinsic_projection_(projection_parameters, residual);
  98 //   }
  99 //
 100 //  private:
 101 //   DynamicCostFunctionToFunctor intrinsic_projection_;
 102 // };
 103 class DynamicCostFunctionToFunctor {
 104  public:
 105   // Takes ownership of cost_function.
 106   explicit DynamicCostFunctionToFunctor(CostFunction* cost_function)
 107       : cost_function_(cost_function) {
 108     CHECK_NOTNULL(cost_function);
 109   }
 110
 111   bool operator()(double const* const* parameters, double* residuals) const {
 112     return cost_function_->Evaluate(parameters, residuals, NULL);
 113   }
 114
 115   template <typename JetT>
 116   bool operator()(JetT const* const* inputs, JetT* output) const {
 117     const std::vector<int32>& parameter_block_sizes =
 118         cost_function_->parameter_block_sizes();
 119     const int num_parameter_blocks =
 120         static_cast<int>(parameter_block_sizes.size());
 121     const int num_residuals = cost_function_->num_residuals();
 122     const int num_parameters = std::accumulate(parameter_block_sizes.begin(),
 123                                                parameter_block_sizes.end(), 0);
 124
 125     internal::FixedArray<double> parameters(num_parameters);
 126     internal::FixedArray<double*> parameter_blocks(num_parameter_blocks);
 127     internal::FixedArray<double> jacobians(num_residuals * num_parameters);
 128     internal::FixedArray<double*> jacobian_blocks(num_parameter_blocks);
 129     internal::FixedArray<double> residuals(num_residuals);
 130
 131     // Build a set of arrays to get the residuals and jacobians from
 132     // the CostFunction wrapped by this functor.
 133     double* parameter_ptr = parameters.get();
 134     double* jacobian_ptr = jacobians.get();
 135     for (int i = 0; i < num_parameter_blocks; ++i) {
 136       parameter_blocks[i] = parameter_ptr;
 137       jacobian_blocks[i] = jacobian_ptr;
 138       for (int j = 0; j < parameter_block_sizes[i]; ++j) {
 139         *parameter_ptr++ = inputs[i][j].a;
 140       }
 141       jacobian_ptr += num_residuals * parameter_block_sizes[i];
 142     }
 143
 144     if (!cost_function_->Evaluate(parameter_blocks.get(),
 145                                   residuals.get(),
 146                                   jacobian_blocks.get())) {
 147       return false;
 148     }
 149
 150     // Now that we have the incoming Jets, which are carrying the
 151     // partial derivatives of each of the inputs w.r.t to some other
 152     // underlying parameters. The derivative of the outputs of the
 153     // cost function w.r.t to the same underlying parameters can now
 154     // be computed by applying the chain rule.
 155     //
 156     //  d output[i]               d output[i]   d input[j]
 157     //  --------------  = sum_j   ----------- * ------------
 158     //  d parameter[k]            d input[j]    d parameter[k]
 159     //
 160     // d input[j]
 161     // --------------  = inputs[j], so
 162     // d parameter[k]
 163     //
 164     //  outputJet[i]  = sum_k jacobian[i][k] * inputJet[k]
 165     //
 166     // The following loop, iterates over the residuals, computing one
 167     // output jet at a time.
 168     for (int i = 0; i < num_residuals; ++i) {
 169       output[i].a = residuals[i];
 170       output[i].v.setZero();
 171
 172       for (int j = 0; j < num_parameter_blocks; ++j) {
 173         const int32 block_size = parameter_block_sizes[j];
 174         for (int k = 0; k < parameter_block_sizes[j]; ++k) {
 175           output[i].v +=
 176               jacobian_blocks[j][i * block_size + k] * inputs[j][k].v;
 177         }
 178       }
 179     }
 180
 181     return true;
 182   }
 183
 184  private:
 185   internal::scoped_ptr<CostFunction> cost_function_;
 186 };
 187
 188 }  // namespace ceres
 189
 190 #endif  // CERES_PUBLIC_DYNAMIC_COST_FUNCTION_TO_FUNCTOR_H_