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 // Tests shared across evaluators. The tests try all combinations of linear
32 // solver and num_eliminate_blocks (for schur-based solvers).
33
34 #include "ceres/evaluator.h"
35
36 #include "ceres/casts.h"
37 #include "ceres/cost_function.h"
38 #include "ceres/crs_matrix.h"
39 #include "ceres/evaluator_test_utils.h"
40 #include "ceres/internal/eigen.h"
41 #include "ceres/internal/scoped_ptr.h"
42 #include "ceres/local_parameterization.h"
43 #include "ceres/problem_impl.h"
44 #include "ceres/program.h"
45 #include "ceres/sized_cost_function.h"
46 #include "ceres/sparse_matrix.h"
47 #include "ceres/stringprintf.h"
48 #include "ceres/types.h"
49 #include "gtest/gtest.h"
50
51 namespace ceres {
52 namespace internal {
53
54 // TODO(keir): Consider pushing this into a common test utils file.
55 template<int kFactor, int kNumResiduals,
56 int N0 = 0, int N1 = 0, int N2 = 0, bool kSucceeds = true>
57 class ParameterIgnoringCostFunction
58 : public SizedCostFunction<kNumResiduals, N0, N1, N2> {
59 typedef SizedCostFunction<kNumResiduals, N0, N1, N2> Base;
60 public:
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const61 virtual bool Evaluate(double const* const* parameters,
62 double* residuals,
63 double** jacobians) const {
64 for (int i = 0; i < Base::num_residuals(); ++i) {
65 residuals[i] = i + 1;
66 }
67 if (jacobians) {
68 for (int k = 0; k < Base::parameter_block_sizes().size(); ++k) {
69 // The jacobians here are full sized, but they are transformed in the
70 // evaluator into the "local" jacobian. In the tests, the "subset
71 // constant" parameterization is used, which should pick out columns
72 // from these jacobians. Put values in the jacobian that make this
73 // obvious; in particular, make the jacobians like this:
74 //
75 // 1 2 3 4 ...
76 // 1 2 3 4 ... .* kFactor
77 // 1 2 3 4 ...
78 //
79 // where the multiplication by kFactor makes it easier to distinguish
80 // between Jacobians of different residuals for the same parameter.
81 if (jacobians[k] != NULL) {
82 MatrixRef jacobian(jacobians[k],
83 Base::num_residuals(),
84 Base::parameter_block_sizes()[k]);
85 for (int j = 0; j < Base::parameter_block_sizes()[k]; ++j) {
86 jacobian.col(j).setConstant(kFactor * (j + 1));
87 }
88 }
89 }
90 }
91 return kSucceeds;
92 }
93 };
94
95 struct EvaluatorTestOptions {
EvaluatorTestOptionsceres::internal::EvaluatorTestOptions96 EvaluatorTestOptions(LinearSolverType linear_solver_type,
97 int num_eliminate_blocks,
98 bool dynamic_sparsity = false)
99 : linear_solver_type(linear_solver_type),
100 num_eliminate_blocks(num_eliminate_blocks),
101 dynamic_sparsity(dynamic_sparsity) {}
102
103 LinearSolverType linear_solver_type;
104 int num_eliminate_blocks;
105 bool dynamic_sparsity;
106 };
107
108 struct EvaluatorTest
109 : public ::testing::TestWithParam<EvaluatorTestOptions> {
CreateEvaluatorceres::internal::EvaluatorTest110 Evaluator* CreateEvaluator(Program* program) {
111 // This program is straight from the ProblemImpl, and so has no index/offset
112 // yet; compute it here as required by the evalutor implementations.
113 program->SetParameterOffsetsAndIndex();
114
115 if (VLOG_IS_ON(1)) {
116 string report;
117 StringAppendF(&report, "Creating evaluator with type: %d",
118 GetParam().linear_solver_type);
119 if (GetParam().linear_solver_type == SPARSE_NORMAL_CHOLESKY) {
120 StringAppendF(&report, ", dynamic_sparsity: %d",
121 GetParam().dynamic_sparsity);
122 }
123 StringAppendF(&report, " and num_eliminate_blocks: %d",
124 GetParam().num_eliminate_blocks);
125 VLOG(1) << report;
126 }
127 Evaluator::Options options;
128 options.linear_solver_type = GetParam().linear_solver_type;
129 options.num_eliminate_blocks = GetParam().num_eliminate_blocks;
130 options.dynamic_sparsity = GetParam().dynamic_sparsity;
131 string error;
132 return Evaluator::Create(options, program, &error);
133 }
134
EvaluateAndCompareceres::internal::EvaluatorTest135 void EvaluateAndCompare(ProblemImpl *problem,
136 int expected_num_rows,
137 int expected_num_cols,
138 double expected_cost,
139 const double* expected_residuals,
140 const double* expected_gradient,
141 const double* expected_jacobian) {
142 scoped_ptr<Evaluator> evaluator(
143 CreateEvaluator(problem->mutable_program()));
144 int num_residuals = expected_num_rows;
145 int num_parameters = expected_num_cols;
146
147 double cost = -1;
148
149 Vector residuals(num_residuals);
150 residuals.setConstant(-2000);
151
152 Vector gradient(num_parameters);
153 gradient.setConstant(-3000);
154
155 scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
156
157 ASSERT_EQ(expected_num_rows, evaluator->NumResiduals());
158 ASSERT_EQ(expected_num_cols, evaluator->NumEffectiveParameters());
159 ASSERT_EQ(expected_num_rows, jacobian->num_rows());
160 ASSERT_EQ(expected_num_cols, jacobian->num_cols());
161
162 vector<double> state(evaluator->NumParameters());
163
164 ASSERT_TRUE(evaluator->Evaluate(
165 &state[0],
166 &cost,
167 expected_residuals != NULL ? &residuals[0] : NULL,
168 expected_gradient != NULL ? &gradient[0] : NULL,
169 expected_jacobian != NULL ? jacobian.get() : NULL));
170
171 Matrix actual_jacobian;
172 if (expected_jacobian != NULL) {
173 jacobian->ToDenseMatrix(&actual_jacobian);
174 }
175
176 CompareEvaluations(expected_num_rows,
177 expected_num_cols,
178 expected_cost,
179 expected_residuals,
180 expected_gradient,
181 expected_jacobian,
182 cost,
183 &residuals[0],
184 &gradient[0],
185 actual_jacobian.data());
186 }
187
188 // Try all combinations of parameters for the evaluator.
CheckAllEvaluationCombinationsceres::internal::EvaluatorTest189 void CheckAllEvaluationCombinations(const ExpectedEvaluation &expected) {
190 for (int i = 0; i < 8; ++i) {
191 EvaluateAndCompare(&problem,
192 expected.num_rows,
193 expected.num_cols,
194 expected.cost,
195 (i & 1) ? expected.residuals : NULL,
196 (i & 2) ? expected.gradient : NULL,
197 (i & 4) ? expected.jacobian : NULL);
198 }
199 }
200
201 // The values are ignored completely by the cost function.
202 double x[2];
203 double y[3];
204 double z[4];
205
206 ProblemImpl problem;
207 };
208
SetSparseMatrixConstant(SparseMatrix * sparse_matrix,double value)209 void SetSparseMatrixConstant(SparseMatrix* sparse_matrix, double value) {
210 VectorRef(sparse_matrix->mutable_values(),
211 sparse_matrix->num_nonzeros()).setConstant(value);
212 }
213
TEST_P(EvaluatorTest,SingleResidualProblem)214 TEST_P(EvaluatorTest, SingleResidualProblem) {
215 problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 3, 2, 3, 4>,
216 NULL,
217 x, y, z);
218
219 ExpectedEvaluation expected = {
220 // Rows/columns
221 3, 9,
222 // Cost
223 7.0,
224 // Residuals
225 { 1.0, 2.0, 3.0 },
226 // Gradient
227 { 6.0, 12.0, // x
228 6.0, 12.0, 18.0, // y
229 6.0, 12.0, 18.0, 24.0, // z
230 },
231 // Jacobian
232 // x y z
233 { 1, 2, 1, 2, 3, 1, 2, 3, 4,
234 1, 2, 1, 2, 3, 1, 2, 3, 4,
235 1, 2, 1, 2, 3, 1, 2, 3, 4
236 }
237 };
238 CheckAllEvaluationCombinations(expected);
239 }
240
TEST_P(EvaluatorTest,SingleResidualProblemWithPermutedParameters)241 TEST_P(EvaluatorTest, SingleResidualProblemWithPermutedParameters) {
242 // Add the parameters in explicit order to force the ordering in the program.
243 problem.AddParameterBlock(x, 2);
244 problem.AddParameterBlock(y, 3);
245 problem.AddParameterBlock(z, 4);
246
247 // Then use a cost function which is similar to the others, but swap around
248 // the ordering of the parameters to the cost function. This shouldn't affect
249 // the jacobian evaluation, but requires explicit handling in the evaluators.
250 // At one point the compressed row evaluator had a bug that went undetected
251 // for a long time, since by chance most users added parameters to the problem
252 // in the same order that they occured as parameters to a cost function.
253 problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 3, 4, 3, 2>,
254 NULL,
255 z, y, x);
256
257 ExpectedEvaluation expected = {
258 // Rows/columns
259 3, 9,
260 // Cost
261 7.0,
262 // Residuals
263 { 1.0, 2.0, 3.0 },
264 // Gradient
265 { 6.0, 12.0, // x
266 6.0, 12.0, 18.0, // y
267 6.0, 12.0, 18.0, 24.0, // z
268 },
269 // Jacobian
270 // x y z
271 { 1, 2, 1, 2, 3, 1, 2, 3, 4,
272 1, 2, 1, 2, 3, 1, 2, 3, 4,
273 1, 2, 1, 2, 3, 1, 2, 3, 4
274 }
275 };
276 CheckAllEvaluationCombinations(expected);
277 }
278
TEST_P(EvaluatorTest,SingleResidualProblemWithNuisanceParameters)279 TEST_P(EvaluatorTest, SingleResidualProblemWithNuisanceParameters) {
280 // These parameters are not used.
281 double a[2];
282 double b[1];
283 double c[1];
284 double d[3];
285
286 // Add the parameters in a mixed order so the Jacobian is "checkered" with the
287 // values from the other parameters.
288 problem.AddParameterBlock(a, 2);
289 problem.AddParameterBlock(x, 2);
290 problem.AddParameterBlock(b, 1);
291 problem.AddParameterBlock(y, 3);
292 problem.AddParameterBlock(c, 1);
293 problem.AddParameterBlock(z, 4);
294 problem.AddParameterBlock(d, 3);
295
296 problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 3, 2, 3, 4>,
297 NULL,
298 x, y, z);
299
300 ExpectedEvaluation expected = {
301 // Rows/columns
302 3, 16,
303 // Cost
304 7.0,
305 // Residuals
306 { 1.0, 2.0, 3.0 },
307 // Gradient
308 { 0.0, 0.0, // a
309 6.0, 12.0, // x
310 0.0, // b
311 6.0, 12.0, 18.0, // y
312 0.0, // c
313 6.0, 12.0, 18.0, 24.0, // z
314 0.0, 0.0, 0.0, // d
315 },
316 // Jacobian
317 // a x b y c z d
318 { 0, 0, 1, 2, 0, 1, 2, 3, 0, 1, 2, 3, 4, 0, 0, 0,
319 0, 0, 1, 2, 0, 1, 2, 3, 0, 1, 2, 3, 4, 0, 0, 0,
320 0, 0, 1, 2, 0, 1, 2, 3, 0, 1, 2, 3, 4, 0, 0, 0
321 }
322 };
323 CheckAllEvaluationCombinations(expected);
324 }
325
TEST_P(EvaluatorTest,MultipleResidualProblem)326 TEST_P(EvaluatorTest, MultipleResidualProblem) {
327 // Add the parameters in explicit order to force the ordering in the program.
328 problem.AddParameterBlock(x, 2);
329 problem.AddParameterBlock(y, 3);
330 problem.AddParameterBlock(z, 4);
331
332 // f(x, y) in R^2
333 problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 2, 2, 3>,
334 NULL,
335 x, y);
336
337 // g(x, z) in R^3
338 problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,
339 NULL,
340 x, z);
341
342 // h(y, z) in R^4
343 problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,
344 NULL,
345 y, z);
346
347 ExpectedEvaluation expected = {
348 // Rows/columns
349 9, 9,
350 // Cost
351 // f g h
352 ( 1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
353 // Residuals
354 { 1.0, 2.0, // f
355 1.0, 2.0, 3.0, // g
356 1.0, 2.0, 3.0, 4.0 // h
357 },
358 // Gradient
359 { 15.0, 30.0, // x
360 33.0, 66.0, 99.0, // y
361 42.0, 84.0, 126.0, 168.0 // z
362 },
363 // Jacobian
364 // x y z
365 { /* f(x, y) */ 1, 2, 1, 2, 3, 0, 0, 0, 0,
366 1, 2, 1, 2, 3, 0, 0, 0, 0,
367
368 /* g(x, z) */ 2, 4, 0, 0, 0, 2, 4, 6, 8,
369 2, 4, 0, 0, 0, 2, 4, 6, 8,
370 2, 4, 0, 0, 0, 2, 4, 6, 8,
371
372 /* h(y, z) */ 0, 0, 3, 6, 9, 3, 6, 9, 12,
373 0, 0, 3, 6, 9, 3, 6, 9, 12,
374 0, 0, 3, 6, 9, 3, 6, 9, 12,
375 0, 0, 3, 6, 9, 3, 6, 9, 12
376 }
377 };
378 CheckAllEvaluationCombinations(expected);
379 }
380
TEST_P(EvaluatorTest,MultipleResidualsWithLocalParameterizations)381 TEST_P(EvaluatorTest, MultipleResidualsWithLocalParameterizations) {
382 // Add the parameters in explicit order to force the ordering in the program.
383 problem.AddParameterBlock(x, 2);
384
385 // Fix y's first dimension.
386 vector<int> y_fixed;
387 y_fixed.push_back(0);
388 problem.AddParameterBlock(y, 3, new SubsetParameterization(3, y_fixed));
389
390 // Fix z's second dimension.
391 vector<int> z_fixed;
392 z_fixed.push_back(1);
393 problem.AddParameterBlock(z, 4, new SubsetParameterization(4, z_fixed));
394
395 // f(x, y) in R^2
396 problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 2, 2, 3>,
397 NULL,
398 x, y);
399
400 // g(x, z) in R^3
401 problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,
402 NULL,
403 x, z);
404
405 // h(y, z) in R^4
406 problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,
407 NULL,
408 y, z);
409
410 ExpectedEvaluation expected = {
411 // Rows/columns
412 9, 7,
413 // Cost
414 // f g h
415 ( 1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
416 // Residuals
417 { 1.0, 2.0, // f
418 1.0, 2.0, 3.0, // g
419 1.0, 2.0, 3.0, 4.0 // h
420 },
421 // Gradient
422 { 15.0, 30.0, // x
423 66.0, 99.0, // y
424 42.0, 126.0, 168.0 // z
425 },
426 // Jacobian
427 // x y z
428 { /* f(x, y) */ 1, 2, 2, 3, 0, 0, 0,
429 1, 2, 2, 3, 0, 0, 0,
430
431 /* g(x, z) */ 2, 4, 0, 0, 2, 6, 8,
432 2, 4, 0, 0, 2, 6, 8,
433 2, 4, 0, 0, 2, 6, 8,
434
435 /* h(y, z) */ 0, 0, 6, 9, 3, 9, 12,
436 0, 0, 6, 9, 3, 9, 12,
437 0, 0, 6, 9, 3, 9, 12,
438 0, 0, 6, 9, 3, 9, 12
439 }
440 };
441 CheckAllEvaluationCombinations(expected);
442 }
443
TEST_P(EvaluatorTest,MultipleResidualProblemWithSomeConstantParameters)444 TEST_P(EvaluatorTest, MultipleResidualProblemWithSomeConstantParameters) {
445 // The values are ignored completely by the cost function.
446 double x[2];
447 double y[3];
448 double z[4];
449
450 // Add the parameters in explicit order to force the ordering in the program.
451 problem.AddParameterBlock(x, 2);
452 problem.AddParameterBlock(y, 3);
453 problem.AddParameterBlock(z, 4);
454
455 // f(x, y) in R^2
456 problem.AddResidualBlock(new ParameterIgnoringCostFunction<1, 2, 2, 3>,
457 NULL,
458 x, y);
459
460 // g(x, z) in R^3
461 problem.AddResidualBlock(new ParameterIgnoringCostFunction<2, 3, 2, 4>,
462 NULL,
463 x, z);
464
465 // h(y, z) in R^4
466 problem.AddResidualBlock(new ParameterIgnoringCostFunction<3, 4, 3, 4>,
467 NULL,
468 y, z);
469
470 // For this test, "z" is constant.
471 problem.SetParameterBlockConstant(z);
472
473 // Create the reduced program which is missing the fixed "z" variable.
474 // Normally, the preprocessing of the program that happens in solver_impl
475 // takes care of this, but we don't want to invoke the solver here.
476 Program reduced_program;
477 vector<ParameterBlock*>* parameter_blocks =
478 problem.mutable_program()->mutable_parameter_blocks();
479
480 // "z" is the last parameter; save it for later and pop it off temporarily.
481 // Note that "z" will still get read during evaluation, so it cannot be
482 // deleted at this point.
483 ParameterBlock* parameter_block_z = parameter_blocks->back();
484 parameter_blocks->pop_back();
485
486 ExpectedEvaluation expected = {
487 // Rows/columns
488 9, 5,
489 // Cost
490 // f g h
491 ( 1 + 4 + 1 + 4 + 9 + 1 + 4 + 9 + 16) / 2.0,
492 // Residuals
493 { 1.0, 2.0, // f
494 1.0, 2.0, 3.0, // g
495 1.0, 2.0, 3.0, 4.0 // h
496 },
497 // Gradient
498 { 15.0, 30.0, // x
499 33.0, 66.0, 99.0, // y
500 },
501 // Jacobian
502 // x y
503 { /* f(x, y) */ 1, 2, 1, 2, 3,
504 1, 2, 1, 2, 3,
505
506 /* g(x, z) */ 2, 4, 0, 0, 0,
507 2, 4, 0, 0, 0,
508 2, 4, 0, 0, 0,
509
510 /* h(y, z) */ 0, 0, 3, 6, 9,
511 0, 0, 3, 6, 9,
512 0, 0, 3, 6, 9,
513 0, 0, 3, 6, 9
514 }
515 };
516 CheckAllEvaluationCombinations(expected);
517
518 // Restore parameter block z, so it will get freed in a consistent way.
519 parameter_blocks->push_back(parameter_block_z);
520 }
521
TEST_P(EvaluatorTest,EvaluatorAbortsForResidualsThatFailToEvaluate)522 TEST_P(EvaluatorTest, EvaluatorAbortsForResidualsThatFailToEvaluate) {
523 // Switch the return value to failure.
524 problem.AddResidualBlock(
525 new ParameterIgnoringCostFunction<20, 3, 2, 3, 4, false>, NULL, x, y, z);
526
527 // The values are ignored.
528 double state[9];
529
530 scoped_ptr<Evaluator> evaluator(CreateEvaluator(problem.mutable_program()));
531 scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
532 double cost;
533 EXPECT_FALSE(evaluator->Evaluate(state, &cost, NULL, NULL, NULL));
534 }
535
536 // In the pairs, the first argument is the linear solver type, and the second
537 // argument is num_eliminate_blocks. Changing the num_eliminate_blocks only
538 // makes sense for the schur-based solvers.
539 //
540 // Try all values of num_eliminate_blocks that make sense given that in the
541 // tests a maximum of 4 parameter blocks are present.
542 INSTANTIATE_TEST_CASE_P(
543 LinearSolvers,
544 EvaluatorTest,
545 ::testing::Values(
546 EvaluatorTestOptions(DENSE_QR, 0),
547 EvaluatorTestOptions(DENSE_SCHUR, 0),
548 EvaluatorTestOptions(DENSE_SCHUR, 1),
549 EvaluatorTestOptions(DENSE_SCHUR, 2),
550 EvaluatorTestOptions(DENSE_SCHUR, 3),
551 EvaluatorTestOptions(DENSE_SCHUR, 4),
552 EvaluatorTestOptions(SPARSE_SCHUR, 0),
553 EvaluatorTestOptions(SPARSE_SCHUR, 1),
554 EvaluatorTestOptions(SPARSE_SCHUR, 2),
555 EvaluatorTestOptions(SPARSE_SCHUR, 3),
556 EvaluatorTestOptions(SPARSE_SCHUR, 4),
557 EvaluatorTestOptions(ITERATIVE_SCHUR, 0),
558 EvaluatorTestOptions(ITERATIVE_SCHUR, 1),
559 EvaluatorTestOptions(ITERATIVE_SCHUR, 2),
560 EvaluatorTestOptions(ITERATIVE_SCHUR, 3),
561 EvaluatorTestOptions(ITERATIVE_SCHUR, 4),
562 EvaluatorTestOptions(SPARSE_NORMAL_CHOLESKY, 0, false),
563 EvaluatorTestOptions(SPARSE_NORMAL_CHOLESKY, 0, true)));
564
565 // Simple cost function used to check if the evaluator is sensitive to
566 // state changes.
567 class ParameterSensitiveCostFunction : public SizedCostFunction<2, 2> {
568 public:
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const569 virtual bool Evaluate(double const* const* parameters,
570 double* residuals,
571 double** jacobians) const {
572 double x1 = parameters[0][0];
573 double x2 = parameters[0][1];
574 residuals[0] = x1 * x1;
575 residuals[1] = x2 * x2;
576
577 if (jacobians != NULL) {
578 double* jacobian = jacobians[0];
579 if (jacobian != NULL) {
580 jacobian[0] = 2.0 * x1;
581 jacobian[1] = 0.0;
582 jacobian[2] = 0.0;
583 jacobian[3] = 2.0 * x2;
584 }
585 }
586 return true;
587 }
588 };
589
TEST(Evaluator,EvaluatorRespectsParameterChanges)590 TEST(Evaluator, EvaluatorRespectsParameterChanges) {
591 ProblemImpl problem;
592
593 double x[2];
594 x[0] = 1.0;
595 x[1] = 1.0;
596
597 problem.AddResidualBlock(new ParameterSensitiveCostFunction(), NULL, x);
598 Program* program = problem.mutable_program();
599 program->SetParameterOffsetsAndIndex();
600
601 Evaluator::Options options;
602 options.linear_solver_type = DENSE_QR;
603 options.num_eliminate_blocks = 0;
604 string error;
605 scoped_ptr<Evaluator> evaluator(Evaluator::Create(options, program, &error));
606 scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian());
607
608 ASSERT_EQ(2, jacobian->num_rows());
609 ASSERT_EQ(2, jacobian->num_cols());
610
611 double state[2];
612 state[0] = 2.0;
613 state[1] = 3.0;
614
615 // The original state of a residual block comes from the user's
616 // state. So the original state is 1.0, 1.0, and the only way we get
617 // the 2.0, 3.0 results in the following tests is if it respects the
618 // values in the state vector.
619
620 // Cost only; no residuals and no jacobian.
621 {
622 double cost = -1;
623 ASSERT_TRUE(evaluator->Evaluate(state, &cost, NULL, NULL, NULL));
624 EXPECT_EQ(48.5, cost);
625 }
626
627 // Cost and residuals, no jacobian.
628 {
629 double cost = -1;
630 double residuals[2] = { -2, -2 };
631 ASSERT_TRUE(evaluator->Evaluate(state, &cost, residuals, NULL, NULL));
632 EXPECT_EQ(48.5, cost);
633 EXPECT_EQ(4, residuals[0]);
634 EXPECT_EQ(9, residuals[1]);
635 }
636
637 // Cost, residuals, and jacobian.
638 {
639 double cost = -1;
640 double residuals[2] = { -2, -2};
641 SetSparseMatrixConstant(jacobian.get(), -1);
642 ASSERT_TRUE(evaluator->Evaluate(state,
643 &cost,
644 residuals,
645 NULL,
646 jacobian.get()));
647 EXPECT_EQ(48.5, cost);
648 EXPECT_EQ(4, residuals[0]);
649 EXPECT_EQ(9, residuals[1]);
650 Matrix actual_jacobian;
651 jacobian->ToDenseMatrix(&actual_jacobian);
652
653 Matrix expected_jacobian(2, 2);
654 expected_jacobian
655 << 2 * state[0], 0,
656 0, 2 * state[1];
657
658 EXPECT_TRUE((actual_jacobian.array() == expected_jacobian.array()).all())
659 << "Actual:\n" << actual_jacobian
660 << "\nExpected:\n" << expected_jacobian;
661 }
662 }
663
664 } // namespace internal
665 } // namespace ceres
666