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 // 29 // Author: keir (at) google.com (Keir Mierle) 30 31 #include "ceres/gradient_checking_cost_function.h" 32 33 #include <cmath> 34 #include <vector> 35 #include "ceres/cost_function.h" 36 #include "ceres/internal/scoped_ptr.h" 37 #include "ceres/local_parameterization.h" 38 #include "ceres/loss_function.h" 39 #include "ceres/parameter_block.h" 40 #include "ceres/problem_impl.h" 41 #include "ceres/program.h" 42 #include "ceres/random.h" 43 #include "ceres/residual_block.h" 44 #include "ceres/sized_cost_function.h" 45 #include "ceres/types.h" 46 #include "glog/logging.h" 47 #include "gmock/gmock.h" 48 #include "gmock/mock-log.h" 49 #include "gtest/gtest.h" 50 51 using testing::AllOf; 52 using testing::AnyNumber; 53 using testing::HasSubstr; 54 using testing::ScopedMockLog; 55 using testing::_; 56 57 namespace ceres { 58 namespace internal { 59 60 // Pick a (non-quadratic) function whose derivative are easy: 61 // 62 // f = exp(- a' x). 63 // df = - f a. 64 // 65 // where 'a' is a vector of the same size as 'x'. In the block 66 // version, they are both block vectors, of course. 67 template<int bad_block = 1, int bad_variable = 2> 68 class TestTerm : public CostFunction { 69 public: 70 // The constructor of this function needs to know the number 71 // of blocks desired, and the size of each block. 72 TestTerm(int arity, int const *dim) : arity_(arity) { 73 // Make 'arity' random vectors. 74 a_.resize(arity_); 75 for (int j = 0; j < arity_; ++j) { 76 a_[j].resize(dim[j]); 77 for (int u = 0; u < dim[j]; ++u) { 78 a_[j][u] = 2.0 * RandDouble() - 1.0; 79 } 80 } 81 82 for (int i = 0; i < arity_; i++) { 83 mutable_parameter_block_sizes()->push_back(dim[i]); 84 } 85 set_num_residuals(1); 86 } 87 88 bool Evaluate(double const* const* parameters, 89 double* residuals, 90 double** jacobians) const { 91 // Compute a . x. 92 double ax = 0; 93 for (int j = 0; j < arity_; ++j) { 94 for (int u = 0; u < parameter_block_sizes()[j]; ++u) { 95 ax += a_[j][u] * parameters[j][u]; 96 } 97 } 98 99 // This is the cost, but also appears as a factor 100 // in the derivatives. 101 double f = *residuals = exp(-ax); 102 103 // Accumulate 1st order derivatives. 104 if (jacobians) { 105 for (int j = 0; j < arity_; ++j) { 106 if (jacobians[j]) { 107 for (int u = 0; u < parameter_block_sizes()[j]; ++u) { 108 // See comments before class. 109 jacobians[j][u] = - f * a_[j][u]; 110 111 if (bad_block == j && bad_variable == u) { 112 // Whoopsiedoopsie! Deliberately introduce a faulty jacobian entry 113 // like what happens when users make an error in their jacobian 114 // computations. This should get detected. 115 LOG(INFO) << "Poisoning jacobian for parameter block " << j 116 << ", row 0, column " << u; 117 jacobians[j][u] += 500; 118 } 119 } 120 } 121 } 122 } 123 124 return true; 125 } 126 127 private: 128 int arity_; 129 vector<vector<double> > a_; 130 }; 131 132 TEST(GradientCheckingCostFunction, ResidualsAndJacobiansArePreservedTest) { 133 srand(5); 134 135 // Test with 3 blocks of size 2, 3 and 4. 136 int const arity = 3; 137 int const dim[arity] = { 2, 3, 4 }; 138 139 // Make a random set of blocks. 140 vector<double*> parameters(arity); 141 for (int j = 0; j < arity; ++j) { 142 parameters[j] = new double[dim[j]]; 143 for (int u = 0; u < dim[j]; ++u) { 144 parameters[j][u] = 2.0 * RandDouble() - 1.0; 145 } 146 } 147 148 double original_residual; 149 double residual; 150 vector<double*> original_jacobians(arity); 151 vector<double*> jacobians(arity); 152 153 for (int j = 0; j < arity; ++j) { 154 // Since residual is one dimensional the jacobians have the same 155 // size as the parameter blocks. 156 jacobians[j] = new double[dim[j]]; 157 original_jacobians[j] = new double[dim[j]]; 158 } 159 160 const double kRelativeStepSize = 1e-6; 161 const double kRelativePrecision = 1e-4; 162 163 TestTerm<-1, -1> term(arity, dim); 164 scoped_ptr<CostFunction> gradient_checking_cost_function( 165 CreateGradientCheckingCostFunction(&term, 166 kRelativeStepSize, 167 kRelativePrecision, 168 "Ignored.")); 169 term.Evaluate(¶meters[0], 170 &original_residual, 171 &original_jacobians[0]); 172 173 gradient_checking_cost_function->Evaluate(¶meters[0], 174 &residual, 175 &jacobians[0]); 176 EXPECT_EQ(original_residual, residual); 177 178 for (int j = 0; j < arity; j++) { 179 for (int k = 0; k < dim[j]; ++k) { 180 EXPECT_EQ(original_jacobians[j][k], jacobians[j][k]); 181 } 182 183 delete[] parameters[j]; 184 delete[] jacobians[j]; 185 delete[] original_jacobians[j]; 186 } 187 } 188 189 TEST(GradientCheckingCostFunction, SmokeTest) { 190 srand(5); 191 192 // Test with 3 blocks of size 2, 3 and 4. 193 int const arity = 3; 194 int const dim[arity] = { 2, 3, 4 }; 195 196 // Make a random set of blocks. 197 vector<double*> parameters(arity); 198 for (int j = 0; j < arity; ++j) { 199 parameters[j] = new double[dim[j]]; 200 for (int u = 0; u < dim[j]; ++u) { 201 parameters[j][u] = 2.0 * RandDouble() - 1.0; 202 } 203 } 204 205 double residual; 206 vector<double*> jacobians(arity); 207 for (int j = 0; j < arity; ++j) { 208 // Since residual is one dimensional the jacobians have the same size as the 209 // parameter blocks. 210 jacobians[j] = new double[dim[j]]; 211 } 212 213 const double kRelativeStepSize = 1e-6; 214 const double kRelativePrecision = 1e-4; 215 216 // Should have one term that's bad, causing everything to get dumped. 217 LOG(INFO) << "Bad gradient"; 218 { 219 TestTerm<1, 2> term(arity, dim); 220 scoped_ptr<CostFunction> gradient_checking_cost_function( 221 CreateGradientCheckingCostFunction(&term, 222 kRelativeStepSize, 223 kRelativePrecision, 224 "Fuzzy bananas")); 225 226 ScopedMockLog log; 227 EXPECT_CALL(log, Log(_, _, _)).Times(AnyNumber()); 228 EXPECT_CALL(log, Log(WARNING, _, 229 AllOf(HasSubstr("(1,0,2) Relative error worse than"), 230 HasSubstr("Fuzzy bananas")))); 231 232 gradient_checking_cost_function->Evaluate(¶meters[0], 233 &residual, 234 &jacobians[0]); 235 } 236 237 // The gradient is correct, so no errors are reported. 238 LOG(INFO) << "Good gradient"; 239 { 240 TestTerm<-1, -1> term(arity, dim); 241 scoped_ptr<CostFunction> gradient_checking_cost_function( 242 CreateGradientCheckingCostFunction(&term, 243 kRelativeStepSize, 244 kRelativePrecision, 245 "Ignored.")); 246 247 ScopedMockLog log; 248 EXPECT_CALL(log, Log(_, _, _)).Times(0); 249 250 gradient_checking_cost_function->Evaluate(¶meters[0], 251 &residual, 252 &jacobians[0]); 253 } 254 255 for (int j = 0; j < arity; j++) { 256 delete[] parameters[j]; 257 delete[] jacobians[j]; 258 } 259 } 260 261 // The following three classes are for the purposes of defining 262 // function signatures. They have dummy Evaluate functions. 263 264 // Trivial cost function that accepts a single argument. 265 class UnaryCostFunction : public CostFunction { 266 public: 267 UnaryCostFunction(int num_residuals, int32 parameter_block_size) { 268 set_num_residuals(num_residuals); 269 mutable_parameter_block_sizes()->push_back(parameter_block_size); 270 } 271 virtual ~UnaryCostFunction() {} 272 273 virtual bool Evaluate(double const* const* parameters, 274 double* residuals, 275 double** jacobians) const { 276 for (int i = 0; i < num_residuals(); ++i) { 277 residuals[i] = 1; 278 } 279 return true; 280 } 281 }; 282 283 // Trivial cost function that accepts two arguments. 284 class BinaryCostFunction: public CostFunction { 285 public: 286 BinaryCostFunction(int num_residuals, 287 int32 parameter_block1_size, 288 int32 parameter_block2_size) { 289 set_num_residuals(num_residuals); 290 mutable_parameter_block_sizes()->push_back(parameter_block1_size); 291 mutable_parameter_block_sizes()->push_back(parameter_block2_size); 292 } 293 294 virtual bool Evaluate(double const* const* parameters, 295 double* residuals, 296 double** jacobians) const { 297 for (int i = 0; i < num_residuals(); ++i) { 298 residuals[i] = 2; 299 } 300 return true; 301 } 302 }; 303 304 // Trivial cost function that accepts three arguments. 305 class TernaryCostFunction: public CostFunction { 306 public: 307 TernaryCostFunction(int num_residuals, 308 int32 parameter_block1_size, 309 int32 parameter_block2_size, 310 int32 parameter_block3_size) { 311 set_num_residuals(num_residuals); 312 mutable_parameter_block_sizes()->push_back(parameter_block1_size); 313 mutable_parameter_block_sizes()->push_back(parameter_block2_size); 314 mutable_parameter_block_sizes()->push_back(parameter_block3_size); 315 } 316 317 virtual bool Evaluate(double const* const* parameters, 318 double* residuals, 319 double** jacobians) const { 320 for (int i = 0; i < num_residuals(); ++i) { 321 residuals[i] = 3; 322 } 323 return true; 324 } 325 }; 326 327 // Verify that the two ParameterBlocks are formed from the same user 328 // array and have the same LocalParameterization object. 329 void ParameterBlocksAreEquivalent(const ParameterBlock* left, 330 const ParameterBlock* right) { 331 CHECK_NOTNULL(left); 332 CHECK_NOTNULL(right); 333 EXPECT_EQ(left->user_state(), right->user_state()); 334 EXPECT_EQ(left->Size(), right->Size()); 335 EXPECT_EQ(left->Size(), right->Size()); 336 EXPECT_EQ(left->LocalSize(), right->LocalSize()); 337 EXPECT_EQ(left->local_parameterization(), right->local_parameterization()); 338 EXPECT_EQ(left->IsConstant(), right->IsConstant()); 339 } 340 341 TEST(GradientCheckingProblemImpl, ProblemDimensionsMatch) { 342 // Parameter blocks with arbitrarily chosen initial values. 343 double x[] = {1.0, 2.0, 3.0}; 344 double y[] = {4.0, 5.0, 6.0, 7.0}; 345 double z[] = {8.0, 9.0, 10.0, 11.0, 12.0}; 346 double w[] = {13.0, 14.0, 15.0, 16.0}; 347 348 ProblemImpl problem_impl; 349 problem_impl.AddParameterBlock(x, 3); 350 problem_impl.AddParameterBlock(y, 4); 351 problem_impl.SetParameterBlockConstant(y); 352 problem_impl.AddParameterBlock(z, 5); 353 problem_impl.AddParameterBlock(w, 4, new QuaternionParameterization); 354 problem_impl.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); 355 problem_impl.AddResidualBlock(new BinaryCostFunction(6, 5, 4) , 356 NULL, z, y); 357 problem_impl.AddResidualBlock(new BinaryCostFunction(3, 3, 5), 358 new TrivialLoss, x, z); 359 problem_impl.AddResidualBlock(new BinaryCostFunction(7, 5, 3), 360 NULL, z, x); 361 problem_impl.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4), 362 NULL, z, x, y); 363 364 scoped_ptr<ProblemImpl> gradient_checking_problem_impl( 365 CreateGradientCheckingProblemImpl(&problem_impl, 1.0, 1.0)); 366 367 // The dimensions of the two problems match. 368 EXPECT_EQ(problem_impl.NumParameterBlocks(), 369 gradient_checking_problem_impl->NumParameterBlocks()); 370 EXPECT_EQ(problem_impl.NumResidualBlocks(), 371 gradient_checking_problem_impl->NumResidualBlocks()); 372 373 EXPECT_EQ(problem_impl.NumParameters(), 374 gradient_checking_problem_impl->NumParameters()); 375 EXPECT_EQ(problem_impl.NumResiduals(), 376 gradient_checking_problem_impl->NumResiduals()); 377 378 const Program& program = problem_impl.program(); 379 const Program& gradient_checking_program = 380 gradient_checking_problem_impl->program(); 381 382 // Since we added the ParameterBlocks and ResidualBlocks explicitly, 383 // they should be in the same order in the two programs. It is 384 // possible that may change due to implementation changes to 385 // Program. This is not exepected to be the case and writing code to 386 // anticipate that possibility not worth the extra complexity in 387 // this test. 388 for (int i = 0; i < program.parameter_blocks().size(); ++i) { 389 ParameterBlocksAreEquivalent( 390 program.parameter_blocks()[i], 391 gradient_checking_program.parameter_blocks()[i]); 392 } 393 394 for (int i = 0; i < program.residual_blocks().size(); ++i) { 395 // Compare the sizes of the two ResidualBlocks. 396 const ResidualBlock* original_residual_block = 397 program.residual_blocks()[i]; 398 const ResidualBlock* new_residual_block = 399 gradient_checking_program.residual_blocks()[i]; 400 EXPECT_EQ(original_residual_block->NumParameterBlocks(), 401 new_residual_block->NumParameterBlocks()); 402 EXPECT_EQ(original_residual_block->NumResiduals(), 403 new_residual_block->NumResiduals()); 404 EXPECT_EQ(original_residual_block->NumScratchDoublesForEvaluate(), 405 new_residual_block->NumScratchDoublesForEvaluate()); 406 407 // Verify that the ParameterBlocks for the two residuals are equivalent. 408 for (int j = 0; j < original_residual_block->NumParameterBlocks(); ++j) { 409 ParameterBlocksAreEquivalent( 410 original_residual_block->parameter_blocks()[j], 411 new_residual_block->parameter_blocks()[j]); 412 } 413 } 414 } 415 416 } // namespace internal 417 } // namespace ceres 418
