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 (at) 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. 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
