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 // Authors: keir@google.com (Keir Mierle),
30 // dgossow@google.com (David Gossow)
31
32 #include "ceres/gradient_checking_cost_function.h"
33
34 #include <algorithm>
35 #include <cmath>
36 #include <numeric>
37 #include <string>
38 #include <vector>
39
40 #include "ceres/gradient_checker.h"
41 #include "ceres/internal/eigen.h"
42 #include "ceres/internal/scoped_ptr.h"
43 #include "ceres/parameter_block.h"
44 #include "ceres/problem.h"
45 #include "ceres/problem_impl.h"
46 #include "ceres/program.h"
47 #include "ceres/residual_block.h"
48 #include "ceres/dynamic_numeric_diff_cost_function.h"
49 #include "ceres/stringprintf.h"
50 #include "ceres/types.h"
51 #include "glog/logging.h"
52
53 namespace ceres {
54 namespace internal {
55
56 using std::abs;
57 using std::max;
58 using std::string;
59 using std::vector;
60
61 namespace {
62
63 class GradientCheckingCostFunction : public CostFunction {
64 public:
GradientCheckingCostFunction(const CostFunction * function,const std::vector<const LocalParameterization * > * local_parameterizations,const NumericDiffOptions & options,double relative_precision,const string & extra_info,GradientCheckingIterationCallback * callback)65 GradientCheckingCostFunction(
66 const CostFunction* function,
67 const std::vector<const LocalParameterization*>* local_parameterizations,
68 const NumericDiffOptions& options,
69 double relative_precision,
70 const string& extra_info,
71 GradientCheckingIterationCallback* callback)
72 : function_(function),
73 gradient_checker_(function, local_parameterizations, options),
74 relative_precision_(relative_precision),
75 extra_info_(extra_info),
76 callback_(callback) {
77 CHECK_NOTNULL(callback_);
78 const vector<int32>& parameter_block_sizes =
79 function->parameter_block_sizes();
80 *mutable_parameter_block_sizes() = parameter_block_sizes;
81 set_num_residuals(function->num_residuals());
82 }
83
~GradientCheckingCostFunction()84 virtual ~GradientCheckingCostFunction() { }
85
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const86 virtual bool Evaluate(double const* const* parameters,
87 double* residuals,
88 double** jacobians) const {
89 if (!jacobians) {
90 // Nothing to check in this case; just forward.
91 return function_->Evaluate(parameters, residuals, NULL);
92 }
93
94 GradientChecker::ProbeResults results;
95 bool okay = gradient_checker_.Probe(parameters,
96 relative_precision_,
97 &results);
98
99 // If the cost function returned false, there's nothing we can say about
100 // the gradients.
101 if (results.return_value == false) {
102 return false;
103 }
104
105 // Copy the residuals.
106 const int num_residuals = function_->num_residuals();
107 MatrixRef(residuals, num_residuals, 1) = results.residuals;
108
109 // Copy the original jacobian blocks into the jacobians array.
110 const vector<int32>& block_sizes = function_->parameter_block_sizes();
111 for (int k = 0; k < block_sizes.size(); k++) {
112 if (jacobians[k] != NULL) {
113 MatrixRef(jacobians[k],
114 results.jacobians[k].rows(),
115 results.jacobians[k].cols()) = results.jacobians[k];
116 }
117 }
118
119 if (!okay) {
120 std::string error_log = "Gradient Error detected!\nExtra info for "
121 "this residual: " + extra_info_ + "\n" + results.error_log;
122 callback_->SetGradientErrorDetected(error_log);
123 }
124 return true;
125 }
126
127 private:
128 const CostFunction* function_;
129 GradientChecker gradient_checker_;
130 double relative_precision_;
131 string extra_info_;
132 GradientCheckingIterationCallback* callback_;
133 };
134
135 } // namespace
136
GradientCheckingIterationCallback()137 GradientCheckingIterationCallback::GradientCheckingIterationCallback()
138 : gradient_error_detected_(false) {
139 }
140
operator ()(const IterationSummary & summary)141 CallbackReturnType GradientCheckingIterationCallback::operator()(
142 const IterationSummary& summary) {
143 if (gradient_error_detected_) {
144 LOG(ERROR)<< "Gradient error detected. Terminating solver.";
145 return SOLVER_ABORT;
146 }
147 return SOLVER_CONTINUE;
148 }
SetGradientErrorDetected(std::string & error_log)149 void GradientCheckingIterationCallback::SetGradientErrorDetected(
150 std::string& error_log) {
151 mutex_.Lock();
152 gradient_error_detected_ = true;
153 error_log_ += "\n" + error_log;
154 mutex_.Unlock();
155 }
156
CreateGradientCheckingCostFunction(const CostFunction * cost_function,const std::vector<const LocalParameterization * > * local_parameterizations,double relative_step_size,double relative_precision,const std::string & extra_info,GradientCheckingIterationCallback * callback)157 CostFunction* CreateGradientCheckingCostFunction(
158 const CostFunction* cost_function,
159 const std::vector<const LocalParameterization*>* local_parameterizations,
160 double relative_step_size,
161 double relative_precision,
162 const std::string& extra_info,
163 GradientCheckingIterationCallback* callback) {
164 NumericDiffOptions numeric_diff_options;
165 numeric_diff_options.relative_step_size = relative_step_size;
166
167 return new GradientCheckingCostFunction(cost_function,
168 local_parameterizations,
169 numeric_diff_options,
170 relative_precision, extra_info,
171 callback);
172 }
173
CreateGradientCheckingProblemImpl(ProblemImpl * problem_impl,double relative_step_size,double relative_precision,GradientCheckingIterationCallback * callback)174 ProblemImpl* CreateGradientCheckingProblemImpl(
175 ProblemImpl* problem_impl,
176 double relative_step_size,
177 double relative_precision,
178 GradientCheckingIterationCallback* callback) {
179 CHECK_NOTNULL(callback);
180 // We create new CostFunctions by wrapping the original CostFunction
181 // in a gradient checking CostFunction. So its okay for the
182 // ProblemImpl to take ownership of it and destroy it. The
183 // LossFunctions and LocalParameterizations are reused and since
184 // they are owned by problem_impl, gradient_checking_problem_impl
185 // should not take ownership of it.
186 Problem::Options gradient_checking_problem_options;
187 gradient_checking_problem_options.cost_function_ownership = TAKE_OWNERSHIP;
188 gradient_checking_problem_options.loss_function_ownership =
189 DO_NOT_TAKE_OWNERSHIP;
190 gradient_checking_problem_options.local_parameterization_ownership =
191 DO_NOT_TAKE_OWNERSHIP;
192
193 NumericDiffOptions numeric_diff_options;
194 numeric_diff_options.relative_step_size = relative_step_size;
195
196 ProblemImpl* gradient_checking_problem_impl = new ProblemImpl(
197 gradient_checking_problem_options);
198
199 Program* program = problem_impl->mutable_program();
200
201 // For every ParameterBlock in problem_impl, create a new parameter
202 // block with the same local parameterization and constancy.
203 const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks();
204 for (int i = 0; i < parameter_blocks.size(); ++i) {
205 ParameterBlock* parameter_block = parameter_blocks[i];
206 gradient_checking_problem_impl->AddParameterBlock(
207 parameter_block->mutable_user_state(),
208 parameter_block->Size(),
209 parameter_block->mutable_local_parameterization());
210
211 if (parameter_block->IsConstant()) {
212 gradient_checking_problem_impl->SetParameterBlockConstant(
213 parameter_block->mutable_user_state());
214 }
215 }
216
217 // For every ResidualBlock in problem_impl, create a new
218 // ResidualBlock by wrapping its CostFunction inside a
219 // GradientCheckingCostFunction.
220 const vector<ResidualBlock*>& residual_blocks = program->residual_blocks();
221 for (int i = 0; i < residual_blocks.size(); ++i) {
222 ResidualBlock* residual_block = residual_blocks[i];
223
224 // Build a human readable string which identifies the
225 // ResidualBlock. This is used by the GradientCheckingCostFunction
226 // when logging debugging information.
227 string extra_info = StringPrintf(
228 "Residual block id %d; depends on parameters [", i);
229 vector<double*> parameter_blocks;
230 vector<const LocalParameterization*> local_parameterizations;
231 parameter_blocks.reserve(residual_block->NumParameterBlocks());
232 local_parameterizations.reserve(residual_block->NumParameterBlocks());
233 for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) {
234 ParameterBlock* parameter_block = residual_block->parameter_blocks()[j];
235 parameter_blocks.push_back(parameter_block->mutable_user_state());
236 StringAppendF(&extra_info, "%p", parameter_block->mutable_user_state());
237 extra_info += (j < residual_block->NumParameterBlocks() - 1) ? ", " : "]";
238 local_parameterizations.push_back(problem_impl->GetParameterization(
239 parameter_block->mutable_user_state()));
240 }
241
242 // Wrap the original CostFunction in a GradientCheckingCostFunction.
243 CostFunction* gradient_checking_cost_function =
244 new GradientCheckingCostFunction(residual_block->cost_function(),
245 &local_parameterizations,
246 numeric_diff_options,
247 relative_precision,
248 extra_info,
249 callback);
250
251 // The const_cast is necessary because
252 // ProblemImpl::AddResidualBlock can potentially take ownership of
253 // the LossFunction, but in this case we are guaranteed that this
254 // will not be the case, so this const_cast is harmless.
255 gradient_checking_problem_impl->AddResidualBlock(
256 gradient_checking_cost_function,
257 const_cast<LossFunction*>(residual_block->loss_function()),
258 parameter_blocks);
259 }
260
261 // Normally, when a problem is given to the solver, we guarantee
262 // that the state pointers for each parameter block point to the
263 // user provided data. Since we are creating this new problem from a
264 // problem given to us at an arbitrary stage of the solve, we cannot
265 // depend on this being the case, so we explicitly call
266 // SetParameterBlockStatePtrsToUserStatePtrs to ensure that this is
267 // the case.
268 gradient_checking_problem_impl
269 ->mutable_program()
270 ->SetParameterBlockStatePtrsToUserStatePtrs();
271
272 return gradient_checking_problem_impl;
273 }
274
275
276 } // namespace internal
277 } // namespace ceres
278