1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2013 Google Inc. All rights reserved. 3 // http://code.google.com/p/ceres-solver/ 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are met: 7 // 8 // * Redistributions of source code must retain the above copyright notice, 9 // this list of conditions and the following disclaimer. 10 // * Redistributions in binary form must reproduce the above copyright notice, 11 // this list of conditions and the following disclaimer in the documentation 12 // and/or other materials provided with the distribution. 13 // * Neither the name of Google Inc. nor the names of its contributors may be 14 // used to endorse or promote products derived from this software without 15 // specific prior written permission. 16 // 17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 // POSSIBILITY OF SUCH DAMAGE. 28 // 29 // Author: sameeragarwal (at) google.com (Sameer Agarwal) 30 // keir (at) google.com (Keir Mierle) 31 32 #include "ceres/problem.h" 33 #include "ceres/problem_impl.h" 34 35 #include "ceres/casts.h" 36 #include "ceres/cost_function.h" 37 #include "ceres/crs_matrix.h" 38 #include "ceres/evaluator_test_utils.cc" 39 #include "ceres/internal/eigen.h" 40 #include "ceres/internal/scoped_ptr.h" 41 #include "ceres/local_parameterization.h" 42 #include "ceres/map_util.h" 43 #include "ceres/parameter_block.h" 44 #include "ceres/program.h" 45 #include "ceres/sized_cost_function.h" 46 #include "ceres/sparse_matrix.h" 47 #include "ceres/types.h" 48 #include "gtest/gtest.h" 49 50 namespace ceres { 51 namespace internal { 52 53 // The following three classes are for the purposes of defining 54 // function signatures. They have dummy Evaluate functions. 55 56 // Trivial cost function that accepts a single argument. 57 class UnaryCostFunction : public CostFunction { 58 public: 59 UnaryCostFunction(int num_residuals, int32 parameter_block_size) { 60 set_num_residuals(num_residuals); 61 mutable_parameter_block_sizes()->push_back(parameter_block_size); 62 } 63 virtual ~UnaryCostFunction() {} 64 65 virtual bool Evaluate(double const* const* parameters, 66 double* residuals, 67 double** jacobians) const { 68 for (int i = 0; i < num_residuals(); ++i) { 69 residuals[i] = 1; 70 } 71 return true; 72 } 73 }; 74 75 // Trivial cost function that accepts two arguments. 76 class BinaryCostFunction: public CostFunction { 77 public: 78 BinaryCostFunction(int num_residuals, 79 int32 parameter_block1_size, 80 int32 parameter_block2_size) { 81 set_num_residuals(num_residuals); 82 mutable_parameter_block_sizes()->push_back(parameter_block1_size); 83 mutable_parameter_block_sizes()->push_back(parameter_block2_size); 84 } 85 86 virtual bool Evaluate(double const* const* parameters, 87 double* residuals, 88 double** jacobians) const { 89 for (int i = 0; i < num_residuals(); ++i) { 90 residuals[i] = 2; 91 } 92 return true; 93 } 94 }; 95 96 // Trivial cost function that accepts three arguments. 97 class TernaryCostFunction: public CostFunction { 98 public: 99 TernaryCostFunction(int num_residuals, 100 int32 parameter_block1_size, 101 int32 parameter_block2_size, 102 int32 parameter_block3_size) { 103 set_num_residuals(num_residuals); 104 mutable_parameter_block_sizes()->push_back(parameter_block1_size); 105 mutable_parameter_block_sizes()->push_back(parameter_block2_size); 106 mutable_parameter_block_sizes()->push_back(parameter_block3_size); 107 } 108 109 virtual bool Evaluate(double const* const* parameters, 110 double* residuals, 111 double** jacobians) const { 112 for (int i = 0; i < num_residuals(); ++i) { 113 residuals[i] = 3; 114 } 115 return true; 116 } 117 }; 118 119 TEST(Problem, AddResidualWithNullCostFunctionDies) { 120 double x[3], y[4], z[5]; 121 122 Problem problem; 123 problem.AddParameterBlock(x, 3); 124 problem.AddParameterBlock(y, 4); 125 problem.AddParameterBlock(z, 5); 126 127 EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock(NULL, NULL, x), 128 "'cost_function' Must be non NULL"); 129 } 130 131 TEST(Problem, AddResidualWithIncorrectNumberOfParameterBlocksDies) { 132 double x[3], y[4], z[5]; 133 134 Problem problem; 135 problem.AddParameterBlock(x, 3); 136 problem.AddParameterBlock(y, 4); 137 problem.AddParameterBlock(z, 5); 138 139 // UnaryCostFunction takes only one parameter, but two are passed. 140 EXPECT_DEATH_IF_SUPPORTED( 141 problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x, y), 142 "parameter_blocks.size"); 143 } 144 145 TEST(Problem, AddResidualWithDifferentSizesOnTheSameVariableDies) { 146 double x[3]; 147 148 Problem problem; 149 problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); 150 EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock( 151 new UnaryCostFunction( 152 2, 4 /* 4 != 3 */), NULL, x), 153 "different block sizes"); 154 } 155 156 TEST(Problem, AddResidualWithDuplicateParametersDies) { 157 double x[3], z[5]; 158 159 Problem problem; 160 EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock( 161 new BinaryCostFunction(2, 3, 3), NULL, x, x), 162 "Duplicate parameter blocks"); 163 EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock( 164 new TernaryCostFunction(1, 5, 3, 5), 165 NULL, z, x, z), 166 "Duplicate parameter blocks"); 167 } 168 169 TEST(Problem, AddResidualWithIncorrectSizesOfParameterBlockDies) { 170 double x[3], y[4], z[5]; 171 172 Problem problem; 173 problem.AddParameterBlock(x, 3); 174 problem.AddParameterBlock(y, 4); 175 problem.AddParameterBlock(z, 5); 176 177 // The cost function expects the size of the second parameter, z, to be 4 178 // instead of 5 as declared above. This is fatal. 179 EXPECT_DEATH_IF_SUPPORTED(problem.AddResidualBlock( 180 new BinaryCostFunction(2, 3, 4), NULL, x, z), 181 "different block sizes"); 182 } 183 184 TEST(Problem, AddResidualAddsDuplicatedParametersOnlyOnce) { 185 double x[3], y[4], z[5]; 186 187 Problem problem; 188 problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); 189 problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); 190 problem.AddResidualBlock(new UnaryCostFunction(2, 4), NULL, y); 191 problem.AddResidualBlock(new UnaryCostFunction(2, 5), NULL, z); 192 193 EXPECT_EQ(3, problem.NumParameterBlocks()); 194 EXPECT_EQ(12, problem.NumParameters()); 195 } 196 197 TEST(Problem, AddParameterWithDifferentSizesOnTheSameVariableDies) { 198 double x[3], y[4]; 199 200 Problem problem; 201 problem.AddParameterBlock(x, 3); 202 problem.AddParameterBlock(y, 4); 203 204 EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(x, 4), 205 "different block sizes"); 206 } 207 208 static double *IntToPtr(int i) { 209 return reinterpret_cast<double*>(sizeof(double) * i); // NOLINT 210 } 211 212 TEST(Problem, AddParameterWithAliasedParametersDies) { 213 // Layout is 214 // 215 // 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 216 // [x] x x x x [y] y y 217 // o==o==o o==o==o o==o 218 // o--o--o o--o--o o--o o--o--o 219 // 220 // Parameter block additions are tested as listed above; expected successful 221 // ones marked with o==o and aliasing ones marked with o--o. 222 223 Problem problem; 224 problem.AddParameterBlock(IntToPtr(5), 5); // x 225 problem.AddParameterBlock(IntToPtr(13), 3); // y 226 227 EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 4), 2), 228 "Aliasing detected"); 229 EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 4), 3), 230 "Aliasing detected"); 231 EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 4), 9), 232 "Aliasing detected"); 233 EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr( 8), 3), 234 "Aliasing detected"); 235 EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr(12), 2), 236 "Aliasing detected"); 237 EXPECT_DEATH_IF_SUPPORTED(problem.AddParameterBlock(IntToPtr(14), 3), 238 "Aliasing detected"); 239 240 // These ones should work. 241 problem.AddParameterBlock(IntToPtr( 2), 3); 242 problem.AddParameterBlock(IntToPtr(10), 3); 243 problem.AddParameterBlock(IntToPtr(16), 2); 244 245 ASSERT_EQ(5, problem.NumParameterBlocks()); 246 } 247 248 TEST(Problem, AddParameterIgnoresDuplicateCalls) { 249 double x[3], y[4]; 250 251 Problem problem; 252 problem.AddParameterBlock(x, 3); 253 problem.AddParameterBlock(y, 4); 254 255 // Creating parameter blocks multiple times is ignored. 256 problem.AddParameterBlock(x, 3); 257 problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); 258 259 // ... even repeatedly. 260 problem.AddParameterBlock(x, 3); 261 problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); 262 263 // More parameters are fine. 264 problem.AddParameterBlock(y, 4); 265 problem.AddResidualBlock(new UnaryCostFunction(2, 4), NULL, y); 266 267 EXPECT_EQ(2, problem.NumParameterBlocks()); 268 EXPECT_EQ(7, problem.NumParameters()); 269 } 270 271 TEST(Problem, AddingParametersAndResidualsResultsInExpectedProblem) { 272 double x[3], y[4], z[5], w[4]; 273 274 Problem problem; 275 problem.AddParameterBlock(x, 3); 276 EXPECT_EQ(1, problem.NumParameterBlocks()); 277 EXPECT_EQ(3, problem.NumParameters()); 278 279 problem.AddParameterBlock(y, 4); 280 EXPECT_EQ(2, problem.NumParameterBlocks()); 281 EXPECT_EQ(7, problem.NumParameters()); 282 283 problem.AddParameterBlock(z, 5); 284 EXPECT_EQ(3, problem.NumParameterBlocks()); 285 EXPECT_EQ(12, problem.NumParameters()); 286 287 // Add a parameter that has a local parameterization. 288 w[0] = 1.0; w[1] = 0.0; w[2] = 0.0; w[3] = 0.0; 289 problem.AddParameterBlock(w, 4, new QuaternionParameterization); 290 EXPECT_EQ(4, problem.NumParameterBlocks()); 291 EXPECT_EQ(16, problem.NumParameters()); 292 293 problem.AddResidualBlock(new UnaryCostFunction(2, 3), NULL, x); 294 problem.AddResidualBlock(new BinaryCostFunction(6, 5, 4) , NULL, z, y); 295 problem.AddResidualBlock(new BinaryCostFunction(3, 3, 5), NULL, x, z); 296 problem.AddResidualBlock(new BinaryCostFunction(7, 5, 3), NULL, z, x); 297 problem.AddResidualBlock(new TernaryCostFunction(1, 5, 3, 4), NULL, z, x, y); 298 299 const int total_residuals = 2 + 6 + 3 + 7 + 1; 300 EXPECT_EQ(problem.NumResidualBlocks(), 5); 301 EXPECT_EQ(problem.NumResiduals(), total_residuals); 302 } 303 304 class DestructorCountingCostFunction : public SizedCostFunction<3, 4, 5> { 305 public: 306 explicit DestructorCountingCostFunction(int *num_destructions) 307 : num_destructions_(num_destructions) {} 308 309 virtual ~DestructorCountingCostFunction() { 310 *num_destructions_ += 1; 311 } 312 313 virtual bool Evaluate(double const* const* parameters, 314 double* residuals, 315 double** jacobians) const { 316 return true; 317 } 318 319 private: 320 int* num_destructions_; 321 }; 322 323 TEST(Problem, ReusedCostFunctionsAreOnlyDeletedOnce) { 324 double y[4], z[5]; 325 int num_destructions = 0; 326 327 // Add a cost function multiple times and check to make sure that 328 // the destructor on the cost function is only called once. 329 { 330 Problem problem; 331 problem.AddParameterBlock(y, 4); 332 problem.AddParameterBlock(z, 5); 333 334 CostFunction* cost = new DestructorCountingCostFunction(&num_destructions); 335 problem.AddResidualBlock(cost, NULL, y, z); 336 problem.AddResidualBlock(cost, NULL, y, z); 337 problem.AddResidualBlock(cost, NULL, y, z); 338 EXPECT_EQ(3, problem.NumResidualBlocks()); 339 } 340 341 // Check that the destructor was called only once. 342 CHECK_EQ(num_destructions, 1); 343 } 344 345 TEST(Problem, CostFunctionsAreDeletedEvenWithRemovals) { 346 double y[4], z[5], w[4]; 347 int num_destructions = 0; 348 { 349 Problem problem; 350 problem.AddParameterBlock(y, 4); 351 problem.AddParameterBlock(z, 5); 352 353 CostFunction* cost_yz = 354 new DestructorCountingCostFunction(&num_destructions); 355 CostFunction* cost_wz = 356 new DestructorCountingCostFunction(&num_destructions); 357 ResidualBlock* r_yz = problem.AddResidualBlock(cost_yz, NULL, y, z); 358 ResidualBlock* r_wz = problem.AddResidualBlock(cost_wz, NULL, w, z); 359 EXPECT_EQ(2, problem.NumResidualBlocks()); 360 361 // In the current implementation, the destructor shouldn't get run yet. 362 problem.RemoveResidualBlock(r_yz); 363 CHECK_EQ(num_destructions, 0); 364 problem.RemoveResidualBlock(r_wz); 365 CHECK_EQ(num_destructions, 0); 366 367 EXPECT_EQ(0, problem.NumResidualBlocks()); 368 } 369 CHECK_EQ(num_destructions, 2); 370 } 371 372 // Make the dynamic problem tests (e.g. for removing residual blocks) 373 // parameterized on whether the low-latency mode is enabled or not. 374 // 375 // This tests against ProblemImpl instead of Problem in order to inspect the 376 // state of the resulting Program; this is difficult with only the thin Problem 377 // interface. 378 struct DynamicProblem : public ::testing::TestWithParam<bool> { 379 DynamicProblem() { 380 Problem::Options options; 381 options.enable_fast_removal = GetParam(); 382 problem.reset(new ProblemImpl(options)); 383 } 384 385 ParameterBlock* GetParameterBlock(int block) { 386 return problem->program().parameter_blocks()[block]; 387 } 388 ResidualBlock* GetResidualBlock(int block) { 389 return problem->program().residual_blocks()[block]; 390 } 391 392 bool HasResidualBlock(ResidualBlock* residual_block) { 393 bool have_residual_block = true; 394 if (GetParam()) { 395 have_residual_block &= 396 (problem->residual_block_set().find(residual_block) != 397 problem->residual_block_set().end()); 398 } 399 have_residual_block &= 400 find(problem->program().residual_blocks().begin(), 401 problem->program().residual_blocks().end(), 402 residual_block) != problem->program().residual_blocks().end(); 403 return have_residual_block; 404 } 405 406 int NumResidualBlocks() { 407 // Verify that the hash set of residuals is maintained consistently. 408 if (GetParam()) { 409 EXPECT_EQ(problem->residual_block_set().size(), 410 problem->NumResidualBlocks()); 411 } 412 return problem->NumResidualBlocks(); 413 } 414 415 // The next block of functions until the end are only for testing the 416 // residual block removals. 417 void ExpectParameterBlockContainsResidualBlock( 418 double* values, 419 ResidualBlock* residual_block) { 420 ParameterBlock* parameter_block = 421 FindOrDie(problem->parameter_map(), values); 422 EXPECT_TRUE(ContainsKey(*(parameter_block->mutable_residual_blocks()), 423 residual_block)); 424 } 425 426 void ExpectSize(double* values, int size) { 427 ParameterBlock* parameter_block = 428 FindOrDie(problem->parameter_map(), values); 429 EXPECT_EQ(size, parameter_block->mutable_residual_blocks()->size()); 430 } 431 432 // Degenerate case. 433 void ExpectParameterBlockContains(double* values) { 434 ExpectSize(values, 0); 435 } 436 437 void ExpectParameterBlockContains(double* values, 438 ResidualBlock* r1) { 439 ExpectSize(values, 1); 440 ExpectParameterBlockContainsResidualBlock(values, r1); 441 } 442 443 void ExpectParameterBlockContains(double* values, 444 ResidualBlock* r1, 445 ResidualBlock* r2) { 446 ExpectSize(values, 2); 447 ExpectParameterBlockContainsResidualBlock(values, r1); 448 ExpectParameterBlockContainsResidualBlock(values, r2); 449 } 450 451 void ExpectParameterBlockContains(double* values, 452 ResidualBlock* r1, 453 ResidualBlock* r2, 454 ResidualBlock* r3) { 455 ExpectSize(values, 3); 456 ExpectParameterBlockContainsResidualBlock(values, r1); 457 ExpectParameterBlockContainsResidualBlock(values, r2); 458 ExpectParameterBlockContainsResidualBlock(values, r3); 459 } 460 461 void ExpectParameterBlockContains(double* values, 462 ResidualBlock* r1, 463 ResidualBlock* r2, 464 ResidualBlock* r3, 465 ResidualBlock* r4) { 466 ExpectSize(values, 4); 467 ExpectParameterBlockContainsResidualBlock(values, r1); 468 ExpectParameterBlockContainsResidualBlock(values, r2); 469 ExpectParameterBlockContainsResidualBlock(values, r3); 470 ExpectParameterBlockContainsResidualBlock(values, r4); 471 } 472 473 scoped_ptr<ProblemImpl> problem; 474 double y[4], z[5], w[3]; 475 }; 476 477 TEST(Problem, SetParameterBlockConstantWithUnknownPtrDies) { 478 double x[3]; 479 double y[2]; 480 481 Problem problem; 482 problem.AddParameterBlock(x, 3); 483 484 EXPECT_DEATH_IF_SUPPORTED(problem.SetParameterBlockConstant(y), 485 "Parameter block not found:"); 486 } 487 488 TEST(Problem, SetParameterBlockVariableWithUnknownPtrDies) { 489 double x[3]; 490 double y[2]; 491 492 Problem problem; 493 problem.AddParameterBlock(x, 3); 494 495 EXPECT_DEATH_IF_SUPPORTED(problem.SetParameterBlockVariable(y), 496 "Parameter block not found:"); 497 } 498 499 TEST(Problem, SetLocalParameterizationWithUnknownPtrDies) { 500 double x[3]; 501 double y[2]; 502 503 Problem problem; 504 problem.AddParameterBlock(x, 3); 505 506 EXPECT_DEATH_IF_SUPPORTED( 507 problem.SetParameterization(y, new IdentityParameterization(3)), 508 "Parameter block not found:"); 509 } 510 511 TEST(Problem, RemoveParameterBlockWithUnknownPtrDies) { 512 double x[3]; 513 double y[2]; 514 515 Problem problem; 516 problem.AddParameterBlock(x, 3); 517 518 EXPECT_DEATH_IF_SUPPORTED( 519 problem.RemoveParameterBlock(y), "Parameter block not found:"); 520 } 521 522 TEST(Problem, GetParameterization) { 523 double x[3]; 524 double y[2]; 525 526 Problem problem; 527 problem.AddParameterBlock(x, 3); 528 problem.AddParameterBlock(y, 2); 529 530 LocalParameterization* parameterization = new IdentityParameterization(3); 531 problem.SetParameterization(x, parameterization); 532 EXPECT_EQ(problem.GetParameterization(x), parameterization); 533 EXPECT_TRUE(problem.GetParameterization(y) == NULL); 534 } 535 536 TEST(Problem, ParameterBlockQueryTest) { 537 double x[3]; 538 double y[4]; 539 Problem problem; 540 problem.AddParameterBlock(x, 3); 541 problem.AddParameterBlock(y, 4); 542 543 vector<int> constant_parameters; 544 constant_parameters.push_back(0); 545 problem.SetParameterization( 546 x, 547 new SubsetParameterization(3, constant_parameters)); 548 EXPECT_EQ(problem.ParameterBlockSize(x), 3); 549 EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2); 550 EXPECT_EQ(problem.ParameterBlockLocalSize(y), 4); 551 552 vector<double*> parameter_blocks; 553 problem.GetParameterBlocks(¶meter_blocks); 554 EXPECT_EQ(parameter_blocks.size(), 2); 555 EXPECT_NE(parameter_blocks[0], parameter_blocks[1]); 556 EXPECT_TRUE(parameter_blocks[0] == x || parameter_blocks[0] == y); 557 EXPECT_TRUE(parameter_blocks[1] == x || parameter_blocks[1] == y); 558 559 EXPECT_TRUE(problem.HasParameterBlock(x)); 560 problem.RemoveParameterBlock(x); 561 EXPECT_FALSE(problem.HasParameterBlock(x)); 562 problem.GetParameterBlocks(¶meter_blocks); 563 EXPECT_EQ(parameter_blocks.size(), 1); 564 EXPECT_TRUE(parameter_blocks[0] == y); 565 } 566 567 TEST_P(DynamicProblem, RemoveParameterBlockWithNoResiduals) { 568 problem->AddParameterBlock(y, 4); 569 problem->AddParameterBlock(z, 5); 570 problem->AddParameterBlock(w, 3); 571 ASSERT_EQ(3, problem->NumParameterBlocks()); 572 ASSERT_EQ(0, NumResidualBlocks()); 573 EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 574 EXPECT_EQ(z, GetParameterBlock(1)->user_state()); 575 EXPECT_EQ(w, GetParameterBlock(2)->user_state()); 576 577 // w is at the end, which might break the swapping logic so try adding and 578 // removing it. 579 problem->RemoveParameterBlock(w); 580 ASSERT_EQ(2, problem->NumParameterBlocks()); 581 ASSERT_EQ(0, NumResidualBlocks()); 582 EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 583 EXPECT_EQ(z, GetParameterBlock(1)->user_state()); 584 problem->AddParameterBlock(w, 3); 585 ASSERT_EQ(3, problem->NumParameterBlocks()); 586 ASSERT_EQ(0, NumResidualBlocks()); 587 EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 588 EXPECT_EQ(z, GetParameterBlock(1)->user_state()); 589 EXPECT_EQ(w, GetParameterBlock(2)->user_state()); 590 591 // Now remove z, which is in the middle, and add it back. 592 problem->RemoveParameterBlock(z); 593 ASSERT_EQ(2, problem->NumParameterBlocks()); 594 ASSERT_EQ(0, NumResidualBlocks()); 595 EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 596 EXPECT_EQ(w, GetParameterBlock(1)->user_state()); 597 problem->AddParameterBlock(z, 5); 598 ASSERT_EQ(3, problem->NumParameterBlocks()); 599 ASSERT_EQ(0, NumResidualBlocks()); 600 EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 601 EXPECT_EQ(w, GetParameterBlock(1)->user_state()); 602 EXPECT_EQ(z, GetParameterBlock(2)->user_state()); 603 604 // Now remove everything. 605 // y 606 problem->RemoveParameterBlock(y); 607 ASSERT_EQ(2, problem->NumParameterBlocks()); 608 ASSERT_EQ(0, NumResidualBlocks()); 609 EXPECT_EQ(z, GetParameterBlock(0)->user_state()); 610 EXPECT_EQ(w, GetParameterBlock(1)->user_state()); 611 612 // z 613 problem->RemoveParameterBlock(z); 614 ASSERT_EQ(1, problem->NumParameterBlocks()); 615 ASSERT_EQ(0, NumResidualBlocks()); 616 EXPECT_EQ(w, GetParameterBlock(0)->user_state()); 617 618 // w 619 problem->RemoveParameterBlock(w); 620 EXPECT_EQ(0, problem->NumParameterBlocks()); 621 EXPECT_EQ(0, NumResidualBlocks()); 622 } 623 624 TEST_P(DynamicProblem, RemoveParameterBlockWithResiduals) { 625 problem->AddParameterBlock(y, 4); 626 problem->AddParameterBlock(z, 5); 627 problem->AddParameterBlock(w, 3); 628 ASSERT_EQ(3, problem->NumParameterBlocks()); 629 ASSERT_EQ(0, NumResidualBlocks()); 630 EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 631 EXPECT_EQ(z, GetParameterBlock(1)->user_state()); 632 EXPECT_EQ(w, GetParameterBlock(2)->user_state()); 633 634 // Add all combinations of cost functions. 635 CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3); 636 CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5); 637 CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3); 638 CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3); 639 CostFunction* cost_y = new UnaryCostFunction (1, 4); 640 CostFunction* cost_z = new UnaryCostFunction (1, 5); 641 CostFunction* cost_w = new UnaryCostFunction (1, 3); 642 643 ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w); 644 ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z); 645 ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w); 646 ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w); 647 ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y); 648 ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z); 649 ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); 650 651 EXPECT_EQ(3, problem->NumParameterBlocks()); 652 EXPECT_EQ(7, NumResidualBlocks()); 653 654 // Remove w, which should remove r_yzw, r_yw, r_zw, r_w. 655 problem->RemoveParameterBlock(w); 656 ASSERT_EQ(2, problem->NumParameterBlocks()); 657 ASSERT_EQ(3, NumResidualBlocks()); 658 659 ASSERT_FALSE(HasResidualBlock(r_yzw)); 660 ASSERT_TRUE (HasResidualBlock(r_yz )); 661 ASSERT_FALSE(HasResidualBlock(r_yw )); 662 ASSERT_FALSE(HasResidualBlock(r_zw )); 663 ASSERT_TRUE (HasResidualBlock(r_y )); 664 ASSERT_TRUE (HasResidualBlock(r_z )); 665 ASSERT_FALSE(HasResidualBlock(r_w )); 666 667 // Remove z, which will remove almost everything else. 668 problem->RemoveParameterBlock(z); 669 ASSERT_EQ(1, problem->NumParameterBlocks()); 670 ASSERT_EQ(1, NumResidualBlocks()); 671 672 ASSERT_FALSE(HasResidualBlock(r_yzw)); 673 ASSERT_FALSE(HasResidualBlock(r_yz )); 674 ASSERT_FALSE(HasResidualBlock(r_yw )); 675 ASSERT_FALSE(HasResidualBlock(r_zw )); 676 ASSERT_TRUE (HasResidualBlock(r_y )); 677 ASSERT_FALSE(HasResidualBlock(r_z )); 678 ASSERT_FALSE(HasResidualBlock(r_w )); 679 680 // Remove y; all gone. 681 problem->RemoveParameterBlock(y); 682 EXPECT_EQ(0, problem->NumParameterBlocks()); 683 EXPECT_EQ(0, NumResidualBlocks()); 684 } 685 686 TEST_P(DynamicProblem, RemoveResidualBlock) { 687 problem->AddParameterBlock(y, 4); 688 problem->AddParameterBlock(z, 5); 689 problem->AddParameterBlock(w, 3); 690 691 // Add all combinations of cost functions. 692 CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3); 693 CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5); 694 CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3); 695 CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3); 696 CostFunction* cost_y = new UnaryCostFunction (1, 4); 697 CostFunction* cost_z = new UnaryCostFunction (1, 5); 698 CostFunction* cost_w = new UnaryCostFunction (1, 3); 699 700 ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w); 701 ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z); 702 ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w); 703 ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w); 704 ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y); 705 ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z); 706 ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); 707 708 if (GetParam()) { 709 // In this test parameterization, there should be back-pointers from the 710 // parameter blocks to the residual blocks. 711 ExpectParameterBlockContains(y, r_yzw, r_yz, r_yw, r_y); 712 ExpectParameterBlockContains(z, r_yzw, r_yz, r_zw, r_z); 713 ExpectParameterBlockContains(w, r_yzw, r_yw, r_zw, r_w); 714 } else { 715 // Otherwise, nothing. 716 EXPECT_TRUE(GetParameterBlock(0)->mutable_residual_blocks() == NULL); 717 EXPECT_TRUE(GetParameterBlock(1)->mutable_residual_blocks() == NULL); 718 EXPECT_TRUE(GetParameterBlock(2)->mutable_residual_blocks() == NULL); 719 } 720 EXPECT_EQ(3, problem->NumParameterBlocks()); 721 EXPECT_EQ(7, NumResidualBlocks()); 722 723 // Remove each residual and check the state after each removal. 724 725 // Remove r_yzw. 726 problem->RemoveResidualBlock(r_yzw); 727 ASSERT_EQ(3, problem->NumParameterBlocks()); 728 ASSERT_EQ(6, NumResidualBlocks()); 729 if (GetParam()) { 730 ExpectParameterBlockContains(y, r_yz, r_yw, r_y); 731 ExpectParameterBlockContains(z, r_yz, r_zw, r_z); 732 ExpectParameterBlockContains(w, r_yw, r_zw, r_w); 733 } 734 ASSERT_TRUE (HasResidualBlock(r_yz )); 735 ASSERT_TRUE (HasResidualBlock(r_yw )); 736 ASSERT_TRUE (HasResidualBlock(r_zw )); 737 ASSERT_TRUE (HasResidualBlock(r_y )); 738 ASSERT_TRUE (HasResidualBlock(r_z )); 739 ASSERT_TRUE (HasResidualBlock(r_w )); 740 741 // Remove r_yw. 742 problem->RemoveResidualBlock(r_yw); 743 ASSERT_EQ(3, problem->NumParameterBlocks()); 744 ASSERT_EQ(5, NumResidualBlocks()); 745 if (GetParam()) { 746 ExpectParameterBlockContains(y, r_yz, r_y); 747 ExpectParameterBlockContains(z, r_yz, r_zw, r_z); 748 ExpectParameterBlockContains(w, r_zw, r_w); 749 } 750 ASSERT_TRUE (HasResidualBlock(r_yz )); 751 ASSERT_TRUE (HasResidualBlock(r_zw )); 752 ASSERT_TRUE (HasResidualBlock(r_y )); 753 ASSERT_TRUE (HasResidualBlock(r_z )); 754 ASSERT_TRUE (HasResidualBlock(r_w )); 755 756 // Remove r_zw. 757 problem->RemoveResidualBlock(r_zw); 758 ASSERT_EQ(3, problem->NumParameterBlocks()); 759 ASSERT_EQ(4, NumResidualBlocks()); 760 if (GetParam()) { 761 ExpectParameterBlockContains(y, r_yz, r_y); 762 ExpectParameterBlockContains(z, r_yz, r_z); 763 ExpectParameterBlockContains(w, r_w); 764 } 765 ASSERT_TRUE (HasResidualBlock(r_yz )); 766 ASSERT_TRUE (HasResidualBlock(r_y )); 767 ASSERT_TRUE (HasResidualBlock(r_z )); 768 ASSERT_TRUE (HasResidualBlock(r_w )); 769 770 // Remove r_w. 771 problem->RemoveResidualBlock(r_w); 772 ASSERT_EQ(3, problem->NumParameterBlocks()); 773 ASSERT_EQ(3, NumResidualBlocks()); 774 if (GetParam()) { 775 ExpectParameterBlockContains(y, r_yz, r_y); 776 ExpectParameterBlockContains(z, r_yz, r_z); 777 ExpectParameterBlockContains(w); 778 } 779 ASSERT_TRUE (HasResidualBlock(r_yz )); 780 ASSERT_TRUE (HasResidualBlock(r_y )); 781 ASSERT_TRUE (HasResidualBlock(r_z )); 782 783 // Remove r_yz. 784 problem->RemoveResidualBlock(r_yz); 785 ASSERT_EQ(3, problem->NumParameterBlocks()); 786 ASSERT_EQ(2, NumResidualBlocks()); 787 if (GetParam()) { 788 ExpectParameterBlockContains(y, r_y); 789 ExpectParameterBlockContains(z, r_z); 790 ExpectParameterBlockContains(w); 791 } 792 ASSERT_TRUE (HasResidualBlock(r_y )); 793 ASSERT_TRUE (HasResidualBlock(r_z )); 794 795 // Remove the last two. 796 problem->RemoveResidualBlock(r_z); 797 problem->RemoveResidualBlock(r_y); 798 ASSERT_EQ(3, problem->NumParameterBlocks()); 799 ASSERT_EQ(0, NumResidualBlocks()); 800 if (GetParam()) { 801 ExpectParameterBlockContains(y); 802 ExpectParameterBlockContains(z); 803 ExpectParameterBlockContains(w); 804 } 805 } 806 807 TEST_P(DynamicProblem, RemoveInvalidResidualBlockDies) { 808 problem->AddParameterBlock(y, 4); 809 problem->AddParameterBlock(z, 5); 810 problem->AddParameterBlock(w, 3); 811 812 // Add all combinations of cost functions. 813 CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3); 814 CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5); 815 CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3); 816 CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3); 817 CostFunction* cost_y = new UnaryCostFunction (1, 4); 818 CostFunction* cost_z = new UnaryCostFunction (1, 5); 819 CostFunction* cost_w = new UnaryCostFunction (1, 3); 820 821 ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w); 822 ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z); 823 ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w); 824 ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w); 825 ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y); 826 ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z); 827 ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); 828 829 // Remove r_yzw. 830 problem->RemoveResidualBlock(r_yzw); 831 ASSERT_EQ(3, problem->NumParameterBlocks()); 832 ASSERT_EQ(6, NumResidualBlocks()); 833 // Attempt to remove r_yzw again. 834 EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_yzw), "not found"); 835 836 // Attempt to remove a cast pointer never added as a residual. 837 int trash_memory = 1234; 838 ResidualBlock* invalid_residual = 839 reinterpret_cast<ResidualBlock*>(&trash_memory); 840 EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(invalid_residual), 841 "not found"); 842 843 // Remove a parameter block, which in turn removes the dependent residuals 844 // then attempt to remove them directly. 845 problem->RemoveParameterBlock(z); 846 ASSERT_EQ(2, problem->NumParameterBlocks()); 847 ASSERT_EQ(3, NumResidualBlocks()); 848 EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_yz), "not found"); 849 EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_zw), "not found"); 850 EXPECT_DEATH_IF_SUPPORTED(problem->RemoveResidualBlock(r_z), "not found"); 851 852 problem->RemoveResidualBlock(r_yw); 853 problem->RemoveResidualBlock(r_w); 854 problem->RemoveResidualBlock(r_y); 855 } 856 857 // Check that a null-terminated array, a, has the same elements as b. 858 template<typename T> 859 void ExpectVectorContainsUnordered(const T* a, const vector<T>& b) { 860 // Compute the size of a. 861 int size = 0; 862 while (a[size]) { 863 ++size; 864 } 865 ASSERT_EQ(size, b.size()); 866 867 // Sort a. 868 vector<T> a_sorted(size); 869 copy(a, a + size, a_sorted.begin()); 870 sort(a_sorted.begin(), a_sorted.end()); 871 872 // Sort b. 873 vector<T> b_sorted(b); 874 sort(b_sorted.begin(), b_sorted.end()); 875 876 // Compare. 877 for (int i = 0; i < size; ++i) { 878 EXPECT_EQ(a_sorted[i], b_sorted[i]); 879 } 880 } 881 882 void ExpectProblemHasResidualBlocks( 883 const ProblemImpl &problem, 884 const ResidualBlockId *expected_residual_blocks) { 885 vector<ResidualBlockId> residual_blocks; 886 problem.GetResidualBlocks(&residual_blocks); 887 ExpectVectorContainsUnordered(expected_residual_blocks, residual_blocks); 888 } 889 890 TEST_P(DynamicProblem, GetXXXBlocksForYYYBlock) { 891 problem->AddParameterBlock(y, 4); 892 problem->AddParameterBlock(z, 5); 893 problem->AddParameterBlock(w, 3); 894 895 // Add all combinations of cost functions. 896 CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3); 897 CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5); 898 CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3); 899 CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3); 900 CostFunction* cost_y = new UnaryCostFunction (1, 4); 901 CostFunction* cost_z = new UnaryCostFunction (1, 5); 902 CostFunction* cost_w = new UnaryCostFunction (1, 3); 903 904 ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w); 905 { 906 ResidualBlockId expected_residuals[] = {r_yzw, 0}; 907 ExpectProblemHasResidualBlocks(*problem, expected_residuals); 908 } 909 ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z); 910 { 911 ResidualBlockId expected_residuals[] = {r_yzw, r_yz, 0}; 912 ExpectProblemHasResidualBlocks(*problem, expected_residuals); 913 } 914 ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w); 915 { 916 ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, 0}; 917 ExpectProblemHasResidualBlocks(*problem, expected_residuals); 918 } 919 ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w); 920 { 921 ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, r_zw, 0}; 922 ExpectProblemHasResidualBlocks(*problem, expected_residuals); 923 } 924 ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y); 925 { 926 ResidualBlock *expected_residuals[] = {r_yzw, r_yz, r_yw, r_zw, r_y, 0}; 927 ExpectProblemHasResidualBlocks(*problem, expected_residuals); 928 } 929 ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z); 930 { 931 ResidualBlock *expected_residuals[] = { 932 r_yzw, r_yz, r_yw, r_zw, r_y, r_z, 0 933 }; 934 ExpectProblemHasResidualBlocks(*problem, expected_residuals); 935 } 936 ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); 937 { 938 ResidualBlock *expected_residuals[] = { 939 r_yzw, r_yz, r_yw, r_zw, r_y, r_z, r_w, 0 940 }; 941 ExpectProblemHasResidualBlocks(*problem, expected_residuals); 942 } 943 944 vector<double*> parameter_blocks; 945 vector<ResidualBlockId> residual_blocks; 946 947 // Check GetResidualBlocksForParameterBlock() for all parameter blocks. 948 struct GetResidualBlocksForParameterBlockTestCase { 949 double* parameter_block; 950 ResidualBlockId expected_residual_blocks[10]; 951 }; 952 GetResidualBlocksForParameterBlockTestCase get_residual_blocks_cases[] = { 953 { y, { r_yzw, r_yz, r_yw, r_y, NULL} }, 954 { z, { r_yzw, r_yz, r_zw, r_z, NULL} }, 955 { w, { r_yzw, r_yw, r_zw, r_w, NULL} }, 956 { NULL } 957 }; 958 for (int i = 0; get_residual_blocks_cases[i].parameter_block; ++i) { 959 problem->GetResidualBlocksForParameterBlock( 960 get_residual_blocks_cases[i].parameter_block, 961 &residual_blocks); 962 ExpectVectorContainsUnordered( 963 get_residual_blocks_cases[i].expected_residual_blocks, 964 residual_blocks); 965 } 966 967 // Check GetParameterBlocksForResidualBlock() for all residual blocks. 968 struct GetParameterBlocksForResidualBlockTestCase { 969 ResidualBlockId residual_block; 970 double* expected_parameter_blocks[10]; 971 }; 972 GetParameterBlocksForResidualBlockTestCase get_parameter_blocks_cases[] = { 973 { r_yzw, { y, z, w, NULL } }, 974 { r_yz , { y, z, NULL } }, 975 { r_yw , { y, w, NULL } }, 976 { r_zw , { z, w, NULL } }, 977 { r_y , { y, NULL } }, 978 { r_z , { z, NULL } }, 979 { r_w , { w, NULL } }, 980 { NULL } 981 }; 982 for (int i = 0; get_parameter_blocks_cases[i].residual_block; ++i) { 983 problem->GetParameterBlocksForResidualBlock( 984 get_parameter_blocks_cases[i].residual_block, 985 ¶meter_blocks); 986 ExpectVectorContainsUnordered( 987 get_parameter_blocks_cases[i].expected_parameter_blocks, 988 parameter_blocks); 989 } 990 } 991 992 INSTANTIATE_TEST_CASE_P(OptionsInstantiation, 993 DynamicProblem, 994 ::testing::Values(true, false)); 995 996 // Test for Problem::Evaluate 997 998 // r_i = i - (j + 1) * x_ij^2 999 template <int kNumResiduals, int kNumParameterBlocks> 1000 class QuadraticCostFunction : public CostFunction { 1001 public: 1002 QuadraticCostFunction() { 1003 CHECK_GT(kNumResiduals, 0); 1004 CHECK_GT(kNumParameterBlocks, 0); 1005 set_num_residuals(kNumResiduals); 1006 for (int i = 0; i < kNumParameterBlocks; ++i) { 1007 mutable_parameter_block_sizes()->push_back(kNumResiduals); 1008 } 1009 } 1010 1011 virtual bool Evaluate(double const* const* parameters, 1012 double* residuals, 1013 double** jacobians) const { 1014 for (int i = 0; i < kNumResiduals; ++i) { 1015 residuals[i] = i; 1016 for (int j = 0; j < kNumParameterBlocks; ++j) { 1017 residuals[i] -= (j + 1.0) * parameters[j][i] * parameters[j][i]; 1018 } 1019 } 1020 1021 if (jacobians == NULL) { 1022 return true; 1023 } 1024 1025 for (int j = 0; j < kNumParameterBlocks; ++j) { 1026 if (jacobians[j] != NULL) { 1027 MatrixRef(jacobians[j], kNumResiduals, kNumResiduals) = 1028 (-2.0 * (j + 1.0) * 1029 ConstVectorRef(parameters[j], kNumResiduals)).asDiagonal(); 1030 } 1031 } 1032 1033 return true; 1034 } 1035 }; 1036 1037 // Convert a CRSMatrix to a dense Eigen matrix. 1038 void CRSToDenseMatrix(const CRSMatrix& input, Matrix* output) { 1039 Matrix& m = *CHECK_NOTNULL(output); 1040 m.resize(input.num_rows, input.num_cols); 1041 m.setZero(); 1042 for (int row = 0; row < input.num_rows; ++row) { 1043 for (int j = input.rows[row]; j < input.rows[row + 1]; ++j) { 1044 const int col = input.cols[j]; 1045 m(row, col) = input.values[j]; 1046 } 1047 } 1048 } 1049 1050 class ProblemEvaluateTest : public ::testing::Test { 1051 protected: 1052 void SetUp() { 1053 for (int i = 0; i < 6; ++i) { 1054 parameters_[i] = static_cast<double>(i + 1); 1055 } 1056 1057 parameter_blocks_.push_back(parameters_); 1058 parameter_blocks_.push_back(parameters_ + 2); 1059 parameter_blocks_.push_back(parameters_ + 4); 1060 1061 1062 CostFunction* cost_function = new QuadraticCostFunction<2, 2>; 1063 1064 // f(x, y) 1065 residual_blocks_.push_back( 1066 problem_.AddResidualBlock(cost_function, 1067 NULL, 1068 parameters_, 1069 parameters_ + 2)); 1070 // g(y, z) 1071 residual_blocks_.push_back( 1072 problem_.AddResidualBlock(cost_function, 1073 NULL, parameters_ + 2, 1074 parameters_ + 4)); 1075 // h(z, x) 1076 residual_blocks_.push_back( 1077 problem_.AddResidualBlock(cost_function, 1078 NULL, 1079 parameters_ + 4, 1080 parameters_)); 1081 } 1082 1083 void TearDown() { 1084 EXPECT_TRUE(problem_.program().IsValid()); 1085 } 1086 1087 void EvaluateAndCompare(const Problem::EvaluateOptions& options, 1088 const int expected_num_rows, 1089 const int expected_num_cols, 1090 const double expected_cost, 1091 const double* expected_residuals, 1092 const double* expected_gradient, 1093 const double* expected_jacobian) { 1094 double cost; 1095 vector<double> residuals; 1096 vector<double> gradient; 1097 CRSMatrix jacobian; 1098 1099 EXPECT_TRUE( 1100 problem_.Evaluate(options, 1101 &cost, 1102 expected_residuals != NULL ? &residuals : NULL, 1103 expected_gradient != NULL ? &gradient : NULL, 1104 expected_jacobian != NULL ? &jacobian : NULL)); 1105 1106 if (expected_residuals != NULL) { 1107 EXPECT_EQ(residuals.size(), expected_num_rows); 1108 } 1109 1110 if (expected_gradient != NULL) { 1111 EXPECT_EQ(gradient.size(), expected_num_cols); 1112 } 1113 1114 if (expected_jacobian != NULL) { 1115 EXPECT_EQ(jacobian.num_rows, expected_num_rows); 1116 EXPECT_EQ(jacobian.num_cols, expected_num_cols); 1117 } 1118 1119 Matrix dense_jacobian; 1120 if (expected_jacobian != NULL) { 1121 CRSToDenseMatrix(jacobian, &dense_jacobian); 1122 } 1123 1124 CompareEvaluations(expected_num_rows, 1125 expected_num_cols, 1126 expected_cost, 1127 expected_residuals, 1128 expected_gradient, 1129 expected_jacobian, 1130 cost, 1131 residuals.size() > 0 ? &residuals[0] : NULL, 1132 gradient.size() > 0 ? &gradient[0] : NULL, 1133 dense_jacobian.data()); 1134 } 1135 1136 void CheckAllEvaluationCombinations(const Problem::EvaluateOptions& options, 1137 const ExpectedEvaluation& expected) { 1138 for (int i = 0; i < 8; ++i) { 1139 EvaluateAndCompare(options, 1140 expected.num_rows, 1141 expected.num_cols, 1142 expected.cost, 1143 (i & 1) ? expected.residuals : NULL, 1144 (i & 2) ? expected.gradient : NULL, 1145 (i & 4) ? expected.jacobian : NULL); 1146 } 1147 } 1148 1149 ProblemImpl problem_; 1150 double parameters_[6]; 1151 vector<double*> parameter_blocks_; 1152 vector<ResidualBlockId> residual_blocks_; 1153 }; 1154 1155 1156 TEST_F(ProblemEvaluateTest, MultipleParameterAndResidualBlocks) { 1157 ExpectedEvaluation expected = { 1158 // Rows/columns 1159 6, 6, 1160 // Cost 1161 7607.0, 1162 // Residuals 1163 { -19.0, -35.0, // f 1164 -59.0, -87.0, // g 1165 -27.0, -43.0 // h 1166 }, 1167 // Gradient 1168 { 146.0, 484.0, // x 1169 582.0, 1256.0, // y 1170 1450.0, 2604.0, // z 1171 }, 1172 // Jacobian 1173 // x y z 1174 { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0, 1175 0.0, -4.0, 0.0, -16.0, 0.0, 0.0, 1176 /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0, 1177 0.0, 0.0, 0.0, -8.0, 0.0, -24.0, 1178 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 1179 0.0, -8.0, 0.0, 0.0, 0.0, -12.0 1180 } 1181 }; 1182 1183 CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected); 1184 } 1185 1186 TEST_F(ProblemEvaluateTest, ParameterAndResidualBlocksPassedInOptions) { 1187 ExpectedEvaluation expected = { 1188 // Rows/columns 1189 6, 6, 1190 // Cost 1191 7607.0, 1192 // Residuals 1193 { -19.0, -35.0, // f 1194 -59.0, -87.0, // g 1195 -27.0, -43.0 // h 1196 }, 1197 // Gradient 1198 { 146.0, 484.0, // x 1199 582.0, 1256.0, // y 1200 1450.0, 2604.0, // z 1201 }, 1202 // Jacobian 1203 // x y z 1204 { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0, 1205 0.0, -4.0, 0.0, -16.0, 0.0, 0.0, 1206 /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0, 1207 0.0, 0.0, 0.0, -8.0, 0.0, -24.0, 1208 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 1209 0.0, -8.0, 0.0, 0.0, 0.0, -12.0 1210 } 1211 }; 1212 1213 Problem::EvaluateOptions evaluate_options; 1214 evaluate_options.parameter_blocks = parameter_blocks_; 1215 evaluate_options.residual_blocks = residual_blocks_; 1216 CheckAllEvaluationCombinations(evaluate_options, expected); 1217 } 1218 1219 TEST_F(ProblemEvaluateTest, ReorderedResidualBlocks) { 1220 ExpectedEvaluation expected = { 1221 // Rows/columns 1222 6, 6, 1223 // Cost 1224 7607.0, 1225 // Residuals 1226 { -19.0, -35.0, // f 1227 -27.0, -43.0, // h 1228 -59.0, -87.0 // g 1229 }, 1230 // Gradient 1231 { 146.0, 484.0, // x 1232 582.0, 1256.0, // y 1233 1450.0, 2604.0, // z 1234 }, 1235 // Jacobian 1236 // x y z 1237 { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0, 1238 0.0, -4.0, 0.0, -16.0, 0.0, 0.0, 1239 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 1240 0.0, -8.0, 0.0, 0.0, 0.0, -12.0, 1241 /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0, 1242 0.0, 0.0, 0.0, -8.0, 0.0, -24.0 1243 } 1244 }; 1245 1246 Problem::EvaluateOptions evaluate_options; 1247 evaluate_options.parameter_blocks = parameter_blocks_; 1248 1249 // f, h, g 1250 evaluate_options.residual_blocks.push_back(residual_blocks_[0]); 1251 evaluate_options.residual_blocks.push_back(residual_blocks_[2]); 1252 evaluate_options.residual_blocks.push_back(residual_blocks_[1]); 1253 1254 CheckAllEvaluationCombinations(evaluate_options, expected); 1255 } 1256 1257 TEST_F(ProblemEvaluateTest, ReorderedResidualBlocksAndReorderedParameterBlocks) { 1258 ExpectedEvaluation expected = { 1259 // Rows/columns 1260 6, 6, 1261 // Cost 1262 7607.0, 1263 // Residuals 1264 { -19.0, -35.0, // f 1265 -27.0, -43.0, // h 1266 -59.0, -87.0 // g 1267 }, 1268 // Gradient 1269 { 1450.0, 2604.0, // z 1270 582.0, 1256.0, // y 1271 146.0, 484.0, // x 1272 }, 1273 // Jacobian 1274 // z y x 1275 { /* f(x, y) */ 0.0, 0.0, -12.0, 0.0, -2.0, 0.0, 1276 0.0, 0.0, 0.0, -16.0, 0.0, -4.0, 1277 /* h(z, x) */ -10.0, 0.0, 0.0, 0.0, -4.0, 0.0, 1278 0.0, -12.0, 0.0, 0.0, 0.0, -8.0, 1279 /* g(y, z) */ -20.0, 0.0, -6.0, 0.0, 0.0, 0.0, 1280 0.0, -24.0, 0.0, -8.0, 0.0, 0.0 1281 } 1282 }; 1283 1284 Problem::EvaluateOptions evaluate_options; 1285 // z, y, x 1286 evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]); 1287 evaluate_options.parameter_blocks.push_back(parameter_blocks_[1]); 1288 evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]); 1289 1290 // f, h, g 1291 evaluate_options.residual_blocks.push_back(residual_blocks_[0]); 1292 evaluate_options.residual_blocks.push_back(residual_blocks_[2]); 1293 evaluate_options.residual_blocks.push_back(residual_blocks_[1]); 1294 1295 CheckAllEvaluationCombinations(evaluate_options, expected); 1296 } 1297 1298 TEST_F(ProblemEvaluateTest, ConstantParameterBlock) { 1299 ExpectedEvaluation expected = { 1300 // Rows/columns 1301 6, 6, 1302 // Cost 1303 7607.0, 1304 // Residuals 1305 { -19.0, -35.0, // f 1306 -59.0, -87.0, // g 1307 -27.0, -43.0 // h 1308 }, 1309 1310 // Gradient 1311 { 146.0, 484.0, // x 1312 0.0, 0.0, // y 1313 1450.0, 2604.0, // z 1314 }, 1315 1316 // Jacobian 1317 // x y z 1318 { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1319 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 1320 /* g(y, z) */ 0.0, 0.0, 0.0, 0.0, -20.0, 0.0, 1321 0.0, 0.0, 0.0, 0.0, 0.0, -24.0, 1322 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 1323 0.0, -8.0, 0.0, 0.0, 0.0, -12.0 1324 } 1325 }; 1326 1327 problem_.SetParameterBlockConstant(parameters_ + 2); 1328 CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected); 1329 } 1330 1331 TEST_F(ProblemEvaluateTest, ExcludedAResidualBlock) { 1332 ExpectedEvaluation expected = { 1333 // Rows/columns 1334 4, 6, 1335 // Cost 1336 2082.0, 1337 // Residuals 1338 { -19.0, -35.0, // f 1339 -27.0, -43.0 // h 1340 }, 1341 // Gradient 1342 { 146.0, 484.0, // x 1343 228.0, 560.0, // y 1344 270.0, 516.0, // z 1345 }, 1346 // Jacobian 1347 // x y z 1348 { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0, 1349 0.0, -4.0, 0.0, -16.0, 0.0, 0.0, 1350 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 1351 0.0, -8.0, 0.0, 0.0, 0.0, -12.0 1352 } 1353 }; 1354 1355 Problem::EvaluateOptions evaluate_options; 1356 evaluate_options.residual_blocks.push_back(residual_blocks_[0]); 1357 evaluate_options.residual_blocks.push_back(residual_blocks_[2]); 1358 1359 CheckAllEvaluationCombinations(evaluate_options, expected); 1360 } 1361 1362 TEST_F(ProblemEvaluateTest, ExcludedParameterBlock) { 1363 ExpectedEvaluation expected = { 1364 // Rows/columns 1365 6, 4, 1366 // Cost 1367 7607.0, 1368 // Residuals 1369 { -19.0, -35.0, // f 1370 -59.0, -87.0, // g 1371 -27.0, -43.0 // h 1372 }, 1373 1374 // Gradient 1375 { 146.0, 484.0, // x 1376 1450.0, 2604.0, // z 1377 }, 1378 1379 // Jacobian 1380 // x z 1381 { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 1382 0.0, -4.0, 0.0, 0.0, 1383 /* g(y, z) */ 0.0, 0.0, -20.0, 0.0, 1384 0.0, 0.0, 0.0, -24.0, 1385 /* h(z, x) */ -4.0, 0.0, -10.0, 0.0, 1386 0.0, -8.0, 0.0, -12.0 1387 } 1388 }; 1389 1390 Problem::EvaluateOptions evaluate_options; 1391 // x, z 1392 evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]); 1393 evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]); 1394 evaluate_options.residual_blocks = residual_blocks_; 1395 CheckAllEvaluationCombinations(evaluate_options, expected); 1396 } 1397 1398 TEST_F(ProblemEvaluateTest, ExcludedParameterBlockAndExcludedResidualBlock) { 1399 ExpectedEvaluation expected = { 1400 // Rows/columns 1401 4, 4, 1402 // Cost 1403 6318.0, 1404 // Residuals 1405 { -19.0, -35.0, // f 1406 -59.0, -87.0, // g 1407 }, 1408 1409 // Gradient 1410 { 38.0, 140.0, // x 1411 1180.0, 2088.0, // z 1412 }, 1413 1414 // Jacobian 1415 // x z 1416 { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 1417 0.0, -4.0, 0.0, 0.0, 1418 /* g(y, z) */ 0.0, 0.0, -20.0, 0.0, 1419 0.0, 0.0, 0.0, -24.0, 1420 } 1421 }; 1422 1423 Problem::EvaluateOptions evaluate_options; 1424 // x, z 1425 evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]); 1426 evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]); 1427 evaluate_options.residual_blocks.push_back(residual_blocks_[0]); 1428 evaluate_options.residual_blocks.push_back(residual_blocks_[1]); 1429 1430 CheckAllEvaluationCombinations(evaluate_options, expected); 1431 } 1432 1433 TEST_F(ProblemEvaluateTest, LocalParameterization) { 1434 ExpectedEvaluation expected = { 1435 // Rows/columns 1436 6, 5, 1437 // Cost 1438 7607.0, 1439 // Residuals 1440 { -19.0, -35.0, // f 1441 -59.0, -87.0, // g 1442 -27.0, -43.0 // h 1443 }, 1444 // Gradient 1445 { 146.0, 484.0, // x 1446 1256.0, // y with SubsetParameterization 1447 1450.0, 2604.0, // z 1448 }, 1449 // Jacobian 1450 // x y z 1451 { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 0.0, 1452 0.0, -4.0, -16.0, 0.0, 0.0, 1453 /* g(y, z) */ 0.0, 0.0, 0.0, -20.0, 0.0, 1454 0.0, 0.0, -8.0, 0.0, -24.0, 1455 /* h(z, x) */ -4.0, 0.0, 0.0, -10.0, 0.0, 1456 0.0, -8.0, 0.0, 0.0, -12.0 1457 } 1458 }; 1459 1460 vector<int> constant_parameters; 1461 constant_parameters.push_back(0); 1462 problem_.SetParameterization(parameters_ + 2, 1463 new SubsetParameterization(2, 1464 constant_parameters)); 1465 1466 CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected); 1467 } 1468 1469 } // namespace internal 1470 } // namespace ceres 1471
