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
39 namespace ceres {
40 namespace internal {
41
42 // Trivial cost function that accepts three arguments.
43 class TernaryCostFunction: public CostFunction {
44 public:
TernaryCostFunction(int num_residuals,int32 parameter_block1_size,int32 parameter_block2_size,int32 parameter_block3_size)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
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const55 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
TEST(ResidualBlock,EvaluteWithNoLossFunctionOrLocalParameterizations)75 TEST(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.
168 class LocallyParameterizedCostFunction: public SizedCostFunction<3, 2, 3, 4> {
169 public:
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const170 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
TEST(ResidualBlock,EvaluteWithLocalParameterizations)202 TEST(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