1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 3 // http://code.google.com/p/ceres-solver/ 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are met: 7 // 8 // * Redistributions of source code must retain the above copyright notice, 9 // this list of conditions and the following disclaimer. 10 // * Redistributions in binary form must reproduce the above copyright notice, 11 // this list of conditions and the following disclaimer in the documentation 12 // and/or other materials provided with the distribution. 13 // * Neither the name of Google Inc. nor the names of its contributors may be 14 // used to endorse or promote products derived from this software without 15 // specific prior written permission. 16 // 17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 // POSSIBILITY OF SUCH DAMAGE. 28 // 29 // Author: 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, int16 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 int16 parameter_block1_size, 80 int16 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 int16 parameter_block1_size, 101 int16 parameter_block2_size, 102 int16 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_parameter_block_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 return find(problem->program().residual_blocks().begin(), 394 problem->program().residual_blocks().end(), 395 residual_block) != problem->program().residual_blocks().end(); 396 } 397 398 // The next block of functions until the end are only for testing the 399 // residual block removals. 400 void ExpectParameterBlockContainsResidualBlock( 401 double* values, 402 ResidualBlock* residual_block) { 403 ParameterBlock* parameter_block = 404 FindOrDie(problem->parameter_map(), values); 405 EXPECT_TRUE(ContainsKey(*(parameter_block->mutable_residual_blocks()), 406 residual_block)); 407 } 408 409 void ExpectSize(double* values, int size) { 410 ParameterBlock* parameter_block = 411 FindOrDie(problem->parameter_map(), values); 412 EXPECT_EQ(size, parameter_block->mutable_residual_blocks()->size()); 413 } 414 415 // Degenerate case. 416 void ExpectParameterBlockContains(double* values) { 417 ExpectSize(values, 0); 418 } 419 420 void ExpectParameterBlockContains(double* values, 421 ResidualBlock* r1) { 422 ExpectSize(values, 1); 423 ExpectParameterBlockContainsResidualBlock(values, r1); 424 } 425 426 void ExpectParameterBlockContains(double* values, 427 ResidualBlock* r1, 428 ResidualBlock* r2) { 429 ExpectSize(values, 2); 430 ExpectParameterBlockContainsResidualBlock(values, r1); 431 ExpectParameterBlockContainsResidualBlock(values, r2); 432 } 433 434 void ExpectParameterBlockContains(double* values, 435 ResidualBlock* r1, 436 ResidualBlock* r2, 437 ResidualBlock* r3) { 438 ExpectSize(values, 3); 439 ExpectParameterBlockContainsResidualBlock(values, r1); 440 ExpectParameterBlockContainsResidualBlock(values, r2); 441 ExpectParameterBlockContainsResidualBlock(values, r3); 442 } 443 444 void ExpectParameterBlockContains(double* values, 445 ResidualBlock* r1, 446 ResidualBlock* r2, 447 ResidualBlock* r3, 448 ResidualBlock* r4) { 449 ExpectSize(values, 4); 450 ExpectParameterBlockContainsResidualBlock(values, r1); 451 ExpectParameterBlockContainsResidualBlock(values, r2); 452 ExpectParameterBlockContainsResidualBlock(values, r3); 453 ExpectParameterBlockContainsResidualBlock(values, r4); 454 } 455 456 scoped_ptr<ProblemImpl> problem; 457 double y[4], z[5], w[3]; 458 }; 459 460 TEST(Problem, SetParameterBlockConstantWithUnknownPtrDies) { 461 double x[3]; 462 double y[2]; 463 464 Problem problem; 465 problem.AddParameterBlock(x, 3); 466 467 EXPECT_DEATH_IF_SUPPORTED(problem.SetParameterBlockConstant(y), 468 "Parameter block not found:"); 469 } 470 471 TEST(Problem, SetParameterBlockVariableWithUnknownPtrDies) { 472 double x[3]; 473 double y[2]; 474 475 Problem problem; 476 problem.AddParameterBlock(x, 3); 477 478 EXPECT_DEATH_IF_SUPPORTED(problem.SetParameterBlockVariable(y), 479 "Parameter block not found:"); 480 } 481 482 TEST(Problem, SetLocalParameterizationWithUnknownPtrDies) { 483 double x[3]; 484 double y[2]; 485 486 Problem problem; 487 problem.AddParameterBlock(x, 3); 488 489 EXPECT_DEATH_IF_SUPPORTED( 490 problem.SetParameterization(y, new IdentityParameterization(3)), 491 "Parameter block not found:"); 492 } 493 494 TEST(Problem, RemoveParameterBlockWithUnknownPtrDies) { 495 double x[3]; 496 double y[2]; 497 498 Problem problem; 499 problem.AddParameterBlock(x, 3); 500 501 EXPECT_DEATH_IF_SUPPORTED( 502 problem.RemoveParameterBlock(y), "Parameter block not found:"); 503 } 504 505 TEST(Problem, ParameterBlockQueryTest) { 506 double x[3]; 507 double y[4]; 508 Problem problem; 509 problem.AddParameterBlock(x, 3); 510 problem.AddParameterBlock(y, 4); 511 512 vector<int> constant_parameters; 513 constant_parameters.push_back(0); 514 problem.SetParameterization( 515 x, 516 new SubsetParameterization(3, constant_parameters)); 517 EXPECT_EQ(problem.ParameterBlockSize(x), 3); 518 EXPECT_EQ(problem.ParameterBlockLocalSize(x), 2); 519 EXPECT_EQ(problem.ParameterBlockLocalSize(y), 4); 520 521 vector<double*> parameter_blocks; 522 problem.GetParameterBlocks(¶meter_blocks); 523 EXPECT_EQ(parameter_blocks.size(), 2); 524 EXPECT_NE(parameter_blocks[0], parameter_blocks[1]); 525 EXPECT_TRUE(parameter_blocks[0] == x || parameter_blocks[0] == y); 526 EXPECT_TRUE(parameter_blocks[1] == x || parameter_blocks[1] == y); 527 528 problem.RemoveParameterBlock(x); 529 problem.GetParameterBlocks(¶meter_blocks); 530 EXPECT_EQ(parameter_blocks.size(), 1); 531 EXPECT_TRUE(parameter_blocks[0] == y); 532 } 533 534 TEST_P(DynamicProblem, RemoveParameterBlockWithNoResiduals) { 535 problem->AddParameterBlock(y, 4); 536 problem->AddParameterBlock(z, 5); 537 problem->AddParameterBlock(w, 3); 538 ASSERT_EQ(3, problem->NumParameterBlocks()); 539 ASSERT_EQ(0, problem->NumResidualBlocks()); 540 EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 541 EXPECT_EQ(z, GetParameterBlock(1)->user_state()); 542 EXPECT_EQ(w, GetParameterBlock(2)->user_state()); 543 544 // w is at the end, which might break the swapping logic so try adding and 545 // removing it. 546 problem->RemoveParameterBlock(w); 547 ASSERT_EQ(2, problem->NumParameterBlocks()); 548 ASSERT_EQ(0, problem->NumResidualBlocks()); 549 EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 550 EXPECT_EQ(z, GetParameterBlock(1)->user_state()); 551 problem->AddParameterBlock(w, 3); 552 ASSERT_EQ(3, problem->NumParameterBlocks()); 553 ASSERT_EQ(0, problem->NumResidualBlocks()); 554 EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 555 EXPECT_EQ(z, GetParameterBlock(1)->user_state()); 556 EXPECT_EQ(w, GetParameterBlock(2)->user_state()); 557 558 // Now remove z, which is in the middle, and add it back. 559 problem->RemoveParameterBlock(z); 560 ASSERT_EQ(2, problem->NumParameterBlocks()); 561 ASSERT_EQ(0, problem->NumResidualBlocks()); 562 EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 563 EXPECT_EQ(w, GetParameterBlock(1)->user_state()); 564 problem->AddParameterBlock(z, 5); 565 ASSERT_EQ(3, problem->NumParameterBlocks()); 566 ASSERT_EQ(0, problem->NumResidualBlocks()); 567 EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 568 EXPECT_EQ(w, GetParameterBlock(1)->user_state()); 569 EXPECT_EQ(z, GetParameterBlock(2)->user_state()); 570 571 // Now remove everything. 572 // y 573 problem->RemoveParameterBlock(y); 574 ASSERT_EQ(2, problem->NumParameterBlocks()); 575 ASSERT_EQ(0, problem->NumResidualBlocks()); 576 EXPECT_EQ(z, GetParameterBlock(0)->user_state()); 577 EXPECT_EQ(w, GetParameterBlock(1)->user_state()); 578 579 // z 580 problem->RemoveParameterBlock(z); 581 ASSERT_EQ(1, problem->NumParameterBlocks()); 582 ASSERT_EQ(0, problem->NumResidualBlocks()); 583 EXPECT_EQ(w, GetParameterBlock(0)->user_state()); 584 585 // w 586 problem->RemoveParameterBlock(w); 587 EXPECT_EQ(0, problem->NumParameterBlocks()); 588 EXPECT_EQ(0, problem->NumResidualBlocks()); 589 } 590 591 TEST_P(DynamicProblem, RemoveParameterBlockWithResiduals) { 592 problem->AddParameterBlock(y, 4); 593 problem->AddParameterBlock(z, 5); 594 problem->AddParameterBlock(w, 3); 595 ASSERT_EQ(3, problem->NumParameterBlocks()); 596 ASSERT_EQ(0, problem->NumResidualBlocks()); 597 EXPECT_EQ(y, GetParameterBlock(0)->user_state()); 598 EXPECT_EQ(z, GetParameterBlock(1)->user_state()); 599 EXPECT_EQ(w, GetParameterBlock(2)->user_state()); 600 601 // Add all combinations of cost functions. 602 CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3); 603 CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5); 604 CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3); 605 CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3); 606 CostFunction* cost_y = new UnaryCostFunction (1, 4); 607 CostFunction* cost_z = new UnaryCostFunction (1, 5); 608 CostFunction* cost_w = new UnaryCostFunction (1, 3); 609 610 ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w); 611 ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z); 612 ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w); 613 ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w); 614 ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y); 615 ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z); 616 ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); 617 618 EXPECT_EQ(3, problem->NumParameterBlocks()); 619 EXPECT_EQ(7, problem->NumResidualBlocks()); 620 621 // Remove w, which should remove r_yzw, r_yw, r_zw, r_w. 622 problem->RemoveParameterBlock(w); 623 ASSERT_EQ(2, problem->NumParameterBlocks()); 624 ASSERT_EQ(3, problem->NumResidualBlocks()); 625 626 ASSERT_FALSE(HasResidualBlock(r_yzw)); 627 ASSERT_TRUE (HasResidualBlock(r_yz )); 628 ASSERT_FALSE(HasResidualBlock(r_yw )); 629 ASSERT_FALSE(HasResidualBlock(r_zw )); 630 ASSERT_TRUE (HasResidualBlock(r_y )); 631 ASSERT_TRUE (HasResidualBlock(r_z )); 632 ASSERT_FALSE(HasResidualBlock(r_w )); 633 634 // Remove z, which will remove almost everything else. 635 problem->RemoveParameterBlock(z); 636 ASSERT_EQ(1, problem->NumParameterBlocks()); 637 ASSERT_EQ(1, problem->NumResidualBlocks()); 638 639 ASSERT_FALSE(HasResidualBlock(r_yzw)); 640 ASSERT_FALSE(HasResidualBlock(r_yz )); 641 ASSERT_FALSE(HasResidualBlock(r_yw )); 642 ASSERT_FALSE(HasResidualBlock(r_zw )); 643 ASSERT_TRUE (HasResidualBlock(r_y )); 644 ASSERT_FALSE(HasResidualBlock(r_z )); 645 ASSERT_FALSE(HasResidualBlock(r_w )); 646 647 // Remove y; all gone. 648 problem->RemoveParameterBlock(y); 649 EXPECT_EQ(0, problem->NumParameterBlocks()); 650 EXPECT_EQ(0, problem->NumResidualBlocks()); 651 } 652 653 TEST_P(DynamicProblem, RemoveResidualBlock) { 654 problem->AddParameterBlock(y, 4); 655 problem->AddParameterBlock(z, 5); 656 problem->AddParameterBlock(w, 3); 657 658 // Add all combinations of cost functions. 659 CostFunction* cost_yzw = new TernaryCostFunction(1, 4, 5, 3); 660 CostFunction* cost_yz = new BinaryCostFunction (1, 4, 5); 661 CostFunction* cost_yw = new BinaryCostFunction (1, 4, 3); 662 CostFunction* cost_zw = new BinaryCostFunction (1, 5, 3); 663 CostFunction* cost_y = new UnaryCostFunction (1, 4); 664 CostFunction* cost_z = new UnaryCostFunction (1, 5); 665 CostFunction* cost_w = new UnaryCostFunction (1, 3); 666 667 ResidualBlock* r_yzw = problem->AddResidualBlock(cost_yzw, NULL, y, z, w); 668 ResidualBlock* r_yz = problem->AddResidualBlock(cost_yz, NULL, y, z); 669 ResidualBlock* r_yw = problem->AddResidualBlock(cost_yw, NULL, y, w); 670 ResidualBlock* r_zw = problem->AddResidualBlock(cost_zw, NULL, z, w); 671 ResidualBlock* r_y = problem->AddResidualBlock(cost_y, NULL, y); 672 ResidualBlock* r_z = problem->AddResidualBlock(cost_z, NULL, z); 673 ResidualBlock* r_w = problem->AddResidualBlock(cost_w, NULL, w); 674 675 if (GetParam()) { 676 // In this test parameterization, there should be back-pointers from the 677 // parameter blocks to the residual blocks. 678 ExpectParameterBlockContains(y, r_yzw, r_yz, r_yw, r_y); 679 ExpectParameterBlockContains(z, r_yzw, r_yz, r_zw, r_z); 680 ExpectParameterBlockContains(w, r_yzw, r_yw, r_zw, r_w); 681 } else { 682 // Otherwise, nothing. 683 EXPECT_TRUE(GetParameterBlock(0)->mutable_residual_blocks() == NULL); 684 EXPECT_TRUE(GetParameterBlock(1)->mutable_residual_blocks() == NULL); 685 EXPECT_TRUE(GetParameterBlock(2)->mutable_residual_blocks() == NULL); 686 } 687 EXPECT_EQ(3, problem->NumParameterBlocks()); 688 EXPECT_EQ(7, problem->NumResidualBlocks()); 689 690 // Remove each residual and check the state after each removal. 691 692 // Remove r_yzw. 693 problem->RemoveResidualBlock(r_yzw); 694 ASSERT_EQ(3, problem->NumParameterBlocks()); 695 ASSERT_EQ(6, problem->NumResidualBlocks()); 696 if (GetParam()) { 697 ExpectParameterBlockContains(y, r_yz, r_yw, r_y); 698 ExpectParameterBlockContains(z, r_yz, r_zw, r_z); 699 ExpectParameterBlockContains(w, r_yw, r_zw, r_w); 700 } 701 ASSERT_TRUE (HasResidualBlock(r_yz )); 702 ASSERT_TRUE (HasResidualBlock(r_yw )); 703 ASSERT_TRUE (HasResidualBlock(r_zw )); 704 ASSERT_TRUE (HasResidualBlock(r_y )); 705 ASSERT_TRUE (HasResidualBlock(r_z )); 706 ASSERT_TRUE (HasResidualBlock(r_w )); 707 708 // Remove r_yw. 709 problem->RemoveResidualBlock(r_yw); 710 ASSERT_EQ(3, problem->NumParameterBlocks()); 711 ASSERT_EQ(5, problem->NumResidualBlocks()); 712 if (GetParam()) { 713 ExpectParameterBlockContains(y, r_yz, r_y); 714 ExpectParameterBlockContains(z, r_yz, r_zw, r_z); 715 ExpectParameterBlockContains(w, r_zw, r_w); 716 } 717 ASSERT_TRUE (HasResidualBlock(r_yz )); 718 ASSERT_TRUE (HasResidualBlock(r_zw )); 719 ASSERT_TRUE (HasResidualBlock(r_y )); 720 ASSERT_TRUE (HasResidualBlock(r_z )); 721 ASSERT_TRUE (HasResidualBlock(r_w )); 722 723 // Remove r_zw. 724 problem->RemoveResidualBlock(r_zw); 725 ASSERT_EQ(3, problem->NumParameterBlocks()); 726 ASSERT_EQ(4, problem->NumResidualBlocks()); 727 if (GetParam()) { 728 ExpectParameterBlockContains(y, r_yz, r_y); 729 ExpectParameterBlockContains(z, r_yz, r_z); 730 ExpectParameterBlockContains(w, r_w); 731 } 732 ASSERT_TRUE (HasResidualBlock(r_yz )); 733 ASSERT_TRUE (HasResidualBlock(r_y )); 734 ASSERT_TRUE (HasResidualBlock(r_z )); 735 ASSERT_TRUE (HasResidualBlock(r_w )); 736 737 // Remove r_w. 738 problem->RemoveResidualBlock(r_w); 739 ASSERT_EQ(3, problem->NumParameterBlocks()); 740 ASSERT_EQ(3, problem->NumResidualBlocks()); 741 if (GetParam()) { 742 ExpectParameterBlockContains(y, r_yz, r_y); 743 ExpectParameterBlockContains(z, r_yz, r_z); 744 ExpectParameterBlockContains(w); 745 } 746 ASSERT_TRUE (HasResidualBlock(r_yz )); 747 ASSERT_TRUE (HasResidualBlock(r_y )); 748 ASSERT_TRUE (HasResidualBlock(r_z )); 749 750 // Remove r_yz. 751 problem->RemoveResidualBlock(r_yz); 752 ASSERT_EQ(3, problem->NumParameterBlocks()); 753 ASSERT_EQ(2, problem->NumResidualBlocks()); 754 if (GetParam()) { 755 ExpectParameterBlockContains(y, r_y); 756 ExpectParameterBlockContains(z, r_z); 757 ExpectParameterBlockContains(w); 758 } 759 ASSERT_TRUE (HasResidualBlock(r_y )); 760 ASSERT_TRUE (HasResidualBlock(r_z )); 761 762 // Remove the last two. 763 problem->RemoveResidualBlock(r_z); 764 problem->RemoveResidualBlock(r_y); 765 ASSERT_EQ(3, problem->NumParameterBlocks()); 766 ASSERT_EQ(0, problem->NumResidualBlocks()); 767 if (GetParam()) { 768 ExpectParameterBlockContains(y); 769 ExpectParameterBlockContains(z); 770 ExpectParameterBlockContains(w); 771 } 772 } 773 774 INSTANTIATE_TEST_CASE_P(OptionsInstantiation, 775 DynamicProblem, 776 ::testing::Values(true, false)); 777 778 // Test for Problem::Evaluate 779 780 // r_i = i - (j + 1) * x_ij^2 781 template <int kNumResiduals, int kNumParameterBlocks> 782 class QuadraticCostFunction : public CostFunction { 783 public: 784 QuadraticCostFunction() { 785 CHECK_GT(kNumResiduals, 0); 786 CHECK_GT(kNumParameterBlocks, 0); 787 set_num_residuals(kNumResiduals); 788 for (int i = 0; i < kNumParameterBlocks; ++i) { 789 mutable_parameter_block_sizes()->push_back(kNumResiduals); 790 } 791 } 792 793 virtual bool Evaluate(double const* const* parameters, 794 double* residuals, 795 double** jacobians) const { 796 for (int i = 0; i < kNumResiduals; ++i) { 797 residuals[i] = i; 798 for (int j = 0; j < kNumParameterBlocks; ++j) { 799 residuals[i] -= (j + 1.0) * parameters[j][i] * parameters[j][i]; 800 } 801 } 802 803 if (jacobians == NULL) { 804 return true; 805 } 806 807 for (int j = 0; j < kNumParameterBlocks; ++j) { 808 if (jacobians[j] != NULL) { 809 MatrixRef(jacobians[j], kNumResiduals, kNumResiduals) = 810 (-2.0 * (j + 1.0) * 811 ConstVectorRef(parameters[j], kNumResiduals)).asDiagonal(); 812 } 813 } 814 815 return true; 816 } 817 }; 818 819 // Convert a CRSMatrix to a dense Eigen matrix. 820 void CRSToDenseMatrix(const CRSMatrix& input, Matrix* output) { 821 Matrix& m = *CHECK_NOTNULL(output); 822 m.resize(input.num_rows, input.num_cols); 823 m.setZero(); 824 for (int row = 0; row < input.num_rows; ++row) { 825 for (int j = input.rows[row]; j < input.rows[row + 1]; ++j) { 826 const int col = input.cols[j]; 827 m(row, col) = input.values[j]; 828 } 829 } 830 } 831 832 class ProblemEvaluateTest : public ::testing::Test { 833 protected: 834 void SetUp() { 835 for (int i = 0; i < 6; ++i) { 836 parameters_[i] = static_cast<double>(i + 1); 837 } 838 839 parameter_blocks_.push_back(parameters_); 840 parameter_blocks_.push_back(parameters_ + 2); 841 parameter_blocks_.push_back(parameters_ + 4); 842 843 844 CostFunction* cost_function = new QuadraticCostFunction<2, 2>; 845 846 // f(x, y) 847 residual_blocks_.push_back( 848 problem_.AddResidualBlock(cost_function, 849 NULL, 850 parameters_, 851 parameters_ + 2)); 852 // g(y, z) 853 residual_blocks_.push_back( 854 problem_.AddResidualBlock(cost_function, 855 NULL, parameters_ + 2, 856 parameters_ + 4)); 857 // h(z, x) 858 residual_blocks_.push_back( 859 problem_.AddResidualBlock(cost_function, 860 NULL, 861 parameters_ + 4, 862 parameters_)); 863 } 864 865 866 867 void EvaluateAndCompare(const Problem::EvaluateOptions& options, 868 const int expected_num_rows, 869 const int expected_num_cols, 870 const double expected_cost, 871 const double* expected_residuals, 872 const double* expected_gradient, 873 const double* expected_jacobian) { 874 double cost; 875 vector<double> residuals; 876 vector<double> gradient; 877 CRSMatrix jacobian; 878 879 EXPECT_TRUE( 880 problem_.Evaluate(options, 881 &cost, 882 expected_residuals != NULL ? &residuals : NULL, 883 expected_gradient != NULL ? &gradient : NULL, 884 expected_jacobian != NULL ? &jacobian : NULL)); 885 886 if (expected_residuals != NULL) { 887 EXPECT_EQ(residuals.size(), expected_num_rows); 888 } 889 890 if (expected_gradient != NULL) { 891 EXPECT_EQ(gradient.size(), expected_num_cols); 892 } 893 894 if (expected_jacobian != NULL) { 895 EXPECT_EQ(jacobian.num_rows, expected_num_rows); 896 EXPECT_EQ(jacobian.num_cols, expected_num_cols); 897 } 898 899 Matrix dense_jacobian; 900 if (expected_jacobian != NULL) { 901 CRSToDenseMatrix(jacobian, &dense_jacobian); 902 } 903 904 CompareEvaluations(expected_num_rows, 905 expected_num_cols, 906 expected_cost, 907 expected_residuals, 908 expected_gradient, 909 expected_jacobian, 910 cost, 911 residuals.size() > 0 ? &residuals[0] : NULL, 912 gradient.size() > 0 ? &gradient[0] : NULL, 913 dense_jacobian.data()); 914 } 915 916 void CheckAllEvaluationCombinations(const Problem::EvaluateOptions& options, 917 const ExpectedEvaluation& expected) { 918 for (int i = 0; i < 8; ++i) { 919 EvaluateAndCompare(options, 920 expected.num_rows, 921 expected.num_cols, 922 expected.cost, 923 (i & 1) ? expected.residuals : NULL, 924 (i & 2) ? expected.gradient : NULL, 925 (i & 4) ? expected.jacobian : NULL); 926 } 927 } 928 929 ProblemImpl problem_; 930 double parameters_[6]; 931 vector<double*> parameter_blocks_; 932 vector<ResidualBlockId> residual_blocks_; 933 }; 934 935 936 TEST_F(ProblemEvaluateTest, MultipleParameterAndResidualBlocks) { 937 ExpectedEvaluation expected = { 938 // Rows/columns 939 6, 6, 940 // Cost 941 7607.0, 942 // Residuals 943 { -19.0, -35.0, // f 944 -59.0, -87.0, // g 945 -27.0, -43.0 // h 946 }, 947 // Gradient 948 { 146.0, 484.0, // x 949 582.0, 1256.0, // y 950 1450.0, 2604.0, // z 951 }, 952 // Jacobian 953 // x y z 954 { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0, 955 0.0, -4.0, 0.0, -16.0, 0.0, 0.0, 956 /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0, 957 0.0, 0.0, 0.0, -8.0, 0.0, -24.0, 958 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 959 0.0, -8.0, 0.0, 0.0, 0.0, -12.0 960 } 961 }; 962 963 CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected); 964 } 965 966 TEST_F(ProblemEvaluateTest, ParameterAndResidualBlocksPassedInOptions) { 967 ExpectedEvaluation expected = { 968 // Rows/columns 969 6, 6, 970 // Cost 971 7607.0, 972 // Residuals 973 { -19.0, -35.0, // f 974 -59.0, -87.0, // g 975 -27.0, -43.0 // h 976 }, 977 // Gradient 978 { 146.0, 484.0, // x 979 582.0, 1256.0, // y 980 1450.0, 2604.0, // z 981 }, 982 // Jacobian 983 // x y z 984 { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0, 985 0.0, -4.0, 0.0, -16.0, 0.0, 0.0, 986 /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0, 987 0.0, 0.0, 0.0, -8.0, 0.0, -24.0, 988 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 989 0.0, -8.0, 0.0, 0.0, 0.0, -12.0 990 } 991 }; 992 993 Problem::EvaluateOptions evaluate_options; 994 evaluate_options.parameter_blocks = parameter_blocks_; 995 evaluate_options.residual_blocks = residual_blocks_; 996 CheckAllEvaluationCombinations(evaluate_options, expected); 997 } 998 999 TEST_F(ProblemEvaluateTest, ReorderedResidualBlocks) { 1000 ExpectedEvaluation expected = { 1001 // Rows/columns 1002 6, 6, 1003 // Cost 1004 7607.0, 1005 // Residuals 1006 { -19.0, -35.0, // f 1007 -27.0, -43.0, // h 1008 -59.0, -87.0 // g 1009 }, 1010 // Gradient 1011 { 146.0, 484.0, // x 1012 582.0, 1256.0, // y 1013 1450.0, 2604.0, // z 1014 }, 1015 // Jacobian 1016 // x y z 1017 { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0, 1018 0.0, -4.0, 0.0, -16.0, 0.0, 0.0, 1019 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 1020 0.0, -8.0, 0.0, 0.0, 0.0, -12.0, 1021 /* g(y, z) */ 0.0, 0.0, -6.0, 0.0, -20.0, 0.0, 1022 0.0, 0.0, 0.0, -8.0, 0.0, -24.0 1023 } 1024 }; 1025 1026 Problem::EvaluateOptions evaluate_options; 1027 evaluate_options.parameter_blocks = parameter_blocks_; 1028 1029 // f, h, g 1030 evaluate_options.residual_blocks.push_back(residual_blocks_[0]); 1031 evaluate_options.residual_blocks.push_back(residual_blocks_[2]); 1032 evaluate_options.residual_blocks.push_back(residual_blocks_[1]); 1033 1034 CheckAllEvaluationCombinations(evaluate_options, expected); 1035 } 1036 1037 TEST_F(ProblemEvaluateTest, ReorderedResidualBlocksAndReorderedParameterBlocks) { 1038 ExpectedEvaluation expected = { 1039 // Rows/columns 1040 6, 6, 1041 // Cost 1042 7607.0, 1043 // Residuals 1044 { -19.0, -35.0, // f 1045 -27.0, -43.0, // h 1046 -59.0, -87.0 // g 1047 }, 1048 // Gradient 1049 { 1450.0, 2604.0, // z 1050 582.0, 1256.0, // y 1051 146.0, 484.0, // x 1052 }, 1053 // Jacobian 1054 // z y x 1055 { /* f(x, y) */ 0.0, 0.0, -12.0, 0.0, -2.0, 0.0, 1056 0.0, 0.0, 0.0, -16.0, 0.0, -4.0, 1057 /* h(z, x) */ -10.0, 0.0, 0.0, 0.0, -4.0, 0.0, 1058 0.0, -12.0, 0.0, 0.0, 0.0, -8.0, 1059 /* g(y, z) */ -20.0, 0.0, -6.0, 0.0, 0.0, 0.0, 1060 0.0, -24.0, 0.0, -8.0, 0.0, 0.0 1061 } 1062 }; 1063 1064 Problem::EvaluateOptions evaluate_options; 1065 // z, y, x 1066 evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]); 1067 evaluate_options.parameter_blocks.push_back(parameter_blocks_[1]); 1068 evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]); 1069 1070 // f, h, g 1071 evaluate_options.residual_blocks.push_back(residual_blocks_[0]); 1072 evaluate_options.residual_blocks.push_back(residual_blocks_[2]); 1073 evaluate_options.residual_blocks.push_back(residual_blocks_[1]); 1074 1075 CheckAllEvaluationCombinations(evaluate_options, expected); 1076 } 1077 1078 TEST_F(ProblemEvaluateTest, ConstantParameterBlock) { 1079 ExpectedEvaluation expected = { 1080 // Rows/columns 1081 6, 6, 1082 // Cost 1083 7607.0, 1084 // Residuals 1085 { -19.0, -35.0, // f 1086 -59.0, -87.0, // g 1087 -27.0, -43.0 // h 1088 }, 1089 1090 // Gradient 1091 { 146.0, 484.0, // x 1092 0.0, 0.0, // y 1093 1450.0, 2604.0, // z 1094 }, 1095 1096 // Jacobian 1097 // x y z 1098 { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1099 0.0, -4.0, 0.0, 0.0, 0.0, 0.0, 1100 /* g(y, z) */ 0.0, 0.0, 0.0, 0.0, -20.0, 0.0, 1101 0.0, 0.0, 0.0, 0.0, 0.0, -24.0, 1102 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 1103 0.0, -8.0, 0.0, 0.0, 0.0, -12.0 1104 } 1105 }; 1106 1107 problem_.SetParameterBlockConstant(parameters_ + 2); 1108 CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected); 1109 } 1110 1111 TEST_F(ProblemEvaluateTest, ExcludedAResidualBlock) { 1112 ExpectedEvaluation expected = { 1113 // Rows/columns 1114 4, 6, 1115 // Cost 1116 2082.0, 1117 // Residuals 1118 { -19.0, -35.0, // f 1119 -27.0, -43.0 // h 1120 }, 1121 // Gradient 1122 { 146.0, 484.0, // x 1123 228.0, 560.0, // y 1124 270.0, 516.0, // z 1125 }, 1126 // Jacobian 1127 // x y z 1128 { /* f(x, y) */ -2.0, 0.0, -12.0, 0.0, 0.0, 0.0, 1129 0.0, -4.0, 0.0, -16.0, 0.0, 0.0, 1130 /* h(z, x) */ -4.0, 0.0, 0.0, 0.0, -10.0, 0.0, 1131 0.0, -8.0, 0.0, 0.0, 0.0, -12.0 1132 } 1133 }; 1134 1135 Problem::EvaluateOptions evaluate_options; 1136 evaluate_options.residual_blocks.push_back(residual_blocks_[0]); 1137 evaluate_options.residual_blocks.push_back(residual_blocks_[2]); 1138 1139 CheckAllEvaluationCombinations(evaluate_options, expected); 1140 } 1141 1142 TEST_F(ProblemEvaluateTest, ExcludedParameterBlock) { 1143 ExpectedEvaluation expected = { 1144 // Rows/columns 1145 6, 4, 1146 // Cost 1147 7607.0, 1148 // Residuals 1149 { -19.0, -35.0, // f 1150 -59.0, -87.0, // g 1151 -27.0, -43.0 // h 1152 }, 1153 1154 // Gradient 1155 { 146.0, 484.0, // x 1156 1450.0, 2604.0, // z 1157 }, 1158 1159 // Jacobian 1160 // x z 1161 { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 1162 0.0, -4.0, 0.0, 0.0, 1163 /* g(y, z) */ 0.0, 0.0, -20.0, 0.0, 1164 0.0, 0.0, 0.0, -24.0, 1165 /* h(z, x) */ -4.0, 0.0, -10.0, 0.0, 1166 0.0, -8.0, 0.0, -12.0 1167 } 1168 }; 1169 1170 Problem::EvaluateOptions evaluate_options; 1171 // x, z 1172 evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]); 1173 evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]); 1174 evaluate_options.residual_blocks = residual_blocks_; 1175 CheckAllEvaluationCombinations(evaluate_options, expected); 1176 } 1177 1178 TEST_F(ProblemEvaluateTest, ExcludedParameterBlockAndExcludedResidualBlock) { 1179 ExpectedEvaluation expected = { 1180 // Rows/columns 1181 4, 4, 1182 // Cost 1183 6318.0, 1184 // Residuals 1185 { -19.0, -35.0, // f 1186 -59.0, -87.0, // g 1187 }, 1188 1189 // Gradient 1190 { 38.0, 140.0, // x 1191 1180.0, 2088.0, // z 1192 }, 1193 1194 // Jacobian 1195 // x z 1196 { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 1197 0.0, -4.0, 0.0, 0.0, 1198 /* g(y, z) */ 0.0, 0.0, -20.0, 0.0, 1199 0.0, 0.0, 0.0, -24.0, 1200 } 1201 }; 1202 1203 Problem::EvaluateOptions evaluate_options; 1204 // x, z 1205 evaluate_options.parameter_blocks.push_back(parameter_blocks_[0]); 1206 evaluate_options.parameter_blocks.push_back(parameter_blocks_[2]); 1207 evaluate_options.residual_blocks.push_back(residual_blocks_[0]); 1208 evaluate_options.residual_blocks.push_back(residual_blocks_[1]); 1209 1210 CheckAllEvaluationCombinations(evaluate_options, expected); 1211 } 1212 1213 TEST_F(ProblemEvaluateTest, LocalParameterization) { 1214 ExpectedEvaluation expected = { 1215 // Rows/columns 1216 6, 5, 1217 // Cost 1218 7607.0, 1219 // Residuals 1220 { -19.0, -35.0, // f 1221 -59.0, -87.0, // g 1222 -27.0, -43.0 // h 1223 }, 1224 // Gradient 1225 { 146.0, 484.0, // x 1226 1256.0, // y with SubsetParameterization 1227 1450.0, 2604.0, // z 1228 }, 1229 // Jacobian 1230 // x y z 1231 { /* f(x, y) */ -2.0, 0.0, 0.0, 0.0, 0.0, 1232 0.0, -4.0, -16.0, 0.0, 0.0, 1233 /* g(y, z) */ 0.0, 0.0, 0.0, -20.0, 0.0, 1234 0.0, 0.0, -8.0, 0.0, -24.0, 1235 /* h(z, x) */ -4.0, 0.0, 0.0, -10.0, 0.0, 1236 0.0, -8.0, 0.0, 0.0, -12.0 1237 } 1238 }; 1239 1240 vector<int> constant_parameters; 1241 constant_parameters.push_back(0); 1242 problem_.SetParameterization(parameters_ + 2, 1243 new SubsetParameterization(2, 1244 constant_parameters)); 1245 1246 CheckAllEvaluationCombinations(Problem::EvaluateOptions(), expected); 1247 } 1248 1249 } // namespace internal 1250 } // namespace ceres 1251
