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@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
51using testing::AllOf;
52using testing::AnyNumber;
53using testing::HasSubstr;
54using testing::ScopedMockLog;
55using testing::_;
56
57namespace ceres {
58namespace 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.
67template<int bad_block = 1, int bad_variable = 2>
68class 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
132TEST(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(&parameters[0],
170                &original_residual,
171                &original_jacobians[0]);
172
173  gradient_checking_cost_function->Evaluate(&parameters[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
189TEST(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(&parameters[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(&parameters[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.
265class 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.
284class 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.
305class 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.
329void 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
341TEST(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