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