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/residual_block.h"
32
33#include "gtest/gtest.h"
34#include "ceres/parameter_block.h"
35#include "ceres/sized_cost_function.h"
36#include "ceres/internal/eigen.h"
37#include "ceres/local_parameterization.h"
38
39namespace ceres {
40namespace internal {
41
42// Trivial cost function that accepts three arguments.
43class TernaryCostFunction: public CostFunction {
44 public:
45  TernaryCostFunction(int num_residuals,
46                      int32 parameter_block1_size,
47                      int32 parameter_block2_size,
48                      int32 parameter_block3_size) {
49    set_num_residuals(num_residuals);
50    mutable_parameter_block_sizes()->push_back(parameter_block1_size);
51    mutable_parameter_block_sizes()->push_back(parameter_block2_size);
52    mutable_parameter_block_sizes()->push_back(parameter_block3_size);
53  }
54
55  virtual bool Evaluate(double const* const* parameters,
56                        double* residuals,
57                        double** jacobians) const {
58    for (int i = 0; i < num_residuals(); ++i) {
59      residuals[i] = i;
60    }
61    if (jacobians) {
62      for (int k = 0; k < 3; ++k) {
63        if (jacobians[k] != NULL) {
64          MatrixRef jacobian(jacobians[k],
65                             num_residuals(),
66                             parameter_block_sizes()[k]);
67          jacobian.setConstant(k);
68        }
69      }
70    }
71    return true;
72  }
73};
74
75TEST(ResidualBlock, EvaluteWithNoLossFunctionOrLocalParameterizations) {
76  double scratch[64];
77
78  // Prepare the parameter blocks.
79  double values_x[2];
80  ParameterBlock x(values_x, 2, -1);
81
82  double values_y[3];
83  ParameterBlock y(values_y, 3, -1);
84
85  double values_z[4];
86  ParameterBlock z(values_z, 4, -1);
87
88  vector<ParameterBlock*> parameters;
89  parameters.push_back(&x);
90  parameters.push_back(&y);
91  parameters.push_back(&z);
92
93  TernaryCostFunction cost_function(3, 2, 3, 4);
94
95  // Create the object under tests.
96  ResidualBlock residual_block(&cost_function, NULL, parameters, -1);
97
98  // Verify getters.
99  EXPECT_EQ(&cost_function, residual_block.cost_function());
100  EXPECT_EQ(NULL, residual_block.loss_function());
101  EXPECT_EQ(parameters[0], residual_block.parameter_blocks()[0]);
102  EXPECT_EQ(parameters[1], residual_block.parameter_blocks()[1]);
103  EXPECT_EQ(parameters[2], residual_block.parameter_blocks()[2]);
104  EXPECT_EQ(3, residual_block.NumScratchDoublesForEvaluate());
105
106  // Verify cost-only evaluation.
107  double cost;
108  residual_block.Evaluate(true, &cost, NULL, NULL, scratch);
109  EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
110
111  // Verify cost and residual evaluation.
112  double residuals[3];
113  residual_block.Evaluate(true, &cost, residuals, NULL, scratch);
114  EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
115  EXPECT_EQ(0.0, residuals[0]);
116  EXPECT_EQ(1.0, residuals[1]);
117  EXPECT_EQ(2.0, residuals[2]);
118
119  // Verify cost, residual, and jacobian evaluation.
120  cost = 0.0;
121  VectorRef(residuals, 3).setConstant(0.0);
122
123  Matrix jacobian_rx(3, 2);
124  Matrix jacobian_ry(3, 3);
125  Matrix jacobian_rz(3, 4);
126
127  jacobian_rx.setConstant(-1.0);
128  jacobian_ry.setConstant(-1.0);
129  jacobian_rz.setConstant(-1.0);
130
131  double *jacobian_ptrs[3] = {
132    jacobian_rx.data(),
133    jacobian_ry.data(),
134    jacobian_rz.data()
135  };
136
137  residual_block.Evaluate(true, &cost, residuals, jacobian_ptrs, scratch);
138  EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
139  EXPECT_EQ(0.0, residuals[0]);
140  EXPECT_EQ(1.0, residuals[1]);
141  EXPECT_EQ(2.0, residuals[2]);
142
143  EXPECT_TRUE((jacobian_rx.array() == 0.0).all()) << "\n" << jacobian_rx;
144  EXPECT_TRUE((jacobian_ry.array() == 1.0).all()) << "\n" << jacobian_ry;
145  EXPECT_TRUE((jacobian_rz.array() == 2.0).all()) << "\n" << jacobian_rz;
146
147  // Verify cost, residual, and partial jacobian evaluation.
148  cost = 0.0;
149  VectorRef(residuals, 3).setConstant(0.0);
150  jacobian_rx.setConstant(-1.0);
151  jacobian_ry.setConstant(-1.0);
152  jacobian_rz.setConstant(-1.0);
153
154  jacobian_ptrs[1] = NULL;  // Don't compute the jacobian for y.
155
156  residual_block.Evaluate(true, &cost, residuals, jacobian_ptrs, scratch);
157  EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
158  EXPECT_EQ(0.0, residuals[0]);
159  EXPECT_EQ(1.0, residuals[1]);
160  EXPECT_EQ(2.0, residuals[2]);
161
162  EXPECT_TRUE((jacobian_rx.array() ==  0.0).all()) << "\n" << jacobian_rx;
163  EXPECT_TRUE((jacobian_ry.array() == -1.0).all()) << "\n" << jacobian_ry;
164  EXPECT_TRUE((jacobian_rz.array() ==  2.0).all()) << "\n" << jacobian_rz;
165}
166
167// Trivial cost function that accepts three arguments.
168class LocallyParameterizedCostFunction: public SizedCostFunction<3, 2, 3, 4> {
169 public:
170  virtual bool Evaluate(double const* const* parameters,
171                        double* residuals,
172                        double** jacobians) const {
173    for (int i = 0; i < num_residuals(); ++i) {
174      residuals[i] = i;
175    }
176    if (jacobians) {
177      for (int k = 0; k < 3; ++k) {
178        // The jacobians here are full sized, but they are transformed in the
179        // evaluator into the "local" jacobian. In the tests, the "subset
180        // constant" parameterization is used, which should pick out columns
181        // from these jacobians. Put values in the jacobian that make this
182        // obvious; in particular, make the jacobians like this:
183        //
184        //   0 1 2 3 4 ...
185        //   0 1 2 3 4 ...
186        //   0 1 2 3 4 ...
187        //
188        if (jacobians[k] != NULL) {
189          MatrixRef jacobian(jacobians[k],
190                             num_residuals(),
191                             parameter_block_sizes()[k]);
192          for (int j = 0; j < k + 2; ++j) {
193            jacobian.col(j).setConstant(j);
194          }
195        }
196      }
197    }
198    return true;
199  }
200};
201
202TEST(ResidualBlock, EvaluteWithLocalParameterizations) {
203  double scratch[64];
204
205  // Prepare the parameter blocks.
206  double values_x[2];
207  ParameterBlock x(values_x, 2, -1);
208
209  double values_y[3];
210  ParameterBlock y(values_y, 3, -1);
211
212  double values_z[4];
213  ParameterBlock z(values_z, 4, -1);
214
215  vector<ParameterBlock*> parameters;
216  parameters.push_back(&x);
217  parameters.push_back(&y);
218  parameters.push_back(&z);
219
220  // Make x have the first component fixed.
221  vector<int> x_fixed;
222  x_fixed.push_back(0);
223  SubsetParameterization x_parameterization(2, x_fixed);
224  x.SetParameterization(&x_parameterization);
225
226  // Make z have the last and last component fixed.
227  vector<int> z_fixed;
228  z_fixed.push_back(2);
229  SubsetParameterization z_parameterization(4, z_fixed);
230  z.SetParameterization(&z_parameterization);
231
232  LocallyParameterizedCostFunction cost_function;
233
234  // Create the object under tests.
235  ResidualBlock residual_block(&cost_function, NULL, parameters, -1);
236
237  // Verify getters.
238  EXPECT_EQ(&cost_function, residual_block.cost_function());
239  EXPECT_EQ(NULL, residual_block.loss_function());
240  EXPECT_EQ(parameters[0], residual_block.parameter_blocks()[0]);
241  EXPECT_EQ(parameters[1], residual_block.parameter_blocks()[1]);
242  EXPECT_EQ(parameters[2], residual_block.parameter_blocks()[2]);
243  EXPECT_EQ(3*(2 + 4) + 3, residual_block.NumScratchDoublesForEvaluate());
244
245  // Verify cost-only evaluation.
246  double cost;
247  residual_block.Evaluate(true, &cost, NULL, NULL, scratch);
248  EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
249
250  // Verify cost and residual evaluation.
251  double residuals[3];
252  residual_block.Evaluate(true, &cost, residuals, NULL, scratch);
253  EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
254  EXPECT_EQ(0.0, residuals[0]);
255  EXPECT_EQ(1.0, residuals[1]);
256  EXPECT_EQ(2.0, residuals[2]);
257
258  // Verify cost, residual, and jacobian evaluation.
259  cost = 0.0;
260  VectorRef(residuals, 3).setConstant(0.0);
261
262  Matrix jacobian_rx(3, 1);  // Since the first element is fixed.
263  Matrix jacobian_ry(3, 3);
264  Matrix jacobian_rz(3, 3);  // Since the third element is fixed.
265
266  jacobian_rx.setConstant(-1.0);
267  jacobian_ry.setConstant(-1.0);
268  jacobian_rz.setConstant(-1.0);
269
270  double *jacobian_ptrs[3] = {
271    jacobian_rx.data(),
272    jacobian_ry.data(),
273    jacobian_rz.data()
274  };
275
276  residual_block.Evaluate(true, &cost, residuals, jacobian_ptrs, scratch);
277  EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
278  EXPECT_EQ(0.0, residuals[0]);
279  EXPECT_EQ(1.0, residuals[1]);
280  EXPECT_EQ(2.0, residuals[2]);
281
282  Matrix expected_jacobian_rx(3, 1);
283  expected_jacobian_rx << 1.0, 1.0, 1.0;
284
285  Matrix expected_jacobian_ry(3, 3);
286  expected_jacobian_ry << 0.0, 1.0, 2.0,
287                          0.0, 1.0, 2.0,
288                          0.0, 1.0, 2.0;
289
290  Matrix expected_jacobian_rz(3, 3);
291  expected_jacobian_rz << 0.0, 1.0, /* 2.0, */ 3.0,  // 3rd parameter constant.
292                          0.0, 1.0, /* 2.0, */ 3.0,
293                          0.0, 1.0, /* 2.0, */ 3.0;
294
295  EXPECT_EQ(expected_jacobian_rx, jacobian_rx)
296      << "\nExpected:\n" << expected_jacobian_rx
297      << "\nActual:\n"   << jacobian_rx;
298  EXPECT_EQ(expected_jacobian_ry, jacobian_ry)
299      << "\nExpected:\n" << expected_jacobian_ry
300      << "\nActual:\n"   << jacobian_ry;
301  EXPECT_EQ(expected_jacobian_rz, jacobian_rz)
302      << "\nExpected:\n " << expected_jacobian_rz
303      << "\nActual:\n"   << jacobian_rz;
304
305  // Verify cost, residual, and partial jacobian evaluation.
306  cost = 0.0;
307  VectorRef(residuals, 3).setConstant(0.0);
308  jacobian_rx.setConstant(-1.0);
309  jacobian_ry.setConstant(-1.0);
310  jacobian_rz.setConstant(-1.0);
311
312  jacobian_ptrs[1] = NULL;  // Don't compute the jacobian for y.
313
314  residual_block.Evaluate(true, &cost, residuals, jacobian_ptrs, scratch);
315  EXPECT_EQ(0.5 * (0*0 + 1*1 + 2*2), cost);
316  EXPECT_EQ(0.0, residuals[0]);
317  EXPECT_EQ(1.0, residuals[1]);
318  EXPECT_EQ(2.0, residuals[2]);
319
320  EXPECT_EQ(expected_jacobian_rx, jacobian_rx);
321  EXPECT_TRUE((jacobian_ry.array() == -1.0).all()) << "\n" << jacobian_ry;
322  EXPECT_EQ(expected_jacobian_rz, jacobian_rz);
323}
324
325}  // namespace internal
326}  // namespace ceres
327