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@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
47using Eigen::Dynamic;
48
49namespace ceres {
50namespace internal {
51
52ResidualBlock::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
67bool 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
197int 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