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 <algorithm>
41#include <cstddef>
42#include <vector>
43
44#include <glog/logging.h>
45#include "ceres/internal/eigen.h"
46#include "ceres/internal/fixed_array.h"
47#include "ceres/internal/macros.h"
48#include "ceres/internal/scoped_ptr.h"
49#include "ceres/numeric_diff_cost_function.h"
50
51namespace 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.
64template <typename CostFunctionToProbe,
65          int M = 0, int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0>
66class 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.
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<int16>& 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*> finite_difference_jacobian_pointers(num_blocks);
165    for (int i = 0; i < num_blocks; i++) {
166      results->term_jacobians[i].resize(num_residuals, block_sizes[i]);
167      term_jacobian_pointers[i] = results->term_jacobians[i].data();
168      results->finite_difference_jacobians[i].resize(
169          num_residuals, block_sizes[i]);
170      finite_difference_jacobian_pointers[i] =
171          results->finite_difference_jacobians[i].data();
172    }
173    results->cost.resize(num_residuals, 1);
174
175    CHECK(term->Evaluate(probe_point, results->cost.data(),
176                         term_jacobian_pointers.get()));
177    NumericDiffCostFunction<CostFunctionToProbe, CENTRAL, M, N0, N1, N2, N3, N4>
178        numeric_term(term, DO_NOT_TAKE_OWNERSHIP);
179    CHECK(numeric_term.Evaluate(probe_point, results->cost.data(),
180                                finite_difference_jacobian_pointers.get()));
181
182    results->error_jacobians = 0;
183    for (int i = 0; i < num_blocks; i++) {
184      Matrix jacobian_difference = results->term_jacobians[i] -
185          results->finite_difference_jacobians[i];
186      results->error_jacobians =
187          std::max(results->error_jacobians,
188                   jacobian_difference.lpNorm<Eigen::Infinity>());
189    }
190
191    LOG(INFO) << "========== term-computed derivatives ==========";
192    for (int i = 0; i < num_blocks; i++) {
193      LOG(INFO) << "term_computed block " << i;
194      LOG(INFO) << "\n" << results->term_jacobians[i];
195    }
196
197    LOG(INFO) << "========== finite-difference derivatives ==========";
198    for (int i = 0; i < num_blocks; i++) {
199      LOG(INFO) << "finite_difference block " << i;
200      LOG(INFO) << "\n" << results->finite_difference_jacobians[i];
201    }
202
203    LOG(INFO) << "========== difference ==========";
204    for (int i = 0; i < num_blocks; i++) {
205      LOG(INFO) << "difference block " << i;
206      LOG(INFO) << (results->term_jacobians[i] -
207                    results->finite_difference_jacobians[i]);
208    }
209
210    LOG(INFO) << "||difference|| = " << results->error_jacobians;
211
212    return results->error_jacobians < error_tolerance;
213  }
214
215 private:
216  CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(GradientChecker);
217};
218
219}  // namespace ceres
220
221#endif  // CERES_PUBLIC_GRADIENT_CHECKER_H_
222