1// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2013 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: sameeragarwal@google.com (Sameer Agarwal)
30//         mierle@gmail.com (Keir Mierle)
31
32#include <cstddef>
33
34#include "ceres/dynamic_numeric_diff_cost_function.h"
35#include "ceres/internal/scoped_ptr.h"
36#include "gtest/gtest.h"
37
38namespace ceres {
39namespace internal {
40
41const double kTolerance = 1e-6;
42
43// Takes 2 parameter blocks:
44//     parameters[0] is size 10.
45//     parameters[1] is size 5.
46// Emits 21 residuals:
47//     A: i - parameters[0][i], for i in [0,10)  -- this is 10 residuals
48//     B: parameters[0][i] - i, for i in [0,10)  -- this is another 10.
49//     C: sum(parameters[0][i]^2 - 8*parameters[0][i]) + sum(parameters[1][i])
50class MyCostFunctor {
51 public:
52  bool operator()(double const* const* parameters, double* residuals) const {
53    const double* params0 = parameters[0];
54    int r = 0;
55    for (int i = 0; i < 10; ++i) {
56      residuals[r++] = i - params0[i];
57      residuals[r++] = params0[i] - i;
58    }
59
60    double c_residual = 0.0;
61    for (int i = 0; i < 10; ++i) {
62      c_residual += pow(params0[i], 2) - 8.0 * params0[i];
63    }
64
65    const double* params1 = parameters[1];
66    for (int i = 0; i < 5; ++i) {
67      c_residual += params1[i];
68    }
69    residuals[r++] = c_residual;
70    return true;
71  }
72};
73
74TEST(DynamicNumericdiffCostFunctionTest, TestResiduals) {
75  vector<double> param_block_0(10, 0.0);
76  vector<double> param_block_1(5, 0.0);
77  DynamicNumericDiffCostFunction<MyCostFunctor> cost_function(
78      new MyCostFunctor());
79  cost_function.AddParameterBlock(param_block_0.size());
80  cost_function.AddParameterBlock(param_block_1.size());
81  cost_function.SetNumResiduals(21);
82
83  // Test residual computation.
84  vector<double> residuals(21, -100000);
85  vector<double*> parameter_blocks(2);
86  parameter_blocks[0] = &param_block_0[0];
87  parameter_blocks[1] = &param_block_1[0];
88  EXPECT_TRUE(cost_function.Evaluate(&parameter_blocks[0],
89                                     residuals.data(),
90                                     NULL));
91  for (int r = 0; r < 10; ++r) {
92    EXPECT_EQ(1.0 * r, residuals.at(r * 2));
93    EXPECT_EQ(-1.0 * r, residuals.at(r * 2 + 1));
94  }
95  EXPECT_EQ(0, residuals.at(20));
96}
97
98
99TEST(DynamicNumericdiffCostFunctionTest, TestJacobian) {
100  // Test the residual counting.
101  vector<double> param_block_0(10, 0.0);
102  for (int i = 0; i < 10; ++i) {
103    param_block_0[i] = 2 * i;
104  }
105  vector<double> param_block_1(5, 0.0);
106  DynamicNumericDiffCostFunction<MyCostFunctor> cost_function(
107      new MyCostFunctor());
108  cost_function.AddParameterBlock(param_block_0.size());
109  cost_function.AddParameterBlock(param_block_1.size());
110  cost_function.SetNumResiduals(21);
111
112  // Prepare the residuals.
113  vector<double> residuals(21, -100000);
114
115  // Prepare the parameters.
116  vector<double*> parameter_blocks(2);
117  parameter_blocks[0] = &param_block_0[0];
118  parameter_blocks[1] = &param_block_1[0];
119
120  // Prepare the jacobian.
121  vector<vector<double> > jacobian_vect(2);
122  jacobian_vect[0].resize(21 * 10, -100000);
123  jacobian_vect[1].resize(21 * 5, -100000);
124  vector<double*> jacobian;
125  jacobian.push_back(jacobian_vect[0].data());
126  jacobian.push_back(jacobian_vect[1].data());
127
128  // Test jacobian computation.
129  EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(),
130                                     residuals.data(),
131                                     jacobian.data()));
132
133  for (int r = 0; r < 10; ++r) {
134    EXPECT_EQ(-1.0 * r, residuals.at(r * 2));
135    EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1));
136  }
137  EXPECT_EQ(420, residuals.at(20));
138  for (int p = 0; p < 10; ++p) {
139    // Check "A" Jacobian.
140    EXPECT_NEAR(-1.0, jacobian_vect[0][2*p * 10 + p], kTolerance);
141    // Check "B" Jacobian.
142    EXPECT_NEAR(+1.0, jacobian_vect[0][(2*p+1) * 10 + p], kTolerance);
143    jacobian_vect[0][2*p * 10 + p] = 0.0;
144    jacobian_vect[0][(2*p+1) * 10 + p] = 0.0;
145  }
146
147  // Check "C" Jacobian for first parameter block.
148  for (int p = 0; p < 10; ++p) {
149    EXPECT_NEAR(4 * p - 8, jacobian_vect[0][20 * 10 + p], kTolerance);
150    jacobian_vect[0][20 * 10 + p] = 0.0;
151  }
152  for (int i = 0; i < jacobian_vect[0].size(); ++i) {
153    EXPECT_NEAR(0.0, jacobian_vect[0][i], kTolerance);
154  }
155
156  // Check "C" Jacobian for second parameter block.
157  for (int p = 0; p < 5; ++p) {
158    EXPECT_NEAR(1.0, jacobian_vect[1][20 * 5 + p], kTolerance);
159    jacobian_vect[1][20 * 5 + p] = 0.0;
160  }
161  for (int i = 0; i < jacobian_vect[1].size(); ++i) {
162    EXPECT_NEAR(0.0, jacobian_vect[1][i], kTolerance);
163  }
164}
165
166TEST(DynamicNumericdiffCostFunctionTest, JacobianWithFirstParameterBlockConstant) {  // NOLINT
167  // Test the residual counting.
168  vector<double> param_block_0(10, 0.0);
169  for (int i = 0; i < 10; ++i) {
170    param_block_0[i] = 2 * i;
171  }
172  vector<double> param_block_1(5, 0.0);
173  DynamicNumericDiffCostFunction<MyCostFunctor> cost_function(
174      new MyCostFunctor());
175  cost_function.AddParameterBlock(param_block_0.size());
176  cost_function.AddParameterBlock(param_block_1.size());
177  cost_function.SetNumResiduals(21);
178
179  // Prepare the residuals.
180  vector<double> residuals(21, -100000);
181
182  // Prepare the parameters.
183  vector<double*> parameter_blocks(2);
184  parameter_blocks[0] = &param_block_0[0];
185  parameter_blocks[1] = &param_block_1[0];
186
187  // Prepare the jacobian.
188  vector<vector<double> > jacobian_vect(2);
189  jacobian_vect[0].resize(21 * 10, -100000);
190  jacobian_vect[1].resize(21 * 5, -100000);
191  vector<double*> jacobian;
192  jacobian.push_back(NULL);
193  jacobian.push_back(jacobian_vect[1].data());
194
195  // Test jacobian computation.
196  EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(),
197                                     residuals.data(),
198                                     jacobian.data()));
199
200  for (int r = 0; r < 10; ++r) {
201    EXPECT_EQ(-1.0 * r, residuals.at(r * 2));
202    EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1));
203  }
204  EXPECT_EQ(420, residuals.at(20));
205
206  // Check "C" Jacobian for second parameter block.
207  for (int p = 0; p < 5; ++p) {
208    EXPECT_NEAR(1.0, jacobian_vect[1][20 * 5 + p], kTolerance);
209    jacobian_vect[1][20 * 5 + p] = 0.0;
210  }
211  for (int i = 0; i < jacobian_vect[1].size(); ++i) {
212    EXPECT_EQ(0.0, jacobian_vect[1][i]);
213  }
214}
215
216TEST(DynamicNumericdiffCostFunctionTest, JacobianWithSecondParameterBlockConstant) {  // NOLINT
217  // Test the residual counting.
218  vector<double> param_block_0(10, 0.0);
219  for (int i = 0; i < 10; ++i) {
220    param_block_0[i] = 2 * i;
221  }
222  vector<double> param_block_1(5, 0.0);
223  DynamicNumericDiffCostFunction<MyCostFunctor> cost_function(
224      new MyCostFunctor());
225  cost_function.AddParameterBlock(param_block_0.size());
226  cost_function.AddParameterBlock(param_block_1.size());
227  cost_function.SetNumResiduals(21);
228
229  // Prepare the residuals.
230  vector<double> residuals(21, -100000);
231
232  // Prepare the parameters.
233  vector<double*> parameter_blocks(2);
234  parameter_blocks[0] = &param_block_0[0];
235  parameter_blocks[1] = &param_block_1[0];
236
237  // Prepare the jacobian.
238  vector<vector<double> > jacobian_vect(2);
239  jacobian_vect[0].resize(21 * 10, -100000);
240  jacobian_vect[1].resize(21 * 5, -100000);
241  vector<double*> jacobian;
242  jacobian.push_back(jacobian_vect[0].data());
243  jacobian.push_back(NULL);
244
245  // Test jacobian computation.
246  EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.data(),
247                                     residuals.data(),
248                                     jacobian.data()));
249
250  for (int r = 0; r < 10; ++r) {
251    EXPECT_EQ(-1.0 * r, residuals.at(r * 2));
252    EXPECT_EQ(+1.0 * r, residuals.at(r * 2 + 1));
253  }
254  EXPECT_EQ(420, residuals.at(20));
255  for (int p = 0; p < 10; ++p) {
256    // Check "A" Jacobian.
257    EXPECT_NEAR(-1.0, jacobian_vect[0][2*p * 10 + p], kTolerance);
258    // Check "B" Jacobian.
259    EXPECT_NEAR(+1.0, jacobian_vect[0][(2*p+1) * 10 + p], kTolerance);
260    jacobian_vect[0][2*p * 10 + p] = 0.0;
261    jacobian_vect[0][(2*p+1) * 10 + p] = 0.0;
262  }
263
264  // Check "C" Jacobian for first parameter block.
265  for (int p = 0; p < 10; ++p) {
266    EXPECT_NEAR(4 * p - 8, jacobian_vect[0][20 * 10 + p], kTolerance);
267    jacobian_vect[0][20 * 10 + p] = 0.0;
268  }
269  for (int i = 0; i < jacobian_vect[0].size(); ++i) {
270    EXPECT_EQ(0.0, jacobian_vect[0][i]);
271  }
272}
273
274// Takes 3 parameter blocks:
275//     parameters[0] (x) is size 1.
276//     parameters[1] (y) is size 2.
277//     parameters[2] (z) is size 3.
278// Emits 7 residuals:
279//     A: x[0] (= sum_x)
280//     B: y[0] + 2.0 * y[1] (= sum_y)
281//     C: z[0] + 3.0 * z[1] + 6.0 * z[2] (= sum_z)
282//     D: sum_x * sum_y
283//     E: sum_y * sum_z
284//     F: sum_x * sum_z
285//     G: sum_x * sum_y * sum_z
286class MyThreeParameterCostFunctor {
287 public:
288  template <typename T>
289  bool operator()(T const* const* parameters, T* residuals) const {
290    const T* x = parameters[0];
291    const T* y = parameters[1];
292    const T* z = parameters[2];
293
294    T sum_x = x[0];
295    T sum_y = y[0] + 2.0 * y[1];
296    T sum_z = z[0] + 3.0 * z[1] + 6.0 * z[2];
297
298    residuals[0] = sum_x;
299    residuals[1] = sum_y;
300    residuals[2] = sum_z;
301    residuals[3] = sum_x * sum_y;
302    residuals[4] = sum_y * sum_z;
303    residuals[5] = sum_x * sum_z;
304    residuals[6] = sum_x * sum_y * sum_z;
305    return true;
306  }
307};
308
309class ThreeParameterCostFunctorTest : public ::testing::Test {
310 protected:
311  virtual void SetUp() {
312    // Prepare the parameters.
313    x_.resize(1);
314    x_[0] = 0.0;
315
316    y_.resize(2);
317    y_[0] = 1.0;
318    y_[1] = 3.0;
319
320    z_.resize(3);
321    z_[0] = 2.0;
322    z_[1] = 4.0;
323    z_[2] = 6.0;
324
325    parameter_blocks_.resize(3);
326    parameter_blocks_[0] = &x_[0];
327    parameter_blocks_[1] = &y_[0];
328    parameter_blocks_[2] = &z_[0];
329
330    // Prepare the cost function.
331    typedef DynamicNumericDiffCostFunction<MyThreeParameterCostFunctor>
332      DynamicMyThreeParameterCostFunction;
333    DynamicMyThreeParameterCostFunction * cost_function =
334      new DynamicMyThreeParameterCostFunction(
335        new MyThreeParameterCostFunctor());
336    cost_function->AddParameterBlock(1);
337    cost_function->AddParameterBlock(2);
338    cost_function->AddParameterBlock(3);
339    cost_function->SetNumResiduals(7);
340
341    cost_function_.reset(cost_function);
342
343    // Setup jacobian data.
344    jacobian_vect_.resize(3);
345    jacobian_vect_[0].resize(7 * x_.size(), -100000);
346    jacobian_vect_[1].resize(7 * y_.size(), -100000);
347    jacobian_vect_[2].resize(7 * z_.size(), -100000);
348
349    // Prepare the expected residuals.
350    const double sum_x = x_[0];
351    const double sum_y = y_[0] + 2.0 * y_[1];
352    const double sum_z = z_[0] + 3.0 * z_[1] + 6.0 * z_[2];
353
354    expected_residuals_.resize(7);
355    expected_residuals_[0] = sum_x;
356    expected_residuals_[1] = sum_y;
357    expected_residuals_[2] = sum_z;
358    expected_residuals_[3] = sum_x * sum_y;
359    expected_residuals_[4] = sum_y * sum_z;
360    expected_residuals_[5] = sum_x * sum_z;
361    expected_residuals_[6] = sum_x * sum_y * sum_z;
362
363    // Prepare the expected jacobian entries.
364    expected_jacobian_x_.resize(7);
365    expected_jacobian_x_[0] = 1.0;
366    expected_jacobian_x_[1] = 0.0;
367    expected_jacobian_x_[2] = 0.0;
368    expected_jacobian_x_[3] = sum_y;
369    expected_jacobian_x_[4] = 0.0;
370    expected_jacobian_x_[5] = sum_z;
371    expected_jacobian_x_[6] = sum_y * sum_z;
372
373    expected_jacobian_y_.resize(14);
374    expected_jacobian_y_[0] = 0.0;
375    expected_jacobian_y_[1] = 0.0;
376    expected_jacobian_y_[2] = 1.0;
377    expected_jacobian_y_[3] = 2.0;
378    expected_jacobian_y_[4] = 0.0;
379    expected_jacobian_y_[5] = 0.0;
380    expected_jacobian_y_[6] = sum_x;
381    expected_jacobian_y_[7] = 2.0 * sum_x;
382    expected_jacobian_y_[8] = sum_z;
383    expected_jacobian_y_[9] = 2.0 * sum_z;
384    expected_jacobian_y_[10] = 0.0;
385    expected_jacobian_y_[11] = 0.0;
386    expected_jacobian_y_[12] = sum_x * sum_z;
387    expected_jacobian_y_[13] = 2.0 * sum_x * sum_z;
388
389    expected_jacobian_z_.resize(21);
390    expected_jacobian_z_[0] = 0.0;
391    expected_jacobian_z_[1] = 0.0;
392    expected_jacobian_z_[2] = 0.0;
393    expected_jacobian_z_[3] = 0.0;
394    expected_jacobian_z_[4] = 0.0;
395    expected_jacobian_z_[5] = 0.0;
396    expected_jacobian_z_[6] = 1.0;
397    expected_jacobian_z_[7] = 3.0;
398    expected_jacobian_z_[8] = 6.0;
399    expected_jacobian_z_[9] = 0.0;
400    expected_jacobian_z_[10] = 0.0;
401    expected_jacobian_z_[11] = 0.0;
402    expected_jacobian_z_[12] = sum_y;
403    expected_jacobian_z_[13] = 3.0 * sum_y;
404    expected_jacobian_z_[14] = 6.0 * sum_y;
405    expected_jacobian_z_[15] = sum_x;
406    expected_jacobian_z_[16] = 3.0 * sum_x;
407    expected_jacobian_z_[17] = 6.0 * sum_x;
408    expected_jacobian_z_[18] = sum_x * sum_y;
409    expected_jacobian_z_[19] = 3.0 * sum_x * sum_y;
410    expected_jacobian_z_[20] = 6.0 * sum_x * sum_y;
411  }
412
413 protected:
414  vector<double> x_;
415  vector<double> y_;
416  vector<double> z_;
417
418  vector<double*> parameter_blocks_;
419
420  scoped_ptr<CostFunction> cost_function_;
421
422  vector<vector<double> > jacobian_vect_;
423
424  vector<double> expected_residuals_;
425
426  vector<double> expected_jacobian_x_;
427  vector<double> expected_jacobian_y_;
428  vector<double> expected_jacobian_z_;
429};
430
431TEST_F(ThreeParameterCostFunctorTest, TestThreeParameterResiduals) {
432  vector<double> residuals(7, -100000);
433  EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(),
434                                       residuals.data(),
435                                       NULL));
436  for (int i = 0; i < 7; ++i) {
437    EXPECT_EQ(expected_residuals_[i], residuals[i]);
438  }
439}
440
441TEST_F(ThreeParameterCostFunctorTest, TestThreeParameterJacobian) {
442  vector<double> residuals(7, -100000);
443
444  vector<double*> jacobian;
445  jacobian.push_back(jacobian_vect_[0].data());
446  jacobian.push_back(jacobian_vect_[1].data());
447  jacobian.push_back(jacobian_vect_[2].data());
448
449  EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(),
450                                       residuals.data(),
451                                       jacobian.data()));
452
453  for (int i = 0; i < 7; ++i) {
454    EXPECT_EQ(expected_residuals_[i], residuals[i]);
455  }
456
457  for (int i = 0; i < 7; ++i) {
458    EXPECT_NEAR(expected_jacobian_x_[i], jacobian[0][i], kTolerance);
459  }
460
461  for (int i = 0; i < 14; ++i) {
462    EXPECT_NEAR(expected_jacobian_y_[i], jacobian[1][i], kTolerance);
463  }
464
465  for (int i = 0; i < 21; ++i) {
466    EXPECT_NEAR(expected_jacobian_z_[i], jacobian[2][i], kTolerance);
467  }
468}
469
470TEST_F(ThreeParameterCostFunctorTest,
471       ThreeParameterJacobianWithFirstAndLastParameterBlockConstant) {
472  vector<double> residuals(7, -100000);
473
474  vector<double*> jacobian;
475  jacobian.push_back(NULL);
476  jacobian.push_back(jacobian_vect_[1].data());
477  jacobian.push_back(NULL);
478
479  EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(),
480                                       residuals.data(),
481                                       jacobian.data()));
482
483  for (int i = 0; i < 7; ++i) {
484    EXPECT_EQ(expected_residuals_[i], residuals[i]);
485  }
486
487  for (int i = 0; i < 14; ++i) {
488    EXPECT_NEAR(expected_jacobian_y_[i], jacobian[1][i], kTolerance);
489  }
490}
491
492TEST_F(ThreeParameterCostFunctorTest,
493       ThreeParameterJacobianWithSecondParameterBlockConstant) {
494  vector<double> residuals(7, -100000);
495
496  vector<double*> jacobian;
497  jacobian.push_back(jacobian_vect_[0].data());
498  jacobian.push_back(NULL);
499  jacobian.push_back(jacobian_vect_[2].data());
500
501  EXPECT_TRUE(cost_function_->Evaluate(parameter_blocks_.data(),
502                                       residuals.data(),
503                                       jacobian.data()));
504
505  for (int i = 0; i < 7; ++i) {
506    EXPECT_EQ(expected_residuals_[i], residuals[i]);
507  }
508
509  for (int i = 0; i < 7; ++i) {
510    EXPECT_NEAR(expected_jacobian_x_[i], jacobian[0][i], kTolerance);
511  }
512
513  for (int i = 0; i < 21; ++i) {
514    EXPECT_NEAR(expected_jacobian_z_[i], jacobian[2][i], kTolerance);
515  }
516}
517
518}  // namespace internal
519}  // namespace ceres
520