1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
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 // keir@google.com (Keir Mierle)
31
32 #include "ceres/problem.h"
33
34 #include <memory>
35
36 #include "ceres/autodiff_cost_function.h"
37 #include "ceres/casts.h"
38 #include "ceres/cost_function.h"
39 #include "ceres/crs_matrix.h"
40 #include "ceres/evaluator_test_utils.h"
41 #include "ceres/internal/eigen.h"
42 #include "ceres/local_parameterization.h"
43 #include "ceres/loss_function.h"
44 #include "ceres/map_util.h"
45 #include "ceres/parameter_block.h"
46 #include "ceres/problem_impl.h"
47 #include "ceres/program.h"
48 #include "ceres/sized_cost_function.h"
49 #include "ceres/sparse_matrix.h"
50 #include "ceres/types.h"
51 #include "gmock/gmock.h"
52 #include "gtest/gtest.h"
53
54 namespace ceres {
55 namespace internal {
56
57 using std::vector;
58
59 // The following three classes are for the purposes of defining
60 // function signatures. They have dummy Evaluate functions.
61
62 // Trivial cost function that accepts a single argument.
63 class UnaryCostFunction : public CostFunction {
64 public:
UnaryCostFunction(int num_residuals,int32_t parameter_block_size)65 UnaryCostFunction(int num_residuals, int32_t parameter_block_size) {
66 set_num_residuals(num_residuals);
67 mutable_parameter_block_sizes()->push_back(parameter_block_size);
68 }
69
~UnaryCostFunction()70 virtual ~UnaryCostFunction() {}
71
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const72 bool Evaluate(double const* const* parameters,
73 double* residuals,
74 double** jacobians) const final {
75 for (int i = 0; i < num_residuals(); ++i) {
76 residuals[i] = 1;
77 }
78 return true;
79 }
80 };
81
82 // Trivial cost function that accepts two arguments.
83 class BinaryCostFunction : public CostFunction {
84 public:
BinaryCostFunction(int num_residuals,int32_t parameter_block1_size,int32_t parameter_block2_size)85 BinaryCostFunction(int num_residuals,
86 int32_t parameter_block1_size,
87 int32_t parameter_block2_size) {
88 set_num_residuals(num_residuals);
89 mutable_parameter_block_sizes()->push_back(parameter_block1_size);
90 mutable_parameter_block_sizes()->push_back(parameter_block2_size);
91 }
92
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const93 bool Evaluate(double const* const* parameters,
94 double* residuals,
95 double** jacobians) const final {
96 for (int i = 0; i < num_residuals(); ++i) {
97 residuals[i] = 2;
98 }
99 return true;
100 }
101 };
102
103 // Trivial cost function that accepts three arguments.
104 class TernaryCostFunction : public CostFunction {
105 public:
TernaryCostFunction(int num_residuals,int32_t parameter_block1_size,int32_t parameter_block2_size,int32_t parameter_block3_size)106 TernaryCostFunction(int num_residuals,
107 int32_t parameter_block1_size,
108 int32_t parameter_block2_size,
109 int32_t parameter_block3_size) {
110 set_num_residuals(num_residuals);
111 mutable_parameter_block_sizes()->push_back(parameter_block1_size);
112 mutable_parameter_block_sizes()->push_back(parameter_block2_size);
113 mutable_parameter_block_sizes()->push_back(parameter_block3_size);
114 }
115
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const116 bool Evaluate(double const* const* parameters,
117 double* residuals,
118 double** jacobians) const final {
119 for (int i = 0; i < num_residuals(); ++i) {
120 residuals[i] = 3;
121 }
122 return true;
123 }
124 };
125
TEST(Problem,MoveConstructor)126 TEST(Problem, MoveConstructor) {
127 Problem src;
128 double x;
129 src.AddParameterBlock(&x, 1);
130 Problem dst(std::move(src));
131 EXPECT_TRUE(dst.HasParameterBlock(&x));
132 }
133
TEST(Problem,MoveAssignment)134 TEST(Problem, MoveAssignment) {
135 Problem src;
136 double x;
137 src.AddParameterBlock(&x, 1);
138 Problem dst;
139 dst = std::move(src);
140 EXPECT_TRUE(dst.HasParameterBlock(&x));
141 }
142
TEST(Problem,AddResidualWithNullCostFunctionDies)143 TEST(Problem, AddResidualWithNullCostFunctionDies) {
144 double x[3], y[4], z[5];
145
146 Problem problem;
147 problem.AddParameterBlock(x, 3);
148 problem.AddParameterBlock(y, 4);
149 problem.AddParameterBlock(z, 5);
150
151 EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock(NULL, NULL, x),
152 "cost_function != nullptr");
153 }
154
TEST(Problem,AddResidualWithIncorrectNumberOfParameterBlocksDies)155 TEST(Problem, AddResidualWithIncorrectNumberOfParameterBlocksDies) {
156 double x[3], y[4], z[5];
157
158 Problem problem;
159 problem.AddParameterBlock(x, 3);
160 problem.AddParameterBlock(y, 4);
161 problem.AddParameterBlock(z, 5);
162
163 // UnaryCostFunction takes only one parameter, but two are passed.
164 EXPECT_DEATH_IF_SUPPORTED(
165 problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x, y),
166 "num_parameter_blocks");
167 }
168
TEST(Problem,AddResidualWithDifferentSizesOnTheSameVariableDies)169 TEST(Problem, AddResidualWithDifferentSizesOnTheSameVariableDies) {
170 double x[3];
171
172 Problem problem;
173 problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
174 EXPECT_DEATH_IF_SUPPORTED(
175 problem.AddResidualBlock(
176 new UnaryCostFunction(2, 4 /* 4 != 3 */), NULL, x),
177 "different block sizes");
178 }
179
TEST(Problem,AddResidualWithDuplicateParametersDies)180 TEST(Problem, AddResidualWithDuplicateParametersDies) {
181 double x[3], z[5];
182
183 Problem problem;
184 EXPECT_DEATH_IF_SUPPORTED(
185 problem.AddResidualBlock(new BinaryCostFunction(2, 3, 3), NULL, x, x),
186 "Duplicate parameter blocks");
187 EXPECT_DEATH_IF_SUPPORTED(
188 problem.AddResidualBlock(
189 new TernaryCostFunction(1, 5, 3, 5), NULL, z, x, z),
190 "Duplicate parameter blocks");
191 }
192
TEST(Problem,AddResidualWithIncorrectSizesOfParameterBlockDies)193 TEST(Problem, AddResidualWithIncorrectSizesOfParameterBlockDies) {
194 double x[3], y[4], z[5];
195
196 Problem problem;
197 problem.AddParameterBlock(x, 3);
198 problem.AddParameterBlock(y, 4);
199 problem.AddParameterBlock(z, 5);
200
201 // The cost function expects the size of the second parameter, z, to be 4
202 // instead of 5 as declared above. This is fatal.
203 EXPECT_DEATH_IF_SUPPORTED(
204 problem.AddResidualBlock(new BinaryCostFunction(2, 3, 4), NULL, x, z),
205 "different block sizes");
206 }
207
TEST(Problem,AddResidualAddsDuplicatedParametersOnlyOnce)208 TEST(Problem, AddResidualAddsDuplicatedParametersOnlyOnce) {
209 double x[3], y[4], z[5];
210
211 Problem problem;
212 problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
213 problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
214 problem.AddResidualBlock(new UnaryCostFunction(2, 4), NULL, y);
215 problem.AddResidualBlock(new UnaryCostFunction(2, 5), NULL, z);
216
217 EXPECT_EQ(3, problem.NumParameterBlocks());
218 EXPECT_EQ(12, problem.NumParameters());
219 }
220
TEST(Problem,AddParameterWithDifferentSizesOnTheSameVariableDies)221 TEST(Problem, AddParameterWithDifferentSizesOnTheSameVariableDies) {
222 double x[3], y[4];
223
224 Problem problem;
225 problem.AddParameterBlock(x, 3);
226 problem.AddParameterBlock(y, 4);
227
228 EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(x, 4),
229 "different block sizes");
230 }
231
IntToPtr(int i)232 static double* IntToPtr(int i) {
233 return reinterpret_cast<double*>(sizeof(double) * i); // NOLINT
234 }
235
TEST(Problem,AddParameterWithAliasedParametersDies)236 TEST(Problem, AddParameterWithAliasedParametersDies) {
237 // Layout is
238 //
239 // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17
240 // [x] x x x x [y] y y
241 // o==o==o o==o==o o==o
242 // o--o--o o--o--o o--o o--o--o
243 //
244 // Parameter block additions are tested as listed above; expected successful
245 // ones marked with o==o and aliasing ones marked with o--o.
246
247 Problem problem;
248 problem.AddParameterBlock(IntToPtr(5), 5); // x
249 problem.AddParameterBlock(IntToPtr(13), 3); // y
250
251 EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr(4), 2),
252 "Aliasing detected");
253 EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr(4), 3),
254 "Aliasing detected");
255 EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr(4), 9),
256 "Aliasing detected");
257 EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr(8), 3),
258 "Aliasing detected");
259 EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr(12), 2),
260 "Aliasing detected");
261 EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr(14), 3),
262 "Aliasing detected");
263
264 // These ones should work.
265 problem.AddParameterBlock(IntToPtr(2), 3);
266 problem.AddParameterBlock(IntToPtr(10), 3);
267 problem.AddParameterBlock(IntToPtr(16), 2);
268
269 ASSERT_EQ(5, problem.NumParameterBlocks());
270 }
271
TEST(Problem,AddParameterIgnoresDuplicateCalls)272 TEST(Problem, AddParameterIgnoresDuplicateCalls) {
273 double x[3], y[4];
274
275 Problem problem;
276 problem.AddParameterBlock(x, 3);
277 problem.AddParameterBlock(y, 4);
278
279 // Creating parameter blocks multiple times is ignored.
280 problem.AddParameterBlock(x, 3);
281 problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
282
283 // ... even repeatedly.
284 problem.AddParameterBlock(x, 3);
285 problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
286
287 // More parameters are fine.
288 problem.AddParameterBlock(y, 4);
289 problem.AddResidualBlock(new UnaryCostFunction(2, 4), NULL, y);
290
291 EXPECT_EQ(2, problem.NumParameterBlocks());
292 EXPECT_EQ(7, problem.NumParameters());
293 }
294
TEST(Problem,AddingParametersAndResidualsResultsInExpectedProblem)295 TEST(Problem, AddingParametersAndResidualsResultsInExpectedProblem) {
296 double x[3], y[4], z[5], w[4];
297
298 Problem problem;
299 problem.AddParameterBlock(x, 3);
300 EXPECT_EQ(1, problem.NumParameterBlocks());
301 EXPECT_EQ(3, problem.NumParameters());
302
303 problem.AddParameterBlock(y, 4);
304 EXPECT_EQ(2, problem.NumParameterBlocks());
305 EXPECT_EQ(7, problem.NumParameters());
306
307 problem.AddParameterBlock(z, 5);
308 EXPECT_EQ(3, problem.NumParameterBlocks());
309 EXPECT_EQ(12, problem.NumParameters());
310
311 // Add a parameter that has a local parameterization.
312 w[0] = 1.0;
313 w[1] = 0.0;
314 w[2] = 0.0;
315 w[3] = 0.0;
316 problem.AddParameterBlock(w, 4, new QuaternionParameterization);
317 EXPECT_EQ(4, problem.NumParameterBlocks());
318 EXPECT_EQ(16, problem.NumParameters());
319
320 problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x);
321 problem.AddResidualBlock(new BinaryCostFunction(6, 5, 4), NULL, z, y);
322 problem.AddResidualBlock(new BinaryCostFunction(3, 3, 5), NULL, x, z);
323 problem.AddResidualBlock(new BinaryCostFunction(7, 5, 3), NULL, z, x);
324 problem.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4), NULL, z, x, y);
325
326 const int total_residuals = 2 + 6 + 3 + 7 + 1;
327 EXPECT_EQ(problem.NumResidualBlocks(), 5);
328 EXPECT_EQ(problem.NumResiduals(), total_residuals);
329 }
330
331 class DestructorCountingCostFunction : public SizedCostFunction<3, 4, 5> {
332 public:
DestructorCountingCostFunction(int * num_destructions)333 explicit DestructorCountingCostFunction(int* num_destructions)
334 : num_destructions_(num_destructions) {}
335
~DestructorCountingCostFunction()336 virtual ~DestructorCountingCostFunction() { *num_destructions_ += 1; }
337
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const338 bool Evaluate(double const* const* parameters,
339 double* residuals,
340 double** jacobians) const final {
341 return true;
342 }
343
344 private:
345 int* num_destructions_;
346 };
347
TEST(Problem,ReusedCostFunctionsAreOnlyDeletedOnce)348 TEST(Problem, ReusedCostFunctionsAreOnlyDeletedOnce) {
349 double y[4], z[5];
350 int num_destructions = 0;
351
352 // Add a cost function multiple times and check to make sure that
353 // the destructor on the cost function is only called once.
354 {
355 Problem problem;
356 problem.AddParameterBlock(y, 4);
357 problem.AddParameterBlock(z, 5);
358
359 CostFunction* cost = new DestructorCountingCostFunction(&num_destructions);
360 problem.AddResidualBlock(cost, NULL, y, z);
361 problem.AddResidualBlock(cost, NULL, y, z);
362 problem.AddResidualBlock(cost, NULL, y, z);
363 EXPECT_EQ(3, problem.NumResidualBlocks());
364 }
365
366 // Check that the destructor was called only once.
367 CHECK_EQ(num_destructions, 1);
368 }
369
TEST(Problem,GetCostFunctionForResidualBlock)370 TEST(Problem, GetCostFunctionForResidualBlock) {
371 double x[3];
372 Problem problem;
373 CostFunction* cost_function = new UnaryCostFunction(2, 3);
374 const ResidualBlockId residual_block =
375 problem.AddResidualBlock(cost_function, NULL, x);
376 EXPECT_EQ(problem.GetCostFunctionForResidualBlock(residual_block),
377 cost_function);
378 EXPECT_TRUE(problem.GetLossFunctionForResidualBlock(residual_block) == NULL);
379 }
380
TEST(Problem,GetLossFunctionForResidualBlock)381 TEST(Problem, GetLossFunctionForResidualBlock) {
382 double x[3];
383 Problem problem;
384 CostFunction* cost_function = new UnaryCostFunction(2, 3);
385 LossFunction* loss_function = new TrivialLoss();
386 const ResidualBlockId residual_block =
387 problem.AddResidualBlock(cost_function, loss_function, x);
388 EXPECT_EQ(problem.GetCostFunctionForResidualBlock(residual_block),
389 cost_function);
390 EXPECT_EQ(problem.GetLossFunctionForResidualBlock(residual_block),
391 loss_function);
392 }
393
TEST(Problem,CostFunctionsAreDeletedEvenWithRemovals)394 TEST(Problem, CostFunctionsAreDeletedEvenWithRemovals) {
395 double y[4], z[5], w[4];
396 int num_destructions = 0;
397 {
398 Problem problem;
399 problem.AddParameterBlock(y, 4);
400 problem.AddParameterBlock(z, 5);
401
402 CostFunction* cost_yz =
403 new DestructorCountingCostFunction(&num_destructions);
404 CostFunction* cost_wz =
405 new DestructorCountingCostFunction(&num_destructions);
406 ResidualBlock* r_yz = problem.AddResidualBlock(cost_yz, NULL, y, z);
407 ResidualBlock* r_wz = problem.AddResidualBlock(cost_wz, NULL, w, z);
408 EXPECT_EQ(2, problem.NumResidualBlocks());
409
410 problem.RemoveResidualBlock(r_yz);
411 CHECK_EQ(num_destructions, 1);
412 problem.RemoveResidualBlock(r_wz);
413 CHECK_EQ(num_destructions, 2);
414
415 EXPECT_EQ(0, problem.NumResidualBlocks());
416 }
417 CHECK_EQ(num_destructions, 2);
418 }
419
420 // Make the dynamic problem tests (e.g. for removing residual blocks)
421 // parameterized on whether the low-latency mode is enabled or not.
422 //
423 // This tests against ProblemImpl instead of Problem in order to inspect the
424 // state of the resulting Program; this is difficult with only the thin Problem
425 // interface.
426 struct DynamicProblem : public ::testing::TestWithParam<bool> {
DynamicProblemceres::internal::DynamicProblem427 DynamicProblem() {
428 Problem::Options options;
429 options.enable_fast_removal = GetParam();
430 problem.reset(new ProblemImpl(options));
431 }
432
GetParameterBlockceres::internal::DynamicProblem433 ParameterBlock* GetParameterBlock(int block) {
434 return problem->program().parameter_blocks()[block];
435 }
GetResidualBlockceres::internal::DynamicProblem436 ResidualBlock* GetResidualBlock(int block) {
437 return problem->program().residual_blocks()[block];
438 }
439
HasResidualBlockceres::internal::DynamicProblem440 bool HasResidualBlock(ResidualBlock* residual_block) {
441 bool have_residual_block = true;
442 if (GetParam()) {
443 have_residual_block &=
444 (problem->residual_block_set().find(residual_block) !=
445 problem->residual_block_set().end());
446 }
447 have_residual_block &=
448 find(problem->program().residual_blocks().begin(),
449 problem->program().residual_blocks().end(),
450 residual_block) != problem->program().residual_blocks().end();
451 return have_residual_block;
452 }
453
NumResidualBlocksceres::internal::DynamicProblem454 int NumResidualBlocks() {
455 // Verify that the hash set of residuals is maintained consistently.
456 if (GetParam()) {
457 EXPECT_EQ(problem->residual_block_set().size(),
458 problem->NumResidualBlocks());
459 }
460 return problem->NumResidualBlocks();
461 }
462
463 // The next block of functions until the end are only for testing the
464 // residual block removals.
ExpectParameterBlockContainsResidualBlockceres::internal::DynamicProblem465 void ExpectParameterBlockContainsResidualBlock(
466 double* values, ResidualBlock* residual_block) {
467 ParameterBlock* parameter_block =
468 FindOrDie(problem->parameter_map(), values);
469 EXPECT_TRUE(ContainsKey(*(parameter_block->mutable_residual_blocks()),
470 residual_block));
471 }
472
ExpectSizeceres::internal::DynamicProblem473 void ExpectSize(double* values, int size) {
474 ParameterBlock* parameter_block =
475 FindOrDie(problem->parameter_map(), values);
476 EXPECT_EQ(size, parameter_block->mutable_residual_blocks()->size());
477 }
478
479 // Degenerate case.
ExpectParameterBlockContainsceres::internal::DynamicProblem480 void ExpectParameterBlockContains(double* values) { ExpectSize(values, 0); }
481
ExpectParameterBlockContainsceres::internal::DynamicProblem482 void ExpectParameterBlockContains(double* values, ResidualBlock* r1) {
483 ExpectSize(values, 1);
484 ExpectParameterBlockContainsResidualBlock(values, r1);
485 }
486
ExpectParameterBlockContainsceres::internal::DynamicProblem487 void ExpectParameterBlockContains(double* values,
488 ResidualBlock* r1,
489 ResidualBlock* r2) {
490 ExpectSize(values, 2);
491 ExpectParameterBlockContainsResidualBlock(values, r1);
492 ExpectParameterBlockContainsResidualBlock(values, r2);
493 }
494
ExpectParameterBlockContainsceres::internal::DynamicProblem495 void ExpectParameterBlockContains(double* values,
496 ResidualBlock* r1,
497 ResidualBlock* r2,
498 ResidualBlock* r3) {
499 ExpectSize(values, 3);
500 ExpectParameterBlockContainsResidualBlock(values, r1);
501 ExpectParameterBlockContainsResidualBlock(values, r2);
502 ExpectParameterBlockContainsResidualBlock(values, r3);
503 }
504
ExpectParameterBlockContainsceres::internal::DynamicProblem505 void ExpectParameterBlockContains(double* values,
506 ResidualBlock* r1,
507 ResidualBlock* r2,
508 ResidualBlock* r3,
509 ResidualBlock* r4) {
510 ExpectSize(values, 4);
511 ExpectParameterBlockContainsResidualBlock(values, r1);
512 ExpectParameterBlockContainsResidualBlock(values, r2);
513 ExpectParameterBlockContainsResidualBlock(values, r3);
514 ExpectParameterBlockContainsResidualBlock(values, r4);
515 }
516
517 std::unique_ptr<ProblemImpl> problem;
518 double y[4], z[5], w[3];
519 };
520
TEST(Problem,SetParameterBlockConstantWithUnknownPtrDies)521 TEST(Problem, SetParameterBlockConstantWithUnknownPtrDies) {
522 double x[3];
523 double y[2];
524
525 Problem problem;
526 problem.AddParameterBlock(x, 3);
527
528 EXPECT_DEATH_IF_SUPPORTED(problem.SetParameterBlockConstant(y),
529 "Parameter block not found:");
530 }
531
TEST(Problem,SetParameterBlockVariableWithUnknownPtrDies)532 TEST(Problem, SetParameterBlockVariableWithUnknownPtrDies) {
533 double x[3];
534 double y[2];
535
536 Problem problem;
537 problem.AddParameterBlock(x, 3);
538
539 EXPECT_DEATH_IF_SUPPORTED(problem.SetParameterBlockVariable(y),
540 "Parameter block not found:");
541 }
542
TEST(Problem,IsParameterBlockConstant)543 TEST(Problem, IsParameterBlockConstant) {
544 double x1[3];
545 double x2[3];
546
547 Problem problem;
548 problem.AddParameterBlock(x1, 3);
549 problem.AddParameterBlock(x2, 3);
550
551 EXPECT_FALSE(problem.IsParameterBlockConstant(x1));
552 EXPECT_FALSE(problem.IsParameterBlockConstant(x2));
553
554 problem.SetParameterBlockConstant(x1);
555 EXPECT_TRUE(problem.IsParameterBlockConstant(x1));
556 EXPECT_FALSE(problem.IsParameterBlockConstant(x2));
557
558 problem.SetParameterBlockConstant(x2);
559 EXPECT_TRUE(problem.IsParameterBlockConstant(x1));
560 EXPECT_TRUE(problem.IsParameterBlockConstant(x2));
561
562 problem.SetParameterBlockVariable(x1);
563 EXPECT_FALSE(problem.IsParameterBlockConstant(x1));
564 EXPECT_TRUE(problem.IsParameterBlockConstant(x2));
565 }
566
TEST(Problem,IsParameterBlockConstantWithUnknownPtrDies)567 TEST(Problem, IsParameterBlockConstantWithUnknownPtrDies) {
568 double x[3];
569 double y[2];
570
571 Problem problem;
572 problem.AddParameterBlock(x, 3);
573
574 EXPECT_DEATH_IF_SUPPORTED(problem.IsParameterBlockConstant(y),
575 "Parameter block not found:");
576 }
577
TEST(Problem,SetLocalParameterizationWithUnknownPtrDies)578 TEST(Problem, SetLocalParameterizationWithUnknownPtrDies) {
579 double x[3];
580 double y[2];
581
582 Problem problem;
583 problem.AddParameterBlock(x, 3);
584
585 EXPECT_DEATH_IF_SUPPORTED(
586 problem.SetParameterization(y, new IdentityParameterization(3)),
587 "Parameter block not found:");
588 }
589
TEST(Problem,RemoveParameterBlockWithUnknownPtrDies)590 TEST(Problem, RemoveParameterBlockWithUnknownPtrDies) {
591 double x[3];
592 double y[2];
593
594 Problem problem;
595 problem.AddParameterBlock(x, 3);
596
597 EXPECT_DEATH_IF_SUPPORTED(problem.RemoveParameterBlock(y),
598 "Parameter block not found:");
599 }
600
TEST(Problem,GetParameterization)601 TEST(Problem, GetParameterization) {
602 double x[3];
603 double y[2];
604
605 Problem problem;
606 problem.AddParameterBlock(x, 3);
607 problem.AddParameterBlock(y, 2);
608
609 LocalParameterization* parameterization = new IdentityParameterization(3);
610 problem.SetParameterization(x, parameterization);
611 EXPECT_EQ(problem.GetParameterization(x), parameterization);
612 EXPECT_TRUE(problem.GetParameterization(y) == NULL);
613 }
614
TEST(Problem,ParameterBlockQueryTest)615 TEST(Problem, ParameterBlockQueryTest) {
616 double x[3];
617 double y[4];
618 Problem problem;
619 problem.AddParameterBlock(x, 3);
620 problem.AddParameterBlock(y, 4);
621
622 vector<int> constant_parameters;
623 constant_parameters.push_back(0);
624 problem.SetParameterization(
625 x, new SubsetParameterization(3, constant_parameters));
626 EXPECT_EQ(problem.ParameterBlockSize(x), 3);
627 EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2);
628 EXPECT_EQ(problem.ParameterBlockLocalSize(y), 4);
629
630 vector<double*> parameter_blocks;
631 problem.GetParameterBlocks(¶meter_blocks);
632 EXPECT_EQ(parameter_blocks.size(), 2);
633 EXPECT_NE(parameter_blocks[0], parameter_blocks[1]);
634 EXPECT_TRUE(parameter_blocks[0] == x || parameter_blocks[0] == y);
635 EXPECT_TRUE(parameter_blocks[1] == x || parameter_blocks[1] == y);
636
637 EXPECT_TRUE(problem.HasParameterBlock(x));
638 problem.RemoveParameterBlock(x);
639 EXPECT_FALSE(problem.HasParameterBlock(x));
640 problem.GetParameterBlocks(¶meter_blocks);
641 EXPECT_EQ(parameter_blocks.size(), 1);
642 EXPECT_TRUE(parameter_blocks[0] == y);
643 }
644
TEST_P(DynamicProblem,RemoveParameterBlockWithNoResiduals)645 TEST_P(DynamicProblem, RemoveParameterBlockWithNoResiduals) {
646 problem->AddParameterBlock(y, 4);
647 problem->AddParameterBlock(z, 5);
648 problem->AddParameterBlock(w, 3);
649 ASSERT_EQ(3, problem->NumParameterBlocks());
650 ASSERT_EQ(0, NumResidualBlocks());
651 EXPECT_EQ(y, GetParameterBlock(0)->user_state());
652 EXPECT_EQ(z, GetParameterBlock(1)->user_state());
653 EXPECT_EQ(w, GetParameterBlock(2)->user_state());
654
655 // w is at the end, which might break the swapping logic so try adding and
656 // removing it.
657 problem->RemoveParameterBlock(w);
658 ASSERT_EQ(2, problem->NumParameterBlocks());
659 ASSERT_EQ(0, NumResidualBlocks());
660 EXPECT_EQ(y, GetParameterBlock(0)->user_state());
661 EXPECT_EQ(z, GetParameterBlock(1)->user_state());
662 problem->AddParameterBlock(w, 3);
663 ASSERT_EQ(3, problem->NumParameterBlocks());
664 ASSERT_EQ(0, NumResidualBlocks());
665 EXPECT_EQ(y, GetParameterBlock(0)->user_state());
666 EXPECT_EQ(z, GetParameterBlock(1)->user_state());
667 EXPECT_EQ(w, GetParameterBlock(2)->user_state());
668
669 // Now remove z, which is in the middle, and add it back.
670 problem->RemoveParameterBlock(z);
671 ASSERT_EQ(2, problem->NumParameterBlocks());
672 ASSERT_EQ(0, NumResidualBlocks());
673 EXPECT_EQ(y, GetParameterBlock(0)->user_state());
674 EXPECT_EQ(w, GetParameterBlock(1)->user_state());
675 problem->AddParameterBlock(z, 5);
676 ASSERT_EQ(3, problem->NumParameterBlocks());
677 ASSERT_EQ(0, NumResidualBlocks());
678 EXPECT_EQ(y, GetParameterBlock(0)->user_state());
679 EXPECT_EQ(w, GetParameterBlock(1)->user_state());
680 EXPECT_EQ(z, GetParameterBlock(2)->user_state());
681
682 // Now remove everything.
683 // y
684 problem->RemoveParameterBlock(y);
685 ASSERT_EQ(2, problem->NumParameterBlocks());
686 ASSERT_EQ(0, NumResidualBlocks());
687 EXPECT_EQ(z, GetParameterBlock(0)->user_state());
688 EXPECT_EQ(w, GetParameterBlock(1)->user_state());
689
690 // z
691 problem->RemoveParameterBlock(z);
692 ASSERT_EQ(1, problem->NumParameterBlocks());
693 ASSERT_EQ(0, NumResidualBlocks());
694 EXPECT_EQ(w, GetParameterBlock(0)->user_state());
695
696 // w
697 problem->RemoveParameterBlock(w);
698 EXPECT_EQ(0, problem->NumParameterBlocks());
699 EXPECT_EQ(0, NumResidualBlocks());
700 }
701
TEST_P(DynamicProblem,RemoveParameterBlockWithResiduals)702 TEST_P(DynamicProblem, RemoveParameterBlockWithResiduals) {
703 problem->AddParameterBlock(y, 4);
704 problem->AddParameterBlock(z, 5);
705 problem->AddParameterBlock(w, 3);
706 ASSERT_EQ(3, problem->NumParameterBlocks());
707 ASSERT_EQ(0, NumResidualBlocks());
708 EXPECT_EQ(y, GetParameterBlock(0)->user_state());
709 EXPECT_EQ(z, GetParameterBlock(1)->user_state());
710 EXPECT_EQ(w, GetParameterBlock(2)->user_state());
711
712 // clang-format off
713
714 // Add all combinations of cost functions.
715 CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3);
716 CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5);
717 CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3);
718 CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3);
719 CostFunction* cost_y = new UnaryCostFunction (1, 4);
720 CostFunction* cost_z = new UnaryCostFunction (1, 5);
721 CostFunction* cost_w = new UnaryCostFunction (1, 3);
722
723 ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w);
724 ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z);
725 ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w);
726 ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w);
727 ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y);
728 ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z);
729 ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w);
730
731 EXPECT_EQ(3, problem->NumParameterBlocks());
732 EXPECT_EQ(7, NumResidualBlocks());
733
734 // Remove w, which should remove r_yzw, r_yw, r_zw, r_w.
735 problem->RemoveParameterBlock(w);
736 ASSERT_EQ(2, problem->NumParameterBlocks());
737 ASSERT_EQ(3, NumResidualBlocks());
738
739 ASSERT_FALSE(HasResidualBlock(r_yzw));
740 ASSERT_TRUE (HasResidualBlock(r_yz ));
741 ASSERT_FALSE(HasResidualBlock(r_yw ));
742 ASSERT_FALSE(HasResidualBlock(r_zw ));
743 ASSERT_TRUE (HasResidualBlock(r_y ));
744 ASSERT_TRUE (HasResidualBlock(r_z ));
745 ASSERT_FALSE(HasResidualBlock(r_w ));
746
747 // Remove z, which will remove almost everything else.
748 problem->RemoveParameterBlock(z);
749 ASSERT_EQ(1, problem->NumParameterBlocks());
750 ASSERT_EQ(1, NumResidualBlocks());
751
752 ASSERT_FALSE(HasResidualBlock(r_yzw));
753 ASSERT_FALSE(HasResidualBlock(r_yz ));
754 ASSERT_FALSE(HasResidualBlock(r_yw ));
755 ASSERT_FALSE(HasResidualBlock(r_zw ));
756 ASSERT_TRUE (HasResidualBlock(r_y ));
757 ASSERT_FALSE(HasResidualBlock(r_z ));
758 ASSERT_FALSE(HasResidualBlock(r_w ));
759
760 // Remove y; all gone.
761 problem->RemoveParameterBlock(y);
762 EXPECT_EQ(0, problem->NumParameterBlocks());
763 EXPECT_EQ(0, NumResidualBlocks());
764
765 // clang-format on
766 }
767
TEST_P(DynamicProblem,RemoveResidualBlock)768 TEST_P(DynamicProblem, RemoveResidualBlock) {
769 problem->AddParameterBlock(y, 4);
770 problem->AddParameterBlock(z, 5);
771 problem->AddParameterBlock(w, 3);
772
773 // clang-format off
774
775 // Add all combinations of cost functions.
776 CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3);
777 CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5);
778 CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3);
779 CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3);
780 CostFunction* cost_y = new UnaryCostFunction (1, 4);
781 CostFunction* cost_z = new UnaryCostFunction (1, 5);
782 CostFunction* cost_w = new UnaryCostFunction (1, 3);
783
784 ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w);
785 ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z);
786 ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w);
787 ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w);
788 ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y);
789 ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z);
790 ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w);
791
792 if (GetParam()) {
793 // In this test parameterization, there should be back-pointers from the
794 // parameter blocks to the residual blocks.
795 ExpectParameterBlockContains(y, r_yzw, r_yz, r_yw, r_y);
796 ExpectParameterBlockContains(z, r_yzw, r_yz, r_zw, r_z);
797 ExpectParameterBlockContains(w, r_yzw, r_yw, r_zw, r_w);
798 } else {
799 // Otherwise, nothing.
800 EXPECT_TRUE(GetParameterBlock(0)->mutable_residual_blocks() == NULL);
801 EXPECT_TRUE(GetParameterBlock(1)->mutable_residual_blocks() == NULL);
802 EXPECT_TRUE(GetParameterBlock(2)->mutable_residual_blocks() == NULL);
803 }
804 EXPECT_EQ(3, problem->NumParameterBlocks());
805 EXPECT_EQ(7, NumResidualBlocks());
806
807 // Remove each residual and check the state after each removal.
808
809 // Remove r_yzw.
810 problem->RemoveResidualBlock(r_yzw);
811 ASSERT_EQ(3, problem->NumParameterBlocks());
812 ASSERT_EQ(6, NumResidualBlocks());
813 if (GetParam()) {
814 ExpectParameterBlockContains(y, r_yz, r_yw, r_y);
815 ExpectParameterBlockContains(z, r_yz, r_zw, r_z);
816 ExpectParameterBlockContains(w, r_yw, r_zw, r_w);
817 }
818 ASSERT_TRUE (HasResidualBlock(r_yz ));
819 ASSERT_TRUE (HasResidualBlock(r_yw ));
820 ASSERT_TRUE (HasResidualBlock(r_zw ));
821 ASSERT_TRUE (HasResidualBlock(r_y ));
822 ASSERT_TRUE (HasResidualBlock(r_z ));
823 ASSERT_TRUE (HasResidualBlock(r_w ));
824
825 // Remove r_yw.
826 problem->RemoveResidualBlock(r_yw);
827 ASSERT_EQ(3, problem->NumParameterBlocks());
828 ASSERT_EQ(5, NumResidualBlocks());
829 if (GetParam()) {
830 ExpectParameterBlockContains(y, r_yz, r_y);
831 ExpectParameterBlockContains(z, r_yz, r_zw, r_z);
832 ExpectParameterBlockContains(w, r_zw, r_w);
833 }
834 ASSERT_TRUE (HasResidualBlock(r_yz ));
835 ASSERT_TRUE (HasResidualBlock(r_zw ));
836 ASSERT_TRUE (HasResidualBlock(r_y ));
837 ASSERT_TRUE (HasResidualBlock(r_z ));
838 ASSERT_TRUE (HasResidualBlock(r_w ));
839
840 // Remove r_zw.
841 problem->RemoveResidualBlock(r_zw);
842 ASSERT_EQ(3, problem->NumParameterBlocks());
843 ASSERT_EQ(4, NumResidualBlocks());
844 if (GetParam()) {
845 ExpectParameterBlockContains(y, r_yz, r_y);
846 ExpectParameterBlockContains(z, r_yz, r_z);
847 ExpectParameterBlockContains(w, r_w);
848 }
849 ASSERT_TRUE (HasResidualBlock(r_yz ));
850 ASSERT_TRUE (HasResidualBlock(r_y ));
851 ASSERT_TRUE (HasResidualBlock(r_z ));
852 ASSERT_TRUE (HasResidualBlock(r_w ));
853
854 // Remove r_w.
855 problem->RemoveResidualBlock(r_w);
856 ASSERT_EQ(3, problem->NumParameterBlocks());
857 ASSERT_EQ(3, NumResidualBlocks());
858 if (GetParam()) {
859 ExpectParameterBlockContains(y, r_yz, r_y);
860 ExpectParameterBlockContains(z, r_yz, r_z);
861 ExpectParameterBlockContains(w);
862 }
863 ASSERT_TRUE (HasResidualBlock(r_yz ));
864 ASSERT_TRUE (HasResidualBlock(r_y ));
865 ASSERT_TRUE (HasResidualBlock(r_z ));
866
867 // Remove r_yz.
868 problem->RemoveResidualBlock(r_yz);
869 ASSERT_EQ(3, problem->NumParameterBlocks());
870 ASSERT_EQ(2, NumResidualBlocks());
871 if (GetParam()) {
872 ExpectParameterBlockContains(y, r_y);
873 ExpectParameterBlockContains(z, r_z);
874 ExpectParameterBlockContains(w);
875 }
876 ASSERT_TRUE (HasResidualBlock(r_y ));
877 ASSERT_TRUE (HasResidualBlock(r_z ));
878
879 // Remove the last two.
880 problem->RemoveResidualBlock(r_z);
881 problem->RemoveResidualBlock(r_y);
882 ASSERT_EQ(3, problem->NumParameterBlocks());
883 ASSERT_EQ(0, NumResidualBlocks());
884 if (GetParam()) {
885 ExpectParameterBlockContains(y);
886 ExpectParameterBlockContains(z);
887 ExpectParameterBlockContains(w);
888 }
889
890 // clang-format on
891 }
892
TEST_P(DynamicProblem,RemoveInvalidResidualBlockDies)893 TEST_P(DynamicProblem, RemoveInvalidResidualBlockDies) {
894 problem->AddParameterBlock(y, 4);
895 problem->AddParameterBlock(z, 5);
896 problem->AddParameterBlock(w, 3);
897
898 // clang-format off
899
900 // Add all combinations of cost functions.
901 CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3);
902 CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5);
903 CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3);
904 CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3);
905 CostFunction* cost_y = new UnaryCostFunction (1, 4);
906 CostFunction* cost_z = new UnaryCostFunction (1, 5);
907 CostFunction* cost_w = new UnaryCostFunction (1, 3);
908
909 ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w);
910 ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z);
911 ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w);
912 ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w);
913 ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y);
914 ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z);
915 ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w);
916
917 // clang-format on
918
919 // Remove r_yzw.
920 problem->RemoveResidualBlock(r_yzw);
921 ASSERT_EQ(3, problem->NumParameterBlocks());
922 ASSERT_EQ(6, NumResidualBlocks());
923 // Attempt to remove r_yzw again.
924 EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_yzw), "not found");
925
926 // Attempt to remove a cast pointer never added as a residual.
927 int trash_memory = 1234;
928 ResidualBlock* invalid_residual =
929 reinterpret_cast<ResidualBlock*>(&trash_memory);
930 EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(invalid_residual),
931 "not found");
932
933 // Remove a parameter block, which in turn removes the dependent residuals
934 // then attempt to remove them directly.
935 problem->RemoveParameterBlock(z);
936 ASSERT_EQ(2, problem->NumParameterBlocks());
937 ASSERT_EQ(3, NumResidualBlocks());
938 EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_yz), "not found");
939 EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_zw), "not found");
940 EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_z), "not found");
941
942 problem->RemoveResidualBlock(r_yw);
943 problem->RemoveResidualBlock(r_w);
944 problem->RemoveResidualBlock(r_y);
945 }
946
947 // Check that a null-terminated array, a, has the same elements as b.
948 template <typename T>
ExpectVectorContainsUnordered(const T * a,const vector<T> & b)949 void ExpectVectorContainsUnordered(const T* a, const vector<T>& b) {
950 // Compute the size of a.
951 int size = 0;
952 while (a[size]) {
953 ++size;
954 }
955 ASSERT_EQ(size, b.size());
956
957 // Sort a.
958 vector<T> a_sorted(size);
959 copy(a, a + size, a_sorted.begin());
960 sort(a_sorted.begin(), a_sorted.end());
961
962 // Sort b.
963 vector<T> b_sorted(b);
964 sort(b_sorted.begin(), b_sorted.end());
965
966 // Compare.
967 for (int i = 0; i < size; ++i) {
968 EXPECT_EQ(a_sorted[i], b_sorted[i]);
969 }
970 }
971
ExpectProblemHasResidualBlocks(const ProblemImpl & problem,const ResidualBlockId * expected_residual_blocks)972 static void ExpectProblemHasResidualBlocks(
973 const ProblemImpl& problem,
974 const ResidualBlockId* expected_residual_blocks) {
975 vector<ResidualBlockId> residual_blocks;
976 problem.GetResidualBlocks(&residual_blocks);
977 ExpectVectorContainsUnordered(expected_residual_blocks, residual_blocks);
978 }
979
TEST_P(DynamicProblem,GetXXXBlocksForYYYBlock)980 TEST_P(DynamicProblem, GetXXXBlocksForYYYBlock) {
981 problem->AddParameterBlock(y, 4);
982 problem->AddParameterBlock(z, 5);
983 problem->AddParameterBlock(w, 3);
984
985 // clang-format off
986
987 // Add all combinations of cost functions.
988 CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3);
989 CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5);
990 CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3);
991 CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3);
992 CostFunction* cost_y = new UnaryCostFunction (1, 4);
993 CostFunction* cost_z = new UnaryCostFunction (1, 5);
994 CostFunction* cost_w = new UnaryCostFunction (1, 3);
995
996 ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w);
997 {
998 ResidualBlockId expected_residuals[] = {r_yzw, 0};
999 ExpectProblemHasResidualBlocks(*problem, expected_residuals);
1000 }
1001 ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z);
1002 {
1003 ResidualBlockId expected_residuals[] = {r_yzw, r_yz, 0};
1004 ExpectProblemHasResidualBlocks(*problem, expected_residuals);
1005 }
1006 ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w);
1007 {
1008 ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, 0};
1009 ExpectProblemHasResidualBlocks(*problem, expected_residuals);
1010 }
1011 ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w);
1012 {
1013 ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, r_zw, 0};
1014 ExpectProblemHasResidualBlocks(*problem, expected_residuals);
1015 }
1016 ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y);
1017 {
1018 ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, r_zw, r_y, 0};
1019 ExpectProblemHasResidualBlocks(*problem, expected_residuals);
1020 }
1021 ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z);
1022 {
1023 ResidualBlock *expected_residuals[] = {
1024 r_yzw, r_yz, r_yw, r_zw, r_y, r_z, 0
1025 };
1026 ExpectProblemHasResidualBlocks(*problem, expected_residuals);
1027 }
1028 ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w);
1029 {
1030 ResidualBlock *expected_residuals[] = {
1031 r_yzw, r_yz, r_yw, r_zw, r_y, r_z, r_w, 0
1032 };
1033 ExpectProblemHasResidualBlocks(*problem, expected_residuals);
1034 }
1035
1036 vector<double*> parameter_blocks;
1037 vector<ResidualBlockId> residual_blocks;
1038
1039 // Check GetResidualBlocksForParameterBlock() for all parameter blocks.
1040 struct GetResidualBlocksForParameterBlockTestCase {
1041 double* parameter_block;
1042 ResidualBlockId expected_residual_blocks[10];
1043 };
1044 GetResidualBlocksForParameterBlockTestCase get_residual_blocks_cases[] = {
1045 { y, { r_yzw, r_yz, r_yw, r_y, NULL} },
1046 { z, { r_yzw, r_yz, r_zw, r_z, NULL} },
1047 { w, { r_yzw, r_yw, r_zw, r_w, NULL} },
1048 { NULL }
1049 };
1050 for (int i = 0; get_residual_blocks_cases[i].parameter_block; ++i) {
1051 problem->GetResidualBlocksForParameterBlock(
1052 get_residual_blocks_cases[i].parameter_block,
1053 &residual_blocks);
1054 ExpectVectorContainsUnordered(
1055 get_residual_blocks_cases[i].expected_residual_blocks,
1056 residual_blocks);
1057 }
1058
1059 // Check GetParameterBlocksForResidualBlock() for all residual blocks.
1060 struct GetParameterBlocksForResidualBlockTestCase {
1061 ResidualBlockId residual_block;
1062 double* expected_parameter_blocks[10];
1063 };
1064 GetParameterBlocksForResidualBlockTestCase get_parameter_blocks_cases[] = {
1065 { r_yzw, { y, z, w, NULL } },
1066 { r_yz , { y, z, NULL } },
1067 { r_yw , { y, w, NULL } },
1068 { r_zw , { z, w, NULL } },
1069 { r_y , { y, NULL } },
1070 { r_z , { z, NULL } },
1071 { r_w , { w, NULL } },
1072 { NULL }
1073 };
1074 for (int i = 0; get_parameter_blocks_cases[i].residual_block; ++i) {
1075 problem->GetParameterBlocksForResidualBlock(
1076 get_parameter_blocks_cases[i].residual_block,
1077 ¶meter_blocks);
1078 ExpectVectorContainsUnordered(
1079 get_parameter_blocks_cases[i].expected_parameter_blocks,
1080 parameter_blocks);
1081 }
1082
1083 // clang-format on
1084 }
1085
1086 INSTANTIATE_TEST_SUITE_P(OptionsInstantiation,
1087 DynamicProblem,
1088 ::testing::Values(true, false));
1089
1090 // Test for Problem::Evaluate
1091
1092 // r_i = i - (j + 1) * x_ij^2
1093 template <int kNumResiduals, int kNumParameterBlocks>
1094 class QuadraticCostFunction : public CostFunction {
1095 public:
QuadraticCostFunction()1096 QuadraticCostFunction() {
1097 CHECK_GT(kNumResiduals, 0);
1098 CHECK_GT(kNumParameterBlocks, 0);
1099 set_num_residuals(kNumResiduals);
1100 for (int i = 0; i < kNumParameterBlocks; ++i) {
1101 mutable_parameter_block_sizes()->push_back(kNumResiduals);
1102 }
1103 }
1104
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const1105 bool Evaluate(double const* const* parameters,
1106 double* residuals,
1107 double** jacobians) const final {
1108 for (int i = 0; i < kNumResiduals; ++i) {
1109 residuals[i] = i;
1110 for (int j = 0; j < kNumParameterBlocks; ++j) {
1111 residuals[i] -= (j + 1.0) * parameters[j][i] * parameters[j][i];
1112 }
1113 }
1114
1115 if (jacobians == NULL) {
1116 return true;
1117 }
1118
1119 for (int j = 0; j < kNumParameterBlocks; ++j) {
1120 if (jacobians[j] != NULL) {
1121 MatrixRef(jacobians[j], kNumResiduals, kNumResiduals) =
1122 (-2.0 * (j + 1.0) * ConstVectorRef(parameters[j], kNumResiduals))
1123 .asDiagonal();
1124 }
1125 }
1126
1127 return true;
1128 }
1129 };
1130
1131 // Convert a CRSMatrix to a dense Eigen matrix.
CRSToDenseMatrix(const CRSMatrix & input,Matrix * output)1132 static void CRSToDenseMatrix(const CRSMatrix& input, Matrix* output) {
1133 CHECK(output != nullptr);
1134 Matrix& m = *output;
1135 m.resize(input.num_rows, input.num_cols);
1136 m.setZero();
1137 for (int row = 0; row < input.num_rows; ++row) {
1138 for (int j = input.rows[row]; j < input.rows[row + 1]; ++j) {
1139 const int col = input.cols[j];
1140 m(row, col) = input.values[j];
1141 }
1142 }
1143 }
1144
1145 class ProblemEvaluateTest : public ::testing::Test {
1146 protected:
SetUp()1147 void SetUp() {
1148 for (int i = 0; i < 6; ++i) {
1149 parameters_[i] = static_cast<double>(i + 1);
1150 }
1151
1152 parameter_blocks_.push_back(parameters_);
1153 parameter_blocks_.push_back(parameters_ + 2);
1154 parameter_blocks_.push_back(parameters_ + 4);
1155
1156 CostFunction* cost_function = new QuadraticCostFunction<2, 2>;
1157
1158 // f(x, y)
1159 residual_blocks_.push_back(problem_.AddResidualBlock(
1160 cost_function, NULL, parameters_, parameters_ + 2));
1161 // g(y, z)
1162 residual_blocks_.push_back(problem_.AddResidualBlock(
1163 cost_function, NULL, parameters_ + 2, parameters_ + 4));
1164 // h(z, x)
1165 residual_blocks_.push_back(problem_.AddResidualBlock(
1166 cost_function, NULL, parameters_ + 4, parameters_));
1167 }
1168
TearDown()1169 void TearDown() { EXPECT_TRUE(problem_.program().IsValid()); }
1170
EvaluateAndCompare(const Problem::EvaluateOptions & options,const int expected_num_rows,const int expected_num_cols,const double expected_cost,const double * expected_residuals,const double * expected_gradient,const double * expected_jacobian)1171 void EvaluateAndCompare(const Problem::EvaluateOptions& options,
1172 const int expected_num_rows,
1173 const int expected_num_cols,
1174 const double expected_cost,
1175 const double* expected_residuals,
1176 const double* expected_gradient,
1177 const double* expected_jacobian) {
1178 double cost;
1179 vector<double> residuals;
1180 vector<double> gradient;
1181 CRSMatrix jacobian;
1182
1183 EXPECT_TRUE(
1184 problem_.Evaluate(options,
1185 &cost,
1186 expected_residuals != NULL ? &residuals : NULL,
1187 expected_gradient != NULL ? &gradient : NULL,
1188 expected_jacobian != NULL ? &jacobian : NULL));
1189
1190 if (expected_residuals != NULL) {
1191 EXPECT_EQ(residuals.size(), expected_num_rows);
1192 }
1193
1194 if (expected_gradient != NULL) {
1195 EXPECT_EQ(gradient.size(), expected_num_cols);
1196 }
1197
1198 if (expected_jacobian != NULL) {
1199 EXPECT_EQ(jacobian.num_rows, expected_num_rows);
1200 EXPECT_EQ(jacobian.num_cols, expected_num_cols);
1201 }
1202
1203 Matrix dense_jacobian;
1204 if (expected_jacobian != NULL) {
1205 CRSToDenseMatrix(jacobian, &dense_jacobian);
1206 }
1207
1208 CompareEvaluations(expected_num_rows,
1209 expected_num_cols,
1210 expected_cost,
1211 expected_residuals,
1212 expected_gradient,
1213 expected_jacobian,
1214 cost,
1215 residuals.size() > 0 ? &residuals[0] : NULL,
1216 gradient.size() > 0 ? &gradient[0] : NULL,
1217 dense_jacobian.data());
1218 }
1219
CheckAllEvaluationCombinations(const Problem::EvaluateOptions & options,const ExpectedEvaluation & expected)1220 void CheckAllEvaluationCombinations(const Problem::EvaluateOptions& options,
1221 const ExpectedEvaluation& expected) {
1222 for (int i = 0; i < 8; ++i) {
1223 EvaluateAndCompare(options,
1224 expected.num_rows,
1225 expected.num_cols,
1226 expected.cost,
1227 (i & 1) ? expected.residuals : NULL,
1228 (i & 2) ? expected.gradient : NULL,
1229 (i & 4) ? expected.jacobian : NULL);
1230 }
1231 }
1232
1233 ProblemImpl problem_;
1234 double parameters_[6];
1235 vector<double*> parameter_blocks_;
1236 vector<ResidualBlockId> residual_blocks_;
1237 };
1238
TEST_F(ProblemEvaluateTest,MultipleParameterAndResidualBlocks)1239 TEST_F(ProblemEvaluateTest, MultipleParameterAndResidualBlocks) {
1240 // clang-format off
1241 ExpectedEvaluation expected = {
1242 // Rows/columns
1243 6, 6,
1244 // Cost
1245 7607.0,
1246 // Residuals
1247 { -19.0, -35.0, // f
1248 -59.0, -87.0, // g
1249 -27.0, -43.0 // h
1250 },
1251 // Gradient
1252 { 146.0, 484.0, // x
1253 582.0, 1256.0, // y
1254 1450.0, 2604.0, // z
1255 },
1256 // Jacobian
1257 // x y z
1258 { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0,
1259 0.0, -4.0, 0.0, -16.0, 0.0, 0.0,
1260 /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0,
1261 0.0, 0.0, 0.0, -8.0, 0.0, -24.0,
1262 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0,
1263 0.0, -8.0, 0.0, 0.0, 0.0, -12.0
1264 }
1265 };
1266 // clang-format on
1267
1268 CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected);
1269 }
1270
TEST_F(ProblemEvaluateTest,ParameterAndResidualBlocksPassedInOptions)1271 TEST_F(ProblemEvaluateTest, ParameterAndResidualBlocksPassedInOptions) {
1272 // clang-format off
1273 ExpectedEvaluation expected = {
1274 // Rows/columns
1275 6, 6,
1276 // Cost
1277 7607.0,
1278 // Residuals
1279 { -19.0, -35.0, // f
1280 -59.0, -87.0, // g
1281 -27.0, -43.0 // h
1282 },
1283 // Gradient
1284 { 146.0, 484.0, // x
1285 582.0, 1256.0, // y
1286 1450.0, 2604.0, // z
1287 },
1288 // Jacobian
1289 // x y z
1290 { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0,
1291 0.0, -4.0, 0.0, -16.0, 0.0, 0.0,
1292 /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0,
1293 0.0, 0.0, 0.0, -8.0, 0.0, -24.0,
1294 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0,
1295 0.0, -8.0, 0.0, 0.0, 0.0, -12.0
1296 }
1297 };
1298 // clang-format on
1299
1300 Problem::EvaluateOptions evaluate_options;
1301 evaluate_options.parameter_blocks = parameter_blocks_;
1302 evaluate_options.residual_blocks = residual_blocks_;
1303 CheckAllEvaluationCombinations(evaluate_options, expected);
1304 }
1305
TEST_F(ProblemEvaluateTest,ReorderedResidualBlocks)1306 TEST_F(ProblemEvaluateTest, ReorderedResidualBlocks) {
1307 // clang-format off
1308 ExpectedEvaluation expected = {
1309 // Rows/columns
1310 6, 6,
1311 // Cost
1312 7607.0,
1313 // Residuals
1314 { -19.0, -35.0, // f
1315 -27.0, -43.0, // h
1316 -59.0, -87.0 // g
1317 },
1318 // Gradient
1319 { 146.0, 484.0, // x
1320 582.0, 1256.0, // y
1321 1450.0, 2604.0, // z
1322 },
1323 // Jacobian
1324 // x y z
1325 { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0,
1326 0.0, -4.0, 0.0, -16.0, 0.0, 0.0,
1327 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0,
1328 0.0, -8.0, 0.0, 0.0, 0.0, -12.0,
1329 /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0,
1330 0.0, 0.0, 0.0, -8.0, 0.0, -24.0
1331 }
1332 };
1333 // clang-format on
1334
1335 Problem::EvaluateOptions evaluate_options;
1336 evaluate_options.parameter_blocks = parameter_blocks_;
1337
1338 // f, h, g
1339 evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
1340 evaluate_options.residual_blocks.push_back(residual_blocks_[2]);
1341 evaluate_options.residual_blocks.push_back(residual_blocks_[1]);
1342
1343 CheckAllEvaluationCombinations(evaluate_options, expected);
1344 }
1345
TEST_F(ProblemEvaluateTest,ReorderedResidualBlocksAndReorderedParameterBlocks)1346 TEST_F(ProblemEvaluateTest,
1347 ReorderedResidualBlocksAndReorderedParameterBlocks) {
1348 // clang-format off
1349 ExpectedEvaluation expected = {
1350 // Rows/columns
1351 6, 6,
1352 // Cost
1353 7607.0,
1354 // Residuals
1355 { -19.0, -35.0, // f
1356 -27.0, -43.0, // h
1357 -59.0, -87.0 // g
1358 },
1359 // Gradient
1360 { 1450.0, 2604.0, // z
1361 582.0, 1256.0, // y
1362 146.0, 484.0, // x
1363 },
1364 // Jacobian
1365 // z y x
1366 { /* f(x, y) */ 0.0, 0.0, -12.0, 0.0, -2.0, 0.0,
1367 0.0, 0.0, 0.0, -16.0, 0.0, -4.0,
1368 /* h(z, x) */ -10.0, 0.0, 0.0, 0.0, -4.0, 0.0,
1369 0.0, -12.0, 0.0, 0.0, 0.0, -8.0,
1370 /* g(y, z) */ -20.0, 0.0, -6.0, 0.0, 0.0, 0.0,
1371 0.0, -24.0, 0.0, -8.0, 0.0, 0.0
1372 }
1373 };
1374 // clang-format on
1375
1376 Problem::EvaluateOptions evaluate_options;
1377 // z, y, x
1378 evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]);
1379 evaluate_options.parameter_blocks.push_back(parameter_blocks_[1]);
1380 evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]);
1381
1382 // f, h, g
1383 evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
1384 evaluate_options.residual_blocks.push_back(residual_blocks_[2]);
1385 evaluate_options.residual_blocks.push_back(residual_blocks_[1]);
1386
1387 CheckAllEvaluationCombinations(evaluate_options, expected);
1388 }
1389
TEST_F(ProblemEvaluateTest,ConstantParameterBlock)1390 TEST_F(ProblemEvaluateTest, ConstantParameterBlock) {
1391 // clang-format off
1392 ExpectedEvaluation expected = {
1393 // Rows/columns
1394 6, 6,
1395 // Cost
1396 7607.0,
1397 // Residuals
1398 { -19.0, -35.0, // f
1399 -59.0, -87.0, // g
1400 -27.0, -43.0 // h
1401 },
1402
1403 // Gradient
1404 { 146.0, 484.0, // x
1405 0.0, 0.0, // y
1406 1450.0, 2604.0, // z
1407 },
1408
1409 // Jacobian
1410 // x y z
1411 { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1412 0.0, -4.0, 0.0, 0.0, 0.0, 0.0,
1413 /* g(y, z) */ 0.0, 0.0, 0.0, 0.0, -20.0, 0.0,
1414 0.0, 0.0, 0.0, 0.0, 0.0, -24.0,
1415 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0,
1416 0.0, -8.0, 0.0, 0.0, 0.0, -12.0
1417 }
1418 };
1419 // clang-format on
1420
1421 problem_.SetParameterBlockConstant(parameters_ + 2);
1422 CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected);
1423 }
1424
TEST_F(ProblemEvaluateTest,ExcludedAResidualBlock)1425 TEST_F(ProblemEvaluateTest, ExcludedAResidualBlock) {
1426 // clang-format off
1427 ExpectedEvaluation expected = {
1428 // Rows/columns
1429 4, 6,
1430 // Cost
1431 2082.0,
1432 // Residuals
1433 { -19.0, -35.0, // f
1434 -27.0, -43.0 // h
1435 },
1436 // Gradient
1437 { 146.0, 484.0, // x
1438 228.0, 560.0, // y
1439 270.0, 516.0, // z
1440 },
1441 // Jacobian
1442 // x y z
1443 { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0,
1444 0.0, -4.0, 0.0, -16.0, 0.0, 0.0,
1445 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0,
1446 0.0, -8.0, 0.0, 0.0, 0.0, -12.0
1447 }
1448 };
1449 // clang-format on
1450
1451 Problem::EvaluateOptions evaluate_options;
1452 evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
1453 evaluate_options.residual_blocks.push_back(residual_blocks_[2]);
1454
1455 CheckAllEvaluationCombinations(evaluate_options, expected);
1456 }
1457
TEST_F(ProblemEvaluateTest,ExcludedParameterBlock)1458 TEST_F(ProblemEvaluateTest, ExcludedParameterBlock) {
1459 // clang-format off
1460 ExpectedEvaluation expected = {
1461 // Rows/columns
1462 6, 4,
1463 // Cost
1464 7607.0,
1465 // Residuals
1466 { -19.0, -35.0, // f
1467 -59.0, -87.0, // g
1468 -27.0, -43.0 // h
1469 },
1470
1471 // Gradient
1472 { 146.0, 484.0, // x
1473 1450.0, 2604.0, // z
1474 },
1475
1476 // Jacobian
1477 // x z
1478 { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0,
1479 0.0, -4.0, 0.0, 0.0,
1480 /* g(y, z) */ 0.0, 0.0, -20.0, 0.0,
1481 0.0, 0.0, 0.0, -24.0,
1482 /* h(z, x) */ -4.0, 0.0, -10.0, 0.0,
1483 0.0, -8.0, 0.0, -12.0
1484 }
1485 };
1486 // clang-format on
1487
1488 Problem::EvaluateOptions evaluate_options;
1489 // x, z
1490 evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]);
1491 evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]);
1492 evaluate_options.residual_blocks = residual_blocks_;
1493 CheckAllEvaluationCombinations(evaluate_options, expected);
1494 }
1495
TEST_F(ProblemEvaluateTest,ExcludedParameterBlockAndExcludedResidualBlock)1496 TEST_F(ProblemEvaluateTest, ExcludedParameterBlockAndExcludedResidualBlock) {
1497 // clang-format off
1498 ExpectedEvaluation expected = {
1499 // Rows/columns
1500 4, 4,
1501 // Cost
1502 6318.0,
1503 // Residuals
1504 { -19.0, -35.0, // f
1505 -59.0, -87.0, // g
1506 },
1507
1508 // Gradient
1509 { 38.0, 140.0, // x
1510 1180.0, 2088.0, // z
1511 },
1512
1513 // Jacobian
1514 // x z
1515 { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0,
1516 0.0, -4.0, 0.0, 0.0,
1517 /* g(y, z) */ 0.0, 0.0, -20.0, 0.0,
1518 0.0, 0.0, 0.0, -24.0,
1519 }
1520 };
1521 // clang-format on
1522
1523 Problem::EvaluateOptions evaluate_options;
1524 // x, z
1525 evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]);
1526 evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]);
1527 evaluate_options.residual_blocks.push_back(residual_blocks_[0]);
1528 evaluate_options.residual_blocks.push_back(residual_blocks_[1]);
1529
1530 CheckAllEvaluationCombinations(evaluate_options, expected);
1531 }
1532
TEST_F(ProblemEvaluateTest,LocalParameterization)1533 TEST_F(ProblemEvaluateTest, LocalParameterization) {
1534 // clang-format off
1535 ExpectedEvaluation expected = {
1536 // Rows/columns
1537 6, 5,
1538 // Cost
1539 7607.0,
1540 // Residuals
1541 { -19.0, -35.0, // f
1542 -59.0, -87.0, // g
1543 -27.0, -43.0 // h
1544 },
1545 // Gradient
1546 { 146.0, 484.0, // x
1547 1256.0, // y with SubsetParameterization
1548 1450.0, 2604.0, // z
1549 },
1550 // Jacobian
1551 // x y z
1552 { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 0.0,
1553 0.0, -4.0, -16.0, 0.0, 0.0,
1554 /* g(y, z) */ 0.0, 0.0, 0.0, -20.0, 0.0,
1555 0.0, 0.0, -8.0, 0.0, -24.0,
1556 /* h(z, x) */ -4.0, 0.0, 0.0, -10.0, 0.0,
1557 0.0, -8.0, 0.0, 0.0, -12.0
1558 }
1559 };
1560 // clang-format on
1561
1562 vector<int> constant_parameters;
1563 constant_parameters.push_back(0);
1564 problem_.SetParameterization(
1565 parameters_ + 2, new SubsetParameterization(2, constant_parameters));
1566
1567 CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected);
1568 }
1569
1570 struct IdentityFunctor {
1571 template <typename T>
operator ()ceres::internal::IdentityFunctor1572 bool operator()(const T* x, const T* y, T* residuals) const {
1573 residuals[0] = x[0];
1574 residuals[1] = x[1];
1575 residuals[2] = y[0];
1576 residuals[3] = y[1];
1577 residuals[4] = y[2];
1578 return true;
1579 }
1580
Createceres::internal::IdentityFunctor1581 static CostFunction* Create() {
1582 return new AutoDiffCostFunction<IdentityFunctor, 5, 2, 3>(
1583 new IdentityFunctor);
1584 }
1585 };
1586
1587 class ProblemEvaluateResidualBlockTest : public ::testing::Test {
1588 public:
1589 static constexpr bool kApplyLossFunction = true;
1590 static constexpr bool kDoNotApplyLossFunction = false;
1591 static constexpr bool kNewPoint = true;
1592 static constexpr bool kNotNewPoint = false;
1593 static double loss_function_scale_;
1594
1595 protected:
1596 ProblemImpl problem_;
1597 double x_[2] = {1, 2};
1598 double y_[3] = {1, 2, 3};
1599 };
1600
1601 double ProblemEvaluateResidualBlockTest::loss_function_scale_ = 2.0;
1602
TEST_F(ProblemEvaluateResidualBlockTest,OneResidualBlockNoLossFunctionFullEval)1603 TEST_F(ProblemEvaluateResidualBlockTest,
1604 OneResidualBlockNoLossFunctionFullEval) {
1605 ResidualBlockId residual_block_id =
1606 problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
1607 Vector expected_f(5);
1608 expected_f << 1, 2, 1, 2, 3;
1609 Matrix expected_dfdx = Matrix::Zero(5, 2);
1610 expected_dfdx.block(0, 0, 2, 2) = Matrix::Identity(2, 2);
1611 Matrix expected_dfdy = Matrix::Zero(5, 3);
1612 expected_dfdy.block(2, 0, 3, 3) = Matrix::Identity(3, 3);
1613 double expected_cost = expected_f.squaredNorm() / 2.0;
1614
1615 double actual_cost;
1616 Vector actual_f(5);
1617 Matrix actual_dfdx(5, 2);
1618 Matrix actual_dfdy(5, 3);
1619 double* jacobians[2] = {actual_dfdx.data(), actual_dfdy.data()};
1620 EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id,
1621 kApplyLossFunction,
1622 kNewPoint,
1623 &actual_cost,
1624 actual_f.data(),
1625 jacobians));
1626
1627 EXPECT_NEAR(std::abs(expected_cost - actual_cost) / actual_cost,
1628 0,
1629 std::numeric_limits<double>::epsilon())
1630 << actual_cost;
1631 EXPECT_NEAR((expected_f - actual_f).norm() / actual_f.norm(),
1632 0,
1633 std::numeric_limits<double>::epsilon())
1634 << actual_f;
1635 EXPECT_NEAR((expected_dfdx - actual_dfdx).norm() / actual_dfdx.norm(),
1636 0,
1637 std::numeric_limits<double>::epsilon())
1638 << actual_dfdx;
1639 EXPECT_NEAR((expected_dfdy - actual_dfdy).norm() / actual_dfdy.norm(),
1640 0,
1641 std::numeric_limits<double>::epsilon())
1642 << actual_dfdy;
1643 }
1644
TEST_F(ProblemEvaluateResidualBlockTest,OneResidualBlockNoLossFunctionNullEval)1645 TEST_F(ProblemEvaluateResidualBlockTest,
1646 OneResidualBlockNoLossFunctionNullEval) {
1647 ResidualBlockId residual_block_id =
1648 problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
1649 EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id,
1650 kApplyLossFunction,
1651 kNewPoint,
1652 nullptr,
1653 nullptr,
1654 nullptr));
1655 }
1656
TEST_F(ProblemEvaluateResidualBlockTest,OneResidualBlockNoLossFunctionCost)1657 TEST_F(ProblemEvaluateResidualBlockTest, OneResidualBlockNoLossFunctionCost) {
1658 ResidualBlockId residual_block_id =
1659 problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
1660 Vector expected_f(5);
1661 expected_f << 1, 2, 1, 2, 3;
1662 double expected_cost = expected_f.squaredNorm() / 2.0;
1663
1664 double actual_cost;
1665 EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id,
1666 kApplyLossFunction,
1667 kNewPoint,
1668 &actual_cost,
1669 nullptr,
1670 nullptr));
1671
1672 EXPECT_NEAR(std::abs(expected_cost - actual_cost) / actual_cost,
1673 0,
1674 std::numeric_limits<double>::epsilon())
1675 << actual_cost;
1676 }
1677
TEST_F(ProblemEvaluateResidualBlockTest,OneResidualBlockNoLossFunctionCostAndResidual)1678 TEST_F(ProblemEvaluateResidualBlockTest,
1679 OneResidualBlockNoLossFunctionCostAndResidual) {
1680 ResidualBlockId residual_block_id =
1681 problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
1682 Vector expected_f(5);
1683 expected_f << 1, 2, 1, 2, 3;
1684 double expected_cost = expected_f.squaredNorm() / 2.0;
1685
1686 double actual_cost;
1687 Vector actual_f(5);
1688 EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id,
1689 kApplyLossFunction,
1690 kNewPoint,
1691 &actual_cost,
1692 actual_f.data(),
1693 nullptr));
1694
1695 EXPECT_NEAR(std::abs(expected_cost - actual_cost) / actual_cost,
1696 0,
1697 std::numeric_limits<double>::epsilon())
1698 << actual_cost;
1699 EXPECT_NEAR((expected_f - actual_f).norm() / actual_f.norm(),
1700 0,
1701 std::numeric_limits<double>::epsilon())
1702 << actual_f;
1703 }
1704
TEST_F(ProblemEvaluateResidualBlockTest,OneResidualBlockNoLossFunctionCostResidualAndOneJacobian)1705 TEST_F(ProblemEvaluateResidualBlockTest,
1706 OneResidualBlockNoLossFunctionCostResidualAndOneJacobian) {
1707 ResidualBlockId residual_block_id =
1708 problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
1709 Vector expected_f(5);
1710 expected_f << 1, 2, 1, 2, 3;
1711 Matrix expected_dfdx = Matrix::Zero(5, 2);
1712 expected_dfdx.block(0, 0, 2, 2) = Matrix::Identity(2, 2);
1713 double expected_cost = expected_f.squaredNorm() / 2.0;
1714
1715 double actual_cost;
1716 Vector actual_f(5);
1717 Matrix actual_dfdx(5, 2);
1718 double* jacobians[2] = {actual_dfdx.data(), nullptr};
1719 EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id,
1720 kApplyLossFunction,
1721 kNewPoint,
1722 &actual_cost,
1723 actual_f.data(),
1724 jacobians));
1725
1726 EXPECT_NEAR(std::abs(expected_cost - actual_cost) / actual_cost,
1727 0,
1728 std::numeric_limits<double>::epsilon())
1729 << actual_cost;
1730 EXPECT_NEAR((expected_f - actual_f).norm() / actual_f.norm(),
1731 0,
1732 std::numeric_limits<double>::epsilon())
1733 << actual_f;
1734 EXPECT_NEAR((expected_dfdx - actual_dfdx).norm() / actual_dfdx.norm(),
1735 0,
1736 std::numeric_limits<double>::epsilon())
1737 << actual_dfdx;
1738 }
1739
TEST_F(ProblemEvaluateResidualBlockTest,OneResidualBlockNoLossFunctionResidual)1740 TEST_F(ProblemEvaluateResidualBlockTest,
1741 OneResidualBlockNoLossFunctionResidual) {
1742 ResidualBlockId residual_block_id =
1743 problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
1744 Vector expected_f(5);
1745 expected_f << 1, 2, 1, 2, 3;
1746 Vector actual_f(5);
1747 EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id,
1748 kApplyLossFunction,
1749 kNewPoint,
1750 nullptr,
1751 actual_f.data(),
1752 nullptr));
1753
1754 EXPECT_NEAR((expected_f - actual_f).norm() / actual_f.norm(),
1755 0,
1756 std::numeric_limits<double>::epsilon())
1757 << actual_f;
1758 }
1759
TEST_F(ProblemEvaluateResidualBlockTest,OneResidualBlockWithLossFunction)1760 TEST_F(ProblemEvaluateResidualBlockTest, OneResidualBlockWithLossFunction) {
1761 ResidualBlockId residual_block_id =
1762 problem_.AddResidualBlock(IdentityFunctor::Create(),
1763 new ScaledLoss(nullptr, 2.0, TAKE_OWNERSHIP),
1764 x_,
1765 y_);
1766 Vector expected_f(5);
1767 expected_f << 1, 2, 1, 2, 3;
1768 expected_f *= std::sqrt(loss_function_scale_);
1769 Matrix expected_dfdx = Matrix::Zero(5, 2);
1770 expected_dfdx.block(0, 0, 2, 2) = Matrix::Identity(2, 2);
1771 expected_dfdx *= std::sqrt(loss_function_scale_);
1772 Matrix expected_dfdy = Matrix::Zero(5, 3);
1773 expected_dfdy.block(2, 0, 3, 3) = Matrix::Identity(3, 3);
1774 expected_dfdy *= std::sqrt(loss_function_scale_);
1775 double expected_cost = expected_f.squaredNorm() / 2.0;
1776
1777 double actual_cost;
1778 Vector actual_f(5);
1779 Matrix actual_dfdx(5, 2);
1780 Matrix actual_dfdy(5, 3);
1781 double* jacobians[2] = {actual_dfdx.data(), actual_dfdy.data()};
1782 EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id,
1783 kApplyLossFunction,
1784 kNewPoint,
1785 &actual_cost,
1786 actual_f.data(),
1787 jacobians));
1788
1789 EXPECT_NEAR(std::abs(expected_cost - actual_cost) / actual_cost,
1790 0,
1791 std::numeric_limits<double>::epsilon())
1792 << actual_cost;
1793 EXPECT_NEAR((expected_f - actual_f).norm() / actual_f.norm(),
1794 0,
1795 std::numeric_limits<double>::epsilon())
1796 << actual_f;
1797 EXPECT_NEAR((expected_dfdx - actual_dfdx).norm() / actual_dfdx.norm(),
1798 0,
1799 std::numeric_limits<double>::epsilon())
1800 << actual_dfdx;
1801 EXPECT_NEAR((expected_dfdy - actual_dfdy).norm() / actual_dfdy.norm(),
1802 0,
1803 std::numeric_limits<double>::epsilon())
1804 << actual_dfdy;
1805 }
1806
TEST_F(ProblemEvaluateResidualBlockTest,OneResidualBlockWithLossFunctionDisabled)1807 TEST_F(ProblemEvaluateResidualBlockTest,
1808 OneResidualBlockWithLossFunctionDisabled) {
1809 ResidualBlockId residual_block_id =
1810 problem_.AddResidualBlock(IdentityFunctor::Create(),
1811 new ScaledLoss(nullptr, 2.0, TAKE_OWNERSHIP),
1812 x_,
1813 y_);
1814 Vector expected_f(5);
1815 expected_f << 1, 2, 1, 2, 3;
1816 Matrix expected_dfdx = Matrix::Zero(5, 2);
1817 expected_dfdx.block(0, 0, 2, 2) = Matrix::Identity(2, 2);
1818 Matrix expected_dfdy = Matrix::Zero(5, 3);
1819 expected_dfdy.block(2, 0, 3, 3) = Matrix::Identity(3, 3);
1820 double expected_cost = expected_f.squaredNorm() / 2.0;
1821
1822 double actual_cost;
1823 Vector actual_f(5);
1824 Matrix actual_dfdx(5, 2);
1825 Matrix actual_dfdy(5, 3);
1826 double* jacobians[2] = {actual_dfdx.data(), actual_dfdy.data()};
1827 EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id,
1828 kDoNotApplyLossFunction,
1829 kNewPoint,
1830 &actual_cost,
1831 actual_f.data(),
1832 jacobians));
1833
1834 EXPECT_NEAR(std::abs(expected_cost - actual_cost) / actual_cost,
1835 0,
1836 std::numeric_limits<double>::epsilon())
1837 << actual_cost;
1838 EXPECT_NEAR((expected_f - actual_f).norm() / actual_f.norm(),
1839 0,
1840 std::numeric_limits<double>::epsilon())
1841 << actual_f;
1842 EXPECT_NEAR((expected_dfdx - actual_dfdx).norm() / actual_dfdx.norm(),
1843 0,
1844 std::numeric_limits<double>::epsilon())
1845 << actual_dfdx;
1846 EXPECT_NEAR((expected_dfdy - actual_dfdy).norm() / actual_dfdy.norm(),
1847 0,
1848 std::numeric_limits<double>::epsilon())
1849 << actual_dfdy;
1850 }
1851
TEST_F(ProblemEvaluateResidualBlockTest,OneResidualBlockWithOneLocalParameterization)1852 TEST_F(ProblemEvaluateResidualBlockTest,
1853 OneResidualBlockWithOneLocalParameterization) {
1854 ResidualBlockId residual_block_id =
1855 problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
1856 problem_.SetParameterization(x_, new SubsetParameterization(2, {1}));
1857
1858 Vector expected_f(5);
1859 expected_f << 1, 2, 1, 2, 3;
1860 Matrix expected_dfdx = Matrix::Zero(5, 1);
1861 expected_dfdx.block(0, 0, 1, 1) = Matrix::Identity(1, 1);
1862 Matrix expected_dfdy = Matrix::Zero(5, 3);
1863 expected_dfdy.block(2, 0, 3, 3) = Matrix::Identity(3, 3);
1864 double expected_cost = expected_f.squaredNorm() / 2.0;
1865
1866 double actual_cost;
1867 Vector actual_f(5);
1868 Matrix actual_dfdx(5, 1);
1869 Matrix actual_dfdy(5, 3);
1870 double* jacobians[2] = {actual_dfdx.data(), actual_dfdy.data()};
1871 EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id,
1872 kApplyLossFunction,
1873 kNewPoint,
1874 &actual_cost,
1875 actual_f.data(),
1876 jacobians));
1877
1878 EXPECT_NEAR(std::abs(expected_cost - actual_cost) / actual_cost,
1879 0,
1880 std::numeric_limits<double>::epsilon())
1881 << actual_cost;
1882 EXPECT_NEAR((expected_f - actual_f).norm() / actual_f.norm(),
1883 0,
1884 std::numeric_limits<double>::epsilon())
1885 << actual_f;
1886 EXPECT_NEAR((expected_dfdx - actual_dfdx).norm() / actual_dfdx.norm(),
1887 0,
1888 std::numeric_limits<double>::epsilon())
1889 << actual_dfdx;
1890 EXPECT_NEAR((expected_dfdy - actual_dfdy).norm() / actual_dfdy.norm(),
1891 0,
1892 std::numeric_limits<double>::epsilon())
1893 << actual_dfdy;
1894 }
1895
TEST_F(ProblemEvaluateResidualBlockTest,OneResidualBlockWithTwoLocalParameterizations)1896 TEST_F(ProblemEvaluateResidualBlockTest,
1897 OneResidualBlockWithTwoLocalParameterizations) {
1898 ResidualBlockId residual_block_id =
1899 problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
1900 problem_.SetParameterization(x_, new SubsetParameterization(2, {1}));
1901 problem_.SetParameterization(y_, new SubsetParameterization(3, {2}));
1902
1903 Vector expected_f(5);
1904 expected_f << 1, 2, 1, 2, 3;
1905 Matrix expected_dfdx = Matrix::Zero(5, 1);
1906 expected_dfdx.block(0, 0, 1, 1) = Matrix::Identity(1, 1);
1907 Matrix expected_dfdy = Matrix::Zero(5, 2);
1908 expected_dfdy.block(2, 0, 2, 2) = Matrix::Identity(2, 2);
1909 double expected_cost = expected_f.squaredNorm() / 2.0;
1910
1911 double actual_cost;
1912 Vector actual_f(5);
1913 Matrix actual_dfdx(5, 1);
1914 Matrix actual_dfdy(5, 2);
1915 double* jacobians[2] = {actual_dfdx.data(), actual_dfdy.data()};
1916 EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id,
1917 kApplyLossFunction,
1918 kNewPoint,
1919 &actual_cost,
1920 actual_f.data(),
1921 jacobians));
1922
1923 EXPECT_NEAR(std::abs(expected_cost - actual_cost) / actual_cost,
1924 0,
1925 std::numeric_limits<double>::epsilon())
1926 << actual_cost;
1927 EXPECT_NEAR((expected_f - actual_f).norm() / actual_f.norm(),
1928 0,
1929 std::numeric_limits<double>::epsilon())
1930 << actual_f;
1931 EXPECT_NEAR((expected_dfdx - actual_dfdx).norm() / actual_dfdx.norm(),
1932 0,
1933 std::numeric_limits<double>::epsilon())
1934 << actual_dfdx;
1935 EXPECT_NEAR((expected_dfdy - actual_dfdy).norm() / actual_dfdy.norm(),
1936 0,
1937 std::numeric_limits<double>::epsilon())
1938 << actual_dfdy;
1939 }
1940
TEST_F(ProblemEvaluateResidualBlockTest,OneResidualBlockWithOneConstantParameterBlock)1941 TEST_F(ProblemEvaluateResidualBlockTest,
1942 OneResidualBlockWithOneConstantParameterBlock) {
1943 ResidualBlockId residual_block_id =
1944 problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
1945 problem_.SetParameterBlockConstant(x_);
1946
1947 Vector expected_f(5);
1948 expected_f << 1, 2, 1, 2, 3;
1949 Matrix expected_dfdy = Matrix::Zero(5, 3);
1950 expected_dfdy.block(2, 0, 3, 3) = Matrix::Identity(3, 3);
1951 double expected_cost = expected_f.squaredNorm() / 2.0;
1952
1953 double actual_cost;
1954 Vector actual_f(5);
1955 Matrix actual_dfdx(5, 2);
1956 Matrix actual_dfdy(5, 3);
1957
1958 // Try evaluating both Jacobians, this should fail.
1959 double* jacobians[2] = {actual_dfdx.data(), actual_dfdy.data()};
1960 EXPECT_FALSE(problem_.EvaluateResidualBlock(residual_block_id,
1961 kApplyLossFunction,
1962 kNewPoint,
1963 &actual_cost,
1964 actual_f.data(),
1965 jacobians));
1966
1967 jacobians[0] = nullptr;
1968 EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id,
1969 kApplyLossFunction,
1970 kNewPoint,
1971 &actual_cost,
1972 actual_f.data(),
1973 jacobians));
1974
1975 EXPECT_NEAR(std::abs(expected_cost - actual_cost) / actual_cost,
1976 0,
1977 std::numeric_limits<double>::epsilon())
1978 << actual_cost;
1979 EXPECT_NEAR((expected_f - actual_f).norm() / actual_f.norm(),
1980 0,
1981 std::numeric_limits<double>::epsilon())
1982 << actual_f;
1983 EXPECT_NEAR((expected_dfdy - actual_dfdy).norm() / actual_dfdy.norm(),
1984 0,
1985 std::numeric_limits<double>::epsilon())
1986 << actual_dfdy;
1987 }
1988
TEST_F(ProblemEvaluateResidualBlockTest,OneResidualBlockWithAllConstantParameterBlocks)1989 TEST_F(ProblemEvaluateResidualBlockTest,
1990 OneResidualBlockWithAllConstantParameterBlocks) {
1991 ResidualBlockId residual_block_id =
1992 problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
1993 problem_.SetParameterBlockConstant(x_);
1994 problem_.SetParameterBlockConstant(y_);
1995
1996 Vector expected_f(5);
1997 expected_f << 1, 2, 1, 2, 3;
1998 double expected_cost = expected_f.squaredNorm() / 2.0;
1999
2000 double actual_cost;
2001 Vector actual_f(5);
2002 Matrix actual_dfdx(5, 2);
2003 Matrix actual_dfdy(5, 3);
2004
2005 // Try evaluating with one or more Jacobians, this should fail.
2006 double* jacobians[2] = {actual_dfdx.data(), actual_dfdy.data()};
2007 EXPECT_FALSE(problem_.EvaluateResidualBlock(residual_block_id,
2008 kApplyLossFunction,
2009 kNewPoint,
2010 &actual_cost,
2011 actual_f.data(),
2012 jacobians));
2013
2014 jacobians[0] = nullptr;
2015 EXPECT_FALSE(problem_.EvaluateResidualBlock(residual_block_id,
2016 kApplyLossFunction,
2017 kNewPoint,
2018 &actual_cost,
2019 actual_f.data(),
2020 jacobians));
2021 jacobians[1] = nullptr;
2022 EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id,
2023 kApplyLossFunction,
2024 kNewPoint,
2025 &actual_cost,
2026 actual_f.data(),
2027 jacobians));
2028
2029 EXPECT_NEAR(std::abs(expected_cost - actual_cost) / actual_cost,
2030 0,
2031 std::numeric_limits<double>::epsilon())
2032 << actual_cost;
2033 EXPECT_NEAR((expected_f - actual_f).norm() / actual_f.norm(),
2034 0,
2035 std::numeric_limits<double>::epsilon())
2036 << actual_f;
2037 }
2038
TEST_F(ProblemEvaluateResidualBlockTest,OneResidualBlockWithOneParameterBlockConstantAndParameterBlockChanged)2039 TEST_F(ProblemEvaluateResidualBlockTest,
2040 OneResidualBlockWithOneParameterBlockConstantAndParameterBlockChanged) {
2041 ResidualBlockId residual_block_id =
2042 problem_.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
2043 problem_.SetParameterBlockConstant(x_);
2044
2045 x_[0] = 2;
2046 y_[2] = 1;
2047 Vector expected_f(5);
2048 expected_f << 2, 2, 1, 2, 1;
2049 Matrix expected_dfdy = Matrix::Zero(5, 3);
2050 expected_dfdy.block(2, 0, 3, 3) = Matrix::Identity(3, 3);
2051 double expected_cost = expected_f.squaredNorm() / 2.0;
2052
2053 double actual_cost;
2054 Vector actual_f(5);
2055 Matrix actual_dfdx(5, 2);
2056 Matrix actual_dfdy(5, 3);
2057
2058 // Try evaluating with one or more Jacobians, this should fail.
2059 double* jacobians[2] = {actual_dfdx.data(), actual_dfdy.data()};
2060 EXPECT_FALSE(problem_.EvaluateResidualBlock(residual_block_id,
2061 kApplyLossFunction,
2062 kNewPoint,
2063 &actual_cost,
2064 actual_f.data(),
2065 jacobians));
2066
2067 jacobians[0] = nullptr;
2068 EXPECT_TRUE(problem_.EvaluateResidualBlock(residual_block_id,
2069 kApplyLossFunction,
2070 kNewPoint,
2071 &actual_cost,
2072 actual_f.data(),
2073 jacobians));
2074 EXPECT_NEAR(std::abs(expected_cost - actual_cost) / actual_cost,
2075 0,
2076 std::numeric_limits<double>::epsilon())
2077 << actual_cost;
2078 EXPECT_NEAR((expected_f - actual_f).norm() / actual_f.norm(),
2079 0,
2080 std::numeric_limits<double>::epsilon())
2081 << actual_f;
2082 EXPECT_NEAR((expected_dfdy - actual_dfdy).norm() / actual_dfdy.norm(),
2083 0,
2084 std::numeric_limits<double>::epsilon())
2085 << actual_dfdy;
2086 }
2087
TEST(Problem,SetAndGetParameterLowerBound)2088 TEST(Problem, SetAndGetParameterLowerBound) {
2089 Problem problem;
2090 double x[] = {1.0, 2.0};
2091 problem.AddParameterBlock(x, 2);
2092
2093 EXPECT_EQ(problem.GetParameterLowerBound(x, 0),
2094 -std::numeric_limits<double>::max());
2095 EXPECT_EQ(problem.GetParameterLowerBound(x, 1),
2096 -std::numeric_limits<double>::max());
2097
2098 problem.SetParameterLowerBound(x, 0, -1.0);
2099 EXPECT_EQ(problem.GetParameterLowerBound(x, 0), -1.0);
2100 EXPECT_EQ(problem.GetParameterLowerBound(x, 1),
2101 -std::numeric_limits<double>::max());
2102
2103 problem.SetParameterLowerBound(x, 0, -2.0);
2104 EXPECT_EQ(problem.GetParameterLowerBound(x, 0), -2.0);
2105 EXPECT_EQ(problem.GetParameterLowerBound(x, 1),
2106 -std::numeric_limits<double>::max());
2107
2108 problem.SetParameterLowerBound(x, 0, -std::numeric_limits<double>::max());
2109 EXPECT_EQ(problem.GetParameterLowerBound(x, 0),
2110 -std::numeric_limits<double>::max());
2111 EXPECT_EQ(problem.GetParameterLowerBound(x, 1),
2112 -std::numeric_limits<double>::max());
2113 }
2114
TEST(Problem,SetAndGetParameterUpperBound)2115 TEST(Problem, SetAndGetParameterUpperBound) {
2116 Problem problem;
2117 double x[] = {1.0, 2.0};
2118 problem.AddParameterBlock(x, 2);
2119
2120 EXPECT_EQ(problem.GetParameterUpperBound(x, 0),
2121 std::numeric_limits<double>::max());
2122 EXPECT_EQ(problem.GetParameterUpperBound(x, 1),
2123 std::numeric_limits<double>::max());
2124
2125 problem.SetParameterUpperBound(x, 0, -1.0);
2126 EXPECT_EQ(problem.GetParameterUpperBound(x, 0), -1.0);
2127 EXPECT_EQ(problem.GetParameterUpperBound(x, 1),
2128 std::numeric_limits<double>::max());
2129
2130 problem.SetParameterUpperBound(x, 0, -2.0);
2131 EXPECT_EQ(problem.GetParameterUpperBound(x, 0), -2.0);
2132 EXPECT_EQ(problem.GetParameterUpperBound(x, 1),
2133 std::numeric_limits<double>::max());
2134
2135 problem.SetParameterUpperBound(x, 0, std::numeric_limits<double>::max());
2136 EXPECT_EQ(problem.GetParameterUpperBound(x, 0),
2137 std::numeric_limits<double>::max());
2138 EXPECT_EQ(problem.GetParameterUpperBound(x, 1),
2139 std::numeric_limits<double>::max());
2140 }
2141
TEST(Problem,SetParameterizationTwice)2142 TEST(Problem, SetParameterizationTwice) {
2143 Problem problem;
2144 double x[] = {1.0, 2.0, 3.0};
2145 problem.AddParameterBlock(x, 3);
2146 problem.SetParameterization(x, new SubsetParameterization(3, {1}));
2147 EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 3);
2148 EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 2);
2149
2150 problem.SetParameterization(x, new SubsetParameterization(3, {0, 1}));
2151 EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 3);
2152 EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 1);
2153 }
2154
TEST(Problem,SetParameterizationAndThenClearItWithNull)2155 TEST(Problem, SetParameterizationAndThenClearItWithNull) {
2156 Problem problem;
2157 double x[] = {1.0, 2.0, 3.0};
2158 problem.AddParameterBlock(x, 3);
2159 problem.SetParameterization(x, new SubsetParameterization(3, {1}));
2160 EXPECT_EQ(problem.GetParameterization(x)->GlobalSize(), 3);
2161 EXPECT_EQ(problem.GetParameterization(x)->LocalSize(), 2);
2162
2163 problem.SetParameterization(x, nullptr);
2164 EXPECT_EQ(problem.GetParameterization(x), nullptr);
2165 EXPECT_EQ(problem.ParameterBlockLocalSize(x), 3);
2166 EXPECT_EQ(problem.ParameterBlockSize(x), 3);
2167 }
2168
TEST(Solver,ZeroSizedLocalParameterizationMeansParameterBlockIsConstant)2169 TEST(Solver, ZeroSizedLocalParameterizationMeansParameterBlockIsConstant) {
2170 double x = 0.0;
2171 double y = 1.0;
2172 Problem problem;
2173 problem.AddResidualBlock(new BinaryCostFunction(1, 1, 1), nullptr, &x, &y);
2174 problem.SetParameterization(&y, new SubsetParameterization(1, {0}));
2175 EXPECT_TRUE(problem.IsParameterBlockConstant(&y));
2176 }
2177
2178 class MockEvaluationCallback : public EvaluationCallback {
2179 public:
2180 MOCK_METHOD2(PrepareForEvaluation, void(bool, bool));
2181 };
2182
TEST(ProblemEvaluate,CallsEvaluationCallbackWithoutJacobian)2183 TEST(ProblemEvaluate, CallsEvaluationCallbackWithoutJacobian) {
2184 constexpr bool kDoNotComputeJacobians = false;
2185 constexpr bool kNewPoint = true;
2186
2187 MockEvaluationCallback evaluation_callback;
2188 EXPECT_CALL(evaluation_callback,
2189 PrepareForEvaluation(kDoNotComputeJacobians, kNewPoint))
2190 .Times(1);
2191
2192 Problem::Options options;
2193 options.evaluation_callback = &evaluation_callback;
2194 ProblemImpl problem(options);
2195 double x_[2] = {1, 2};
2196 double y_[3] = {1, 2, 3};
2197 problem.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
2198
2199 double actual_cost;
2200 EXPECT_TRUE(problem.Evaluate(
2201 Problem::EvaluateOptions(), &actual_cost, nullptr, nullptr, nullptr));
2202 }
2203
TEST(ProblemEvaluate,CallsEvaluationCallbackWithJacobian)2204 TEST(ProblemEvaluate, CallsEvaluationCallbackWithJacobian) {
2205 constexpr bool kComputeJacobians = true;
2206 constexpr bool kNewPoint = true;
2207
2208 MockEvaluationCallback evaluation_callback;
2209 EXPECT_CALL(evaluation_callback,
2210 PrepareForEvaluation(kComputeJacobians, kNewPoint))
2211 .Times(1);
2212
2213 Problem::Options options;
2214 options.evaluation_callback = &evaluation_callback;
2215 ProblemImpl problem(options);
2216 double x_[2] = {1, 2};
2217 double y_[3] = {1, 2, 3};
2218 problem.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
2219
2220 double actual_cost;
2221 ceres::CRSMatrix jacobian;
2222 EXPECT_TRUE(problem.Evaluate(
2223 Problem::EvaluateOptions(), &actual_cost, nullptr, nullptr, &jacobian));
2224 }
2225
TEST(ProblemEvaluateResidualBlock,NewPointCallsEvaluationCallback)2226 TEST(ProblemEvaluateResidualBlock, NewPointCallsEvaluationCallback) {
2227 constexpr bool kComputeJacobians = true;
2228 constexpr bool kNewPoint = true;
2229
2230 MockEvaluationCallback evaluation_callback;
2231 EXPECT_CALL(evaluation_callback,
2232 PrepareForEvaluation(kComputeJacobians, kNewPoint))
2233 .Times(1);
2234
2235 Problem::Options options;
2236 options.evaluation_callback = &evaluation_callback;
2237 ProblemImpl problem(options);
2238 double x_[2] = {1, 2};
2239 double y_[3] = {1, 2, 3};
2240 ResidualBlockId residual_block_id =
2241 problem.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
2242
2243 double actual_cost;
2244 Vector actual_f(5);
2245 Matrix actual_dfdx(5, 2);
2246 Matrix actual_dfdy(5, 3);
2247 double* jacobians[2] = {actual_dfdx.data(), actual_dfdy.data()};
2248 EXPECT_TRUE(problem.EvaluateResidualBlock(
2249 residual_block_id, true, true, &actual_cost, actual_f.data(), jacobians));
2250 }
2251
TEST(ProblemEvaluateResidualBlock,OldPointCallsEvaluationCallback)2252 TEST(ProblemEvaluateResidualBlock, OldPointCallsEvaluationCallback) {
2253 constexpr bool kComputeJacobians = true;
2254 constexpr bool kOldPoint = false;
2255
2256 MockEvaluationCallback evaluation_callback;
2257 EXPECT_CALL(evaluation_callback,
2258 PrepareForEvaluation(kComputeJacobians, kOldPoint))
2259 .Times(1);
2260
2261 Problem::Options options;
2262 options.evaluation_callback = &evaluation_callback;
2263 ProblemImpl problem(options);
2264 double x_[2] = {1, 2};
2265 double y_[3] = {1, 2, 3};
2266 ResidualBlockId residual_block_id =
2267 problem.AddResidualBlock(IdentityFunctor::Create(), nullptr, x_, y_);
2268
2269 double actual_cost;
2270 Vector actual_f(5);
2271 Matrix actual_dfdx(5, 2);
2272 Matrix actual_dfdy(5, 3);
2273 double* jacobians[2] = {actual_dfdx.data(), actual_dfdy.data()};
2274 EXPECT_TRUE(problem.EvaluateResidualBlock(residual_block_id,
2275 true,
2276 false,
2277 &actual_cost,
2278 actual_f.data(),
2279 jacobians));
2280 }
2281
2282 } // namespace internal
2283 } // namespace ceres
2284