Ceres: Update to the latest actual version
[blender.git] / extern / ceres / internal / ceres / gradient_checker.cc
1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2016 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: wjr@google.com (William Rucklidge),
30 //          keir@google.com (Keir Mierle),
31 //          dgossow@google.com (David Gossow)
32
33 #include "ceres/gradient_checker.h"
34
35 #include <algorithm>
36 #include <cmath>
37 #include <numeric>
38 #include <string>
39 #include <vector>
40
41 #include "ceres/is_close.h"
42 #include "ceres/stringprintf.h"
43 #include "ceres/types.h"
44
45 namespace ceres {
46
47 using internal::IsClose;
48 using internal::StringAppendF;
49 using internal::StringPrintf;
50 using std::string;
51 using std::vector;
52
53 namespace {
54 // Evaluate the cost function and transform the returned Jacobians to
55 // the local space of the respective local parameterizations.
56 bool EvaluateCostFunction(
57     const ceres::CostFunction* function,
58     double const* const * parameters,
59     const std::vector<const ceres::LocalParameterization*>&
60         local_parameterizations,
61     Vector* residuals,
62     std::vector<Matrix>* jacobians,
63     std::vector<Matrix>* local_jacobians) {
64   CHECK_NOTNULL(residuals);
65   CHECK_NOTNULL(jacobians);
66   CHECK_NOTNULL(local_jacobians);
67
68   const vector<int32>& block_sizes = function->parameter_block_sizes();
69   const int num_parameter_blocks = block_sizes.size();
70
71   // Allocate Jacobian matrices in local space.
72   local_jacobians->resize(num_parameter_blocks);
73   vector<double*> local_jacobian_data(num_parameter_blocks);
74   for (int i = 0; i < num_parameter_blocks; ++i) {
75     int block_size = block_sizes.at(i);
76     if (local_parameterizations.at(i) != NULL) {
77       block_size = local_parameterizations.at(i)->LocalSize();
78     }
79     local_jacobians->at(i).resize(function->num_residuals(), block_size);
80     local_jacobians->at(i).setZero();
81     local_jacobian_data.at(i) = local_jacobians->at(i).data();
82   }
83
84   // Allocate Jacobian matrices in global space.
85   jacobians->resize(num_parameter_blocks);
86   vector<double*> jacobian_data(num_parameter_blocks);
87   for (int i = 0; i < num_parameter_blocks; ++i) {
88     jacobians->at(i).resize(function->num_residuals(), block_sizes.at(i));
89     jacobians->at(i).setZero();
90     jacobian_data.at(i) = jacobians->at(i).data();
91   }
92
93   // Compute residuals & jacobians.
94   CHECK_NE(0, function->num_residuals());
95   residuals->resize(function->num_residuals());
96   residuals->setZero();
97   if (!function->Evaluate(parameters, residuals->data(),
98                           jacobian_data.data())) {
99     return false;
100   }
101
102   // Convert Jacobians from global to local space.
103   for (size_t i = 0; i < local_jacobians->size(); ++i) {
104     if (local_parameterizations.at(i) == NULL) {
105       local_jacobians->at(i) = jacobians->at(i);
106     } else {
107       int global_size = local_parameterizations.at(i)->GlobalSize();
108       int local_size = local_parameterizations.at(i)->LocalSize();
109       CHECK_EQ(jacobians->at(i).cols(), global_size);
110       Matrix global_J_local(global_size, local_size);
111       local_parameterizations.at(i)->ComputeJacobian(
112           parameters[i], global_J_local.data());
113       local_jacobians->at(i) = jacobians->at(i) * global_J_local;
114     }
115   }
116   return true;
117 }
118 } // namespace
119
120 GradientChecker::GradientChecker(
121       const CostFunction* function,
122       const vector<const LocalParameterization*>* local_parameterizations,
123       const NumericDiffOptions& options) :
124         function_(function) {
125   CHECK_NOTNULL(function);
126   if (local_parameterizations != NULL) {
127     local_parameterizations_ = *local_parameterizations;
128   } else {
129     local_parameterizations_.resize(function->parameter_block_sizes().size(),
130                                     NULL);
131   }
132   DynamicNumericDiffCostFunction<CostFunction, CENTRAL>*
133       finite_diff_cost_function =
134       new DynamicNumericDiffCostFunction<CostFunction, CENTRAL>(
135           function, DO_NOT_TAKE_OWNERSHIP, options);
136   finite_diff_cost_function_.reset(finite_diff_cost_function);
137
138   const vector<int32>& parameter_block_sizes =
139       function->parameter_block_sizes();
140   const int num_parameter_blocks = parameter_block_sizes.size();
141   for (int i = 0; i < num_parameter_blocks; ++i) {
142     finite_diff_cost_function->AddParameterBlock(parameter_block_sizes[i]);
143   }
144   finite_diff_cost_function->SetNumResiduals(function->num_residuals());
145 }
146
147 bool GradientChecker::Probe(double const* const * parameters,
148                             double relative_precision,
149                             ProbeResults* results_param) const {
150   int num_residuals = function_->num_residuals();
151
152   // Make sure that we have a place to store results, no matter if the user has
153   // provided an output argument.
154   ProbeResults* results;
155   ProbeResults results_local;
156   if (results_param != NULL) {
157     results = results_param;
158     results->residuals.resize(0);
159     results->jacobians.clear();
160     results->numeric_jacobians.clear();
161     results->local_jacobians.clear();
162     results->local_numeric_jacobians.clear();
163     results->error_log.clear();
164   } else {
165     results = &results_local;
166   }
167   results->maximum_relative_error = 0.0;
168   results->return_value = true;
169
170   // Evaluate the derivative using the user supplied code.
171   vector<Matrix>& jacobians = results->jacobians;
172   vector<Matrix>& local_jacobians = results->local_jacobians;
173   if (!EvaluateCostFunction(function_, parameters, local_parameterizations_,
174                        &results->residuals, &jacobians, &local_jacobians)) {
175     results->error_log = "Function evaluation with Jacobians failed.";
176     results->return_value = false;
177   }
178
179   // Evaluate the derivative using numeric derivatives.
180   vector<Matrix>& numeric_jacobians = results->numeric_jacobians;
181   vector<Matrix>& local_numeric_jacobians = results->local_numeric_jacobians;
182   Vector finite_diff_residuals;
183   if (!EvaluateCostFunction(finite_diff_cost_function_.get(), parameters,
184                             local_parameterizations_, &finite_diff_residuals,
185                             &numeric_jacobians, &local_numeric_jacobians)) {
186     results->error_log += "\nFunction evaluation with numerical "
187         "differentiation failed.";
188     results->return_value = false;
189   }
190
191   if (!results->return_value) {
192     return false;
193   }
194
195   for (int i = 0; i < num_residuals; ++i) {
196     if (!IsClose(
197         results->residuals[i],
198         finite_diff_residuals[i],
199         relative_precision,
200         NULL,
201         NULL)) {
202       results->error_log = "Function evaluation with and without Jacobians "
203           "resulted in different residuals.";
204       LOG(INFO) << results->residuals.transpose();
205       LOG(INFO) << finite_diff_residuals.transpose();
206       return false;
207     }
208   }
209
210   // See if any elements have relative error larger than the threshold.
211   int num_bad_jacobian_components = 0;
212   double& worst_relative_error = results->maximum_relative_error;
213   worst_relative_error = 0;
214
215   // Accumulate the error message for all the jacobians, since it won't get
216   // output if there are no bad jacobian components.
217   string error_log;
218   for (int k = 0; k < function_->parameter_block_sizes().size(); k++) {
219     StringAppendF(&error_log,
220                   "========== "
221                   "Jacobian for " "block %d: (%ld by %ld)) "
222                   "==========\n",
223                   k,
224                   static_cast<long>(local_jacobians[k].rows()),
225                   static_cast<long>(local_jacobians[k].cols()));
226     // The funny spacing creates appropriately aligned column headers.
227     error_log +=
228         " block  row  col        user dx/dy    num diff dx/dy         "
229         "abs error    relative error         parameter          residual\n";
230
231     for (int i = 0; i < local_jacobians[k].rows(); i++) {
232       for (int j = 0; j < local_jacobians[k].cols(); j++) {
233         double term_jacobian = local_jacobians[k](i, j);
234         double finite_jacobian = local_numeric_jacobians[k](i, j);
235         double relative_error, absolute_error;
236         bool bad_jacobian_entry =
237             !IsClose(term_jacobian,
238                      finite_jacobian,
239                      relative_precision,
240                      &relative_error,
241                      &absolute_error);
242         worst_relative_error = std::max(worst_relative_error, relative_error);
243
244         StringAppendF(&error_log,
245                       "%6d %4d %4d %17g %17g %17g %17g %17g %17g",
246                       k, i, j,
247                       term_jacobian, finite_jacobian,
248                       absolute_error, relative_error,
249                       parameters[k][j],
250                       results->residuals[i]);
251
252         if (bad_jacobian_entry) {
253           num_bad_jacobian_components++;
254           StringAppendF(
255               &error_log,
256               " ------ (%d,%d,%d) Relative error worse than %g",
257               k, i, j, relative_precision);
258         }
259         error_log += "\n";
260       }
261     }
262   }
263
264   // Since there were some bad errors, dump comprehensive debug info.
265   if (num_bad_jacobian_components) {
266     string header = StringPrintf("\nDetected %d bad Jacobian component(s). "
267         "Worst relative error was %g.\n",
268         num_bad_jacobian_components,
269         worst_relative_error);
270      results->error_log = header + "\n" + error_log;
271     return false;
272   }
273   return true;
274 }
275
276 }  // namespace ceres