• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
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 // Copyright 2007 Google Inc. All Rights Reserved.
29 //
30 // Author: wjr@google.com (William Rucklidge)
31 //
32 // This file contains a class that exercises a cost function, to make sure
33 // that it is computing reasonable derivatives. It compares the Jacobians
34 // computed by the cost function with those obtained by finite
35 // differences.
36 
37 #ifndef CERES_PUBLIC_GRADIENT_CHECKER_H_
38 #define CERES_PUBLIC_GRADIENT_CHECKER_H_
39 
40 #include <cstddef>
41 #include <algorithm>
42 #include <vector>
43 
44 #include "ceres/internal/eigen.h"
45 #include "ceres/internal/fixed_array.h"
46 #include "ceres/internal/macros.h"
47 #include "ceres/internal/scoped_ptr.h"
48 #include "ceres/numeric_diff_cost_function.h"
49 #include "glog/logging.h"
50 
51 namespace ceres {
52 
53 // An object that exercises a cost function, to compare the answers that it
54 // gives with derivatives estimated using finite differencing.
55 //
56 // The only likely usage of this is for testing.
57 //
58 // How to use: Fill in an array of pointers to parameter blocks for your
59 // CostFunction, and then call Probe(). Check that the return value is
60 // 'true'. See prober_test.cc for an example.
61 //
62 // This is templated similarly to NumericDiffCostFunction, as it internally
63 // uses that.
64 template <typename CostFunctionToProbe,
65           int M = 0, int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0>
66 class GradientChecker {
67  public:
68   // Here we stash some results from the probe, for later
69   // inspection.
70   struct GradientCheckResults {
71     // Computed cost.
72     Vector cost;
73 
74     // The sizes of these matrices are dictated by the cost function's
75     // parameter and residual block sizes. Each vector's length will
76     // term->parameter_block_sizes().size(), and each matrix is the
77     // Jacobian of the residual with respect to the corresponding parameter
78     // block.
79 
80     // Derivatives as computed by the cost function.
81     vector<Matrix> term_jacobians;
82 
83     // Derivatives as computed by finite differencing.
84     vector<Matrix> finite_difference_jacobians;
85 
86     // Infinity-norm of term_jacobians - finite_difference_jacobians.
87     double error_jacobians;
88   };
89 
90   // Checks the Jacobian computed by a cost function.
91   //
92   // probe_point: The parameter values at which to probe.
93   // error_tolerance: A threshold for the infinity-norm difference
94   // between the Jacobians. If the Jacobians differ by more than
95   // this amount, then the probe fails.
96   //
97   // term: The cost function to test. Not retained after this call returns.
98   //
99   // results: On return, the two Jacobians (and other information)
100   // will be stored here.  May be NULL.
101   //
102   // Returns true if no problems are detected and the difference between the
103   // Jacobians is less than error_tolerance.
Probe(double const * const * probe_point,double error_tolerance,CostFunctionToProbe * term,GradientCheckResults * results)104   static bool Probe(double const* const* probe_point,
105                     double error_tolerance,
106                     CostFunctionToProbe *term,
107                     GradientCheckResults* results) {
108     CHECK_NOTNULL(probe_point);
109     CHECK_NOTNULL(term);
110     LOG(INFO) << "-------------------- Starting Probe() --------------------";
111 
112     // We need a GradientCheckeresults, whether or not they supplied one.
113     internal::scoped_ptr<GradientCheckResults> owned_results;
114     if (results == NULL) {
115       owned_results.reset(new GradientCheckResults);
116       results = owned_results.get();
117     }
118 
119     // Do a consistency check between the term and the template parameters.
120     CHECK_EQ(M, term->num_residuals());
121     const int num_residuals = M;
122     const vector<int32>& block_sizes = term->parameter_block_sizes();
123     const int num_blocks = block_sizes.size();
124 
125     CHECK_LE(num_blocks, 5) << "Unable to test functions that take more "
126                             << "than 5 parameter blocks";
127     if (N0) {
128       CHECK_EQ(N0, block_sizes[0]);
129       CHECK_GE(num_blocks, 1);
130     } else {
131       CHECK_LT(num_blocks, 1);
132     }
133     if (N1) {
134       CHECK_EQ(N1, block_sizes[1]);
135       CHECK_GE(num_blocks, 2);
136     } else {
137       CHECK_LT(num_blocks, 2);
138     }
139     if (N2) {
140       CHECK_EQ(N2, block_sizes[2]);
141       CHECK_GE(num_blocks, 3);
142     } else {
143       CHECK_LT(num_blocks, 3);
144     }
145     if (N3) {
146       CHECK_EQ(N3, block_sizes[3]);
147       CHECK_GE(num_blocks, 4);
148     } else {
149       CHECK_LT(num_blocks, 4);
150     }
151     if (N4) {
152       CHECK_EQ(N4, block_sizes[4]);
153       CHECK_GE(num_blocks, 5);
154     } else {
155       CHECK_LT(num_blocks, 5);
156     }
157 
158     results->term_jacobians.clear();
159     results->term_jacobians.resize(num_blocks);
160     results->finite_difference_jacobians.clear();
161     results->finite_difference_jacobians.resize(num_blocks);
162 
163     internal::FixedArray<double*> term_jacobian_pointers(num_blocks);
164     internal::FixedArray<double*>
165         finite_difference_jacobian_pointers(num_blocks);
166     for (int i = 0; i < num_blocks; i++) {
167       results->term_jacobians[i].resize(num_residuals, block_sizes[i]);
168       term_jacobian_pointers[i] = results->term_jacobians[i].data();
169       results->finite_difference_jacobians[i].resize(
170           num_residuals, block_sizes[i]);
171       finite_difference_jacobian_pointers[i] =
172           results->finite_difference_jacobians[i].data();
173     }
174     results->cost.resize(num_residuals, 1);
175 
176     CHECK(term->Evaluate(probe_point, results->cost.data(),
177                          term_jacobian_pointers.get()));
178     NumericDiffCostFunction<CostFunctionToProbe, CENTRAL, M, N0, N1, N2, N3, N4>
179         numeric_term(term, DO_NOT_TAKE_OWNERSHIP);
180     CHECK(numeric_term.Evaluate(probe_point, results->cost.data(),
181                                 finite_difference_jacobian_pointers.get()));
182 
183     results->error_jacobians = 0;
184     for (int i = 0; i < num_blocks; i++) {
185       Matrix jacobian_difference = results->term_jacobians[i] -
186           results->finite_difference_jacobians[i];
187       results->error_jacobians =
188           std::max(results->error_jacobians,
189                    jacobian_difference.lpNorm<Eigen::Infinity>());
190     }
191 
192     LOG(INFO) << "========== term-computed derivatives ==========";
193     for (int i = 0; i < num_blocks; i++) {
194       LOG(INFO) << "term_computed block " << i;
195       LOG(INFO) << "\n" << results->term_jacobians[i];
196     }
197 
198     LOG(INFO) << "========== finite-difference derivatives ==========";
199     for (int i = 0; i < num_blocks; i++) {
200       LOG(INFO) << "finite_difference block " << i;
201       LOG(INFO) << "\n" << results->finite_difference_jacobians[i];
202     }
203 
204     LOG(INFO) << "========== difference ==========";
205     for (int i = 0; i < num_blocks; i++) {
206       LOG(INFO) << "difference block " << i;
207       LOG(INFO) << (results->term_jacobians[i] -
208                     results->finite_difference_jacobians[i]);
209     }
210 
211     LOG(INFO) << "||difference|| = " << results->error_jacobians;
212 
213     return results->error_jacobians < error_tolerance;
214   }
215 
216  private:
217   CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(GradientChecker);
218 };
219 
220 }  // namespace ceres
221 
222 #endif  // CERES_PUBLIC_GRADIENT_CHECKER_H_
223