internal/ceres/residual_block.cc

   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 //         sameeragarwal@google.com (Sameer Agarwal)
  31
  32 #include "ceres/residual_block.h"
  33
  34 #include <algorithm>
  35 #include <cstddef>
  36 #include <vector>
  37 #include "ceres/corrector.h"
  38 #include "ceres/parameter_block.h"
  39 #include "ceres/residual_block_utils.h"
  40 #include "ceres/cost_function.h"
  41 #include "ceres/internal/eigen.h"
  42 #include "ceres/internal/fixed_array.h"
  43 #include "ceres/local_parameterization.h"
  44 #include "ceres/loss_function.h"
  45 #include "ceres/small_blas.h"
  46
  47 using Eigen::Dynamic;
  48
  49 namespace ceres {
  50 namespace internal {
  51
  52 ResidualBlock::ResidualBlock(
  53     const CostFunction* cost_function,
  54     const LossFunction* loss_function,
  55     const std::vector<ParameterBlock*>& parameter_blocks,
  56     int index)
  57     : cost_function_(cost_function),
  58       loss_function_(loss_function),
  59       parameter_blocks_(
  60           new ParameterBlock* [
  61               cost_function->parameter_block_sizes().size()]),
  62       index_(index) {
  63   std::copy(parameter_blocks.begin(),
  64             parameter_blocks.end(),
  65             parameter_blocks_.get());
  66 }
  67
  68 bool ResidualBlock::Evaluate(const bool apply_loss_function,
  69                              double* cost,
  70                              double* residuals,
  71                              double** jacobians,
  72                              double* scratch) const {
  73   const int num_parameter_blocks = NumParameterBlocks();
  74   const int num_residuals = cost_function_->num_residuals();
  75
  76   // Collect the parameters from their blocks. This will rarely allocate, since
  77   // residuals taking more than 8 parameter block arguments are rare.
  78   FixedArray<const double*, 8> parameters(num_parameter_blocks);
  79   for (int i = 0; i < num_parameter_blocks; ++i) {
  80     parameters[i] = parameter_blocks_[i]->state();
  81   }
  82
  83   // Put pointers into the scratch space into global_jacobians as appropriate.
  84   FixedArray<double*, 8> global_jacobians(num_parameter_blocks);
  85   if (jacobians != NULL) {
  86     for (int i = 0; i < num_parameter_blocks; ++i) {
  87       const ParameterBlock* parameter_block = parameter_blocks_[i];
  88       if (jacobians[i] != NULL &&
  89           parameter_block->LocalParameterizationJacobian() != NULL) {
  90         global_jacobians[i] = scratch;
  91         scratch += num_residuals * parameter_block->Size();
  92       } else {
  93         global_jacobians[i] = jacobians[i];
  94       }
  95     }
  96   }
  97
  98   // If the caller didn't request residuals, use the scratch space for them.
  99   bool outputting_residuals = (residuals != NULL);
 100   if (!outputting_residuals) {
 101     residuals = scratch;
 102   }
 103
 104   // Invalidate the evaluation buffers so that we can check them after
 105   // the CostFunction::Evaluate call, to see if all the return values
 106   // that were required were written to and that they are finite.
 107   double** eval_jacobians = (jacobians != NULL) ? global_jacobians.get() : NULL;
 108
 109   InvalidateEvaluation(*this, cost, residuals, eval_jacobians);
 110
 111   if (!cost_function_->Evaluate(parameters.get(), residuals, eval_jacobians)) {
 112     return false;
 113   }
 114
 115   if (!IsEvaluationValid(*this,
 116                          parameters.get(),
 117                          cost,
 118                          residuals,
 119                          eval_jacobians)) {
 120     std::string message =
 121         "\n\n"
 122         "Error in evaluating the ResidualBlock.\n\n"
 123         "There are two possible reasons. Either the CostFunction did not evaluate and fill all    \n"  // NOLINT
 124         "residual and jacobians that were requested or there was a non-finite value (nan/infinite)\n"  // NOLINT
 125         "generated during the or jacobian computation. \n\n" +
 126         EvaluationToString(*this,
 127                            parameters.get(),
 128                            cost,
 129                            residuals,
 130                            eval_jacobians);
 131     LOG(WARNING) << message;
 132     return false;
 133   }
 134
 135   double squared_norm = VectorRef(residuals, num_residuals).squaredNorm();
 136
 137   // Update the jacobians with the local parameterizations.
 138   if (jacobians != NULL) {
 139     for (int i = 0; i < num_parameter_blocks; ++i) {
 140       if (jacobians[i] != NULL) {
 141         const ParameterBlock* parameter_block = parameter_blocks_[i];
 142
 143         // Apply local reparameterization to the jacobians.
 144         if (parameter_block->LocalParameterizationJacobian() != NULL) {
 145           // jacobians[i] = global_jacobians[i] * global_to_local_jacobian.
 146           MatrixMatrixMultiply<Dynamic, Dynamic, Dynamic, Dynamic, 0>(
 147               global_jacobians[i],
 148               num_residuals,
 149               parameter_block->Size(),
 150               parameter_block->LocalParameterizationJacobian(),
 151               parameter_block->Size(),
 152               parameter_block->LocalSize(),
 153               jacobians[i], 0, 0,  num_residuals, parameter_block->LocalSize());
 154         }
 155       }
 156     }
 157   }
 158
 159   if (loss_function_ == NULL || !apply_loss_function) {
 160     *cost = 0.5 * squared_norm;
 161     return true;
 162   }
 163
 164   double rho[3];
 165   loss_function_->Evaluate(squared_norm, rho);
 166   *cost = 0.5 * rho[0];
 167
 168   // No jacobians and not outputting residuals? All done. Doing an early exit
 169   // here avoids constructing the "Corrector" object below in a common case.
 170   if (jacobians == NULL && !outputting_residuals) {
 171     return true;
 172   }
 173
 174   // Correct for the effects of the loss function. The jacobians need to be
 175   // corrected before the residuals, since they use the uncorrected residuals.
 176   Corrector correct(squared_norm, rho);
 177   if (jacobians != NULL) {
 178     for (int i = 0; i < num_parameter_blocks; ++i) {
 179       if (jacobians[i] != NULL) {
 180         const ParameterBlock* parameter_block = parameter_blocks_[i];
 181
 182         // Correct the jacobians for the loss function.
 183         correct.CorrectJacobian(num_residuals,
 184                                 parameter_block->LocalSize(),
 185                                 residuals,
 186                                 jacobians[i]);
 187       }
 188     }
 189   }
 190
 191   // Correct the residuals with the loss function.
 192   if (outputting_residuals) {
 193     correct.CorrectResiduals(num_residuals, residuals);
 194   }
 195   return true;
 196 }
 197
 198 int ResidualBlock::NumScratchDoublesForEvaluate() const {
 199   // Compute the amount of scratch space needed to store the full-sized
 200   // jacobians. For parameters that have no local parameterization  no storage
 201   // is needed and the passed-in jacobian array is used directly. Also include
 202   // space to store the residuals, which is needed for cost-only evaluations.
 203   // This is slightly pessimistic, since both won't be needed all the time, but
 204   // the amount of excess should not cause problems for the caller.
 205   int num_parameters = NumParameterBlocks();
 206   int scratch_doubles = 1;
 207   for (int i = 0; i < num_parameters; ++i) {
 208     const ParameterBlock* parameter_block = parameter_blocks_[i];
 209     if (!parameter_block->IsConstant() &&
 210         parameter_block->LocalParameterizationJacobian() != NULL) {
 211       scratch_doubles += parameter_block->Size();
 212     }
 213   }
 214   scratch_doubles *= NumResiduals();
 215   return scratch_doubles;
 216 }
 217
 218 }  // namespace internal
 219 }  // namespace ceres