Ceres: Update to the latest actual version
[blender.git] / extern / ceres / internal / ceres / gradient_checking_cost_function.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 // 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:
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
84   virtual ~GradientCheckingCostFunction() { }
85
86   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
137 GradientCheckingIterationCallback::GradientCheckingIterationCallback()
138     : gradient_error_detected_(false) {
139 }
140
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 }
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
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
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