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
47 using Eigen::Dynamic;
48
49 namespace ceres {
50 namespace internal {
51
ResidualBlock(const CostFunction * cost_function,const LossFunction * loss_function,const vector<ParameterBlock * > & parameter_blocks,int index)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
Evaluate(const bool apply_loss_function,double * cost,double * residuals,double ** jacobians,double * scratch) const67 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
NumScratchDoublesForEvaluate() const197 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