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 31 #include "gtest/gtest.h" 32 #include "ceres/autodiff_cost_function.h" 33 #include "ceres/linear_solver.h" 34 #include "ceres/ordered_groups.h" 35 #include "ceres/parameter_block.h" 36 #include "ceres/problem_impl.h" 37 #include "ceres/program.h" 38 #include "ceres/residual_block.h" 39 #include "ceres/solver_impl.h" 40 #include "ceres/sized_cost_function.h" 41 42 namespace ceres { 43 namespace internal { 44 45 // A cost function that sipmply returns its argument. 46 class UnaryIdentityCostFunction : public SizedCostFunction<1, 1> { 47 public: 48 virtual bool Evaluate(double const* const* parameters, 49 double* residuals, 50 double** jacobians) const { 51 residuals[0] = parameters[0][0]; 52 if (jacobians != NULL && jacobians[0] != NULL) { 53 jacobians[0][0] = 1.0; 54 } 55 return true; 56 } 57 }; 58 59 // Templated base class for the CostFunction signatures. 60 template <int kNumResiduals, int N0, int N1, int N2> 61 class MockCostFunctionBase : public 62 SizedCostFunction<kNumResiduals, N0, N1, N2> { 63 public: 64 virtual bool Evaluate(double const* const* parameters, 65 double* residuals, 66 double** jacobians) const { 67 // Do nothing. This is never called. 68 return true; 69 } 70 }; 71 72 class UnaryCostFunction : public MockCostFunctionBase<2, 1, 0, 0> {}; 73 class BinaryCostFunction : public MockCostFunctionBase<2, 1, 1, 0> {}; 74 class TernaryCostFunction : public MockCostFunctionBase<2, 1, 1, 1> {}; 75 76 TEST(SolverImpl, RemoveFixedBlocksNothingConstant) { 77 ProblemImpl problem; 78 double x; 79 double y; 80 double z; 81 82 problem.AddParameterBlock(&x, 1); 83 problem.AddParameterBlock(&y, 1); 84 problem.AddParameterBlock(&z, 1); 85 problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); 86 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y); 87 problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z); 88 89 string error; 90 { 91 ParameterBlockOrdering ordering; 92 ordering.AddElementToGroup(&x, 0); 93 ordering.AddElementToGroup(&y, 0); 94 ordering.AddElementToGroup(&z, 0); 95 96 Program program(*problem.mutable_program()); 97 EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program, 98 &ordering, 99 NULL, 100 &error)); 101 EXPECT_EQ(program.NumParameterBlocks(), 3); 102 EXPECT_EQ(program.NumResidualBlocks(), 3); 103 EXPECT_EQ(ordering.NumElements(), 3); 104 } 105 } 106 107 TEST(SolverImpl, RemoveFixedBlocksAllParameterBlocksConstant) { 108 ProblemImpl problem; 109 double x; 110 111 problem.AddParameterBlock(&x, 1); 112 problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); 113 problem.SetParameterBlockConstant(&x); 114 115 ParameterBlockOrdering ordering; 116 ordering.AddElementToGroup(&x, 0); 117 118 Program program(problem.program()); 119 string error; 120 EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program, 121 &ordering, 122 NULL, 123 &error)); 124 EXPECT_EQ(program.NumParameterBlocks(), 0); 125 EXPECT_EQ(program.NumResidualBlocks(), 0); 126 EXPECT_EQ(ordering.NumElements(), 0); 127 } 128 129 TEST(SolverImpl, RemoveFixedBlocksNoResidualBlocks) { 130 ProblemImpl problem; 131 double x; 132 double y; 133 double z; 134 135 problem.AddParameterBlock(&x, 1); 136 problem.AddParameterBlock(&y, 1); 137 problem.AddParameterBlock(&z, 1); 138 139 ParameterBlockOrdering ordering; 140 ordering.AddElementToGroup(&x, 0); 141 ordering.AddElementToGroup(&y, 0); 142 ordering.AddElementToGroup(&z, 0); 143 144 145 Program program(problem.program()); 146 string error; 147 EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program, 148 &ordering, 149 NULL, 150 &error)); 151 EXPECT_EQ(program.NumParameterBlocks(), 0); 152 EXPECT_EQ(program.NumResidualBlocks(), 0); 153 EXPECT_EQ(ordering.NumElements(), 0); 154 } 155 156 TEST(SolverImpl, RemoveFixedBlocksOneParameterBlockConstant) { 157 ProblemImpl problem; 158 double x; 159 double y; 160 double z; 161 162 problem.AddParameterBlock(&x, 1); 163 problem.AddParameterBlock(&y, 1); 164 problem.AddParameterBlock(&z, 1); 165 166 ParameterBlockOrdering ordering; 167 ordering.AddElementToGroup(&x, 0); 168 ordering.AddElementToGroup(&y, 0); 169 ordering.AddElementToGroup(&z, 0); 170 171 problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); 172 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y); 173 problem.SetParameterBlockConstant(&x); 174 175 176 Program program(problem.program()); 177 string error; 178 EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program, 179 &ordering, 180 NULL, 181 &error)); 182 EXPECT_EQ(program.NumParameterBlocks(), 1); 183 EXPECT_EQ(program.NumResidualBlocks(), 1); 184 EXPECT_EQ(ordering.NumElements(), 1); 185 } 186 187 TEST(SolverImpl, RemoveFixedBlocksNumEliminateBlocks) { 188 ProblemImpl problem; 189 double x; 190 double y; 191 double z; 192 193 problem.AddParameterBlock(&x, 1); 194 problem.AddParameterBlock(&y, 1); 195 problem.AddParameterBlock(&z, 1); 196 problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); 197 problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z); 198 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y); 199 problem.SetParameterBlockConstant(&x); 200 201 ParameterBlockOrdering ordering; 202 ordering.AddElementToGroup(&x, 0); 203 ordering.AddElementToGroup(&y, 0); 204 ordering.AddElementToGroup(&z, 1); 205 206 Program program(problem.program()); 207 string error; 208 EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program, 209 &ordering, 210 NULL, 211 &error)); 212 EXPECT_EQ(program.NumParameterBlocks(), 2); 213 EXPECT_EQ(program.NumResidualBlocks(), 2); 214 EXPECT_EQ(ordering.NumElements(), 2); 215 EXPECT_EQ(ordering.GroupId(&y), 0); 216 EXPECT_EQ(ordering.GroupId(&z), 1); 217 } 218 219 TEST(SolverImpl, RemoveFixedBlocksFixedCost) { 220 ProblemImpl problem; 221 double x = 1.23; 222 double y = 4.56; 223 double z = 7.89; 224 225 problem.AddParameterBlock(&x, 1); 226 problem.AddParameterBlock(&y, 1); 227 problem.AddParameterBlock(&z, 1); 228 problem.AddResidualBlock(new UnaryIdentityCostFunction(), NULL, &x); 229 problem.AddResidualBlock(new TernaryCostFunction(), NULL, &x, &y, &z); 230 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y); 231 problem.SetParameterBlockConstant(&x); 232 233 ParameterBlockOrdering ordering; 234 ordering.AddElementToGroup(&x, 0); 235 ordering.AddElementToGroup(&y, 0); 236 ordering.AddElementToGroup(&z, 1); 237 238 double fixed_cost = 0.0; 239 Program program(problem.program()); 240 241 double expected_fixed_cost; 242 ResidualBlock *expected_removed_block = program.residual_blocks()[0]; 243 scoped_array<double> scratch(new double[expected_removed_block->NumScratchDoublesForEvaluate()]); 244 expected_removed_block->Evaluate(&expected_fixed_cost, NULL, NULL, scratch.get()); 245 246 string error; 247 EXPECT_TRUE(SolverImpl::RemoveFixedBlocksFromProgram(&program, 248 &ordering, 249 &fixed_cost, 250 &error)); 251 EXPECT_EQ(program.NumParameterBlocks(), 2); 252 EXPECT_EQ(program.NumResidualBlocks(), 2); 253 EXPECT_EQ(ordering.NumElements(), 2); 254 EXPECT_EQ(ordering.GroupId(&y), 0); 255 EXPECT_EQ(ordering.GroupId(&z), 1); 256 EXPECT_DOUBLE_EQ(fixed_cost, expected_fixed_cost); 257 } 258 259 TEST(SolverImpl, ReorderResidualBlockNormalFunction) { 260 ProblemImpl problem; 261 double x; 262 double y; 263 double z; 264 265 problem.AddParameterBlock(&x, 1); 266 problem.AddParameterBlock(&y, 1); 267 problem.AddParameterBlock(&z, 1); 268 269 problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); 270 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x); 271 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); 272 problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z); 273 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &y); 274 problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y); 275 276 ParameterBlockOrdering* ordering = new ParameterBlockOrdering; 277 ordering->AddElementToGroup(&x, 0); 278 ordering->AddElementToGroup(&y, 0); 279 ordering->AddElementToGroup(&z, 1); 280 281 Solver::Options options; 282 options.linear_solver_type = DENSE_SCHUR; 283 options.linear_solver_ordering = ordering; 284 285 const vector<ResidualBlock*>& residual_blocks = 286 problem.program().residual_blocks(); 287 288 vector<ResidualBlock*> expected_residual_blocks; 289 290 // This is a bit fragile, but it serves the purpose. We know the 291 // bucketing algorithm that the reordering function uses, so we 292 // expect the order for residual blocks for each e_block to be 293 // filled in reverse. 294 expected_residual_blocks.push_back(residual_blocks[4]); 295 expected_residual_blocks.push_back(residual_blocks[1]); 296 expected_residual_blocks.push_back(residual_blocks[0]); 297 expected_residual_blocks.push_back(residual_blocks[5]); 298 expected_residual_blocks.push_back(residual_blocks[2]); 299 expected_residual_blocks.push_back(residual_blocks[3]); 300 301 Program* program = problem.mutable_program(); 302 program->SetParameterOffsetsAndIndex(); 303 304 string error; 305 EXPECT_TRUE(SolverImpl::LexicographicallyOrderResidualBlocks( 306 2, 307 problem.mutable_program(), 308 &error)); 309 EXPECT_EQ(residual_blocks.size(), expected_residual_blocks.size()); 310 for (int i = 0; i < expected_residual_blocks.size(); ++i) { 311 EXPECT_EQ(residual_blocks[i], expected_residual_blocks[i]); 312 } 313 } 314 315 TEST(SolverImpl, ReorderResidualBlockNormalFunctionWithFixedBlocks) { 316 ProblemImpl problem; 317 double x; 318 double y; 319 double z; 320 321 problem.AddParameterBlock(&x, 1); 322 problem.AddParameterBlock(&y, 1); 323 problem.AddParameterBlock(&z, 1); 324 325 // Set one parameter block constant. 326 problem.SetParameterBlockConstant(&z); 327 328 // Mark residuals for x's row block with "x" for readability. 329 problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); // 0 x 330 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x); // 1 x 331 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); // 2 332 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); // 3 333 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z); // 4 x 334 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); // 5 335 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z); // 6 x 336 problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y); // 7 337 338 ParameterBlockOrdering* ordering = new ParameterBlockOrdering; 339 ordering->AddElementToGroup(&x, 0); 340 ordering->AddElementToGroup(&z, 0); 341 ordering->AddElementToGroup(&y, 1); 342 343 Solver::Options options; 344 options.linear_solver_type = DENSE_SCHUR; 345 options.linear_solver_ordering = ordering; 346 347 // Create the reduced program. This should remove the fixed block "z", 348 // marking the index to -1 at the same time. x and y also get indices. 349 string error; 350 scoped_ptr<Program> reduced_program( 351 SolverImpl::CreateReducedProgram(&options, &problem, NULL, &error)); 352 353 const vector<ResidualBlock*>& residual_blocks = 354 problem.program().residual_blocks(); 355 356 // This is a bit fragile, but it serves the purpose. We know the 357 // bucketing algorithm that the reordering function uses, so we 358 // expect the order for residual blocks for each e_block to be 359 // filled in reverse. 360 361 vector<ResidualBlock*> expected_residual_blocks; 362 363 // Row block for residuals involving "x". These are marked "x" in the block 364 // of code calling AddResidual() above. 365 expected_residual_blocks.push_back(residual_blocks[6]); 366 expected_residual_blocks.push_back(residual_blocks[4]); 367 expected_residual_blocks.push_back(residual_blocks[1]); 368 expected_residual_blocks.push_back(residual_blocks[0]); 369 370 // Row block for residuals involving "y". 371 expected_residual_blocks.push_back(residual_blocks[7]); 372 expected_residual_blocks.push_back(residual_blocks[5]); 373 expected_residual_blocks.push_back(residual_blocks[3]); 374 expected_residual_blocks.push_back(residual_blocks[2]); 375 376 EXPECT_TRUE(SolverImpl::LexicographicallyOrderResidualBlocks( 377 2, 378 reduced_program.get(), 379 &error)); 380 381 EXPECT_EQ(reduced_program->residual_blocks().size(), 382 expected_residual_blocks.size()); 383 for (int i = 0; i < expected_residual_blocks.size(); ++i) { 384 EXPECT_EQ(reduced_program->residual_blocks()[i], 385 expected_residual_blocks[i]); 386 } 387 } 388 389 TEST(SolverImpl, AutomaticSchurReorderingRespectsConstantBlocks) { 390 ProblemImpl problem; 391 double x; 392 double y; 393 double z; 394 395 problem.AddParameterBlock(&x, 1); 396 problem.AddParameterBlock(&y, 1); 397 problem.AddParameterBlock(&z, 1); 398 399 // Set one parameter block constant. 400 problem.SetParameterBlockConstant(&z); 401 402 problem.AddResidualBlock(new UnaryCostFunction(), NULL, &x); 403 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &x); 404 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); 405 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); 406 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z); 407 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &z, &y); 408 problem.AddResidualBlock(new BinaryCostFunction(), NULL, &x, &z); 409 problem.AddResidualBlock(new UnaryCostFunction(), NULL, &y); 410 problem.AddResidualBlock(new UnaryCostFunction(), NULL, &z); 411 412 ParameterBlockOrdering* ordering = new ParameterBlockOrdering; 413 ordering->AddElementToGroup(&x, 0); 414 ordering->AddElementToGroup(&z, 0); 415 ordering->AddElementToGroup(&y, 0); 416 417 Solver::Options options; 418 options.linear_solver_type = DENSE_SCHUR; 419 options.linear_solver_ordering = ordering; 420 421 string error; 422 scoped_ptr<Program> reduced_program( 423 SolverImpl::CreateReducedProgram(&options, &problem, NULL, &error)); 424 425 const vector<ResidualBlock*>& residual_blocks = 426 reduced_program->residual_blocks(); 427 const vector<ParameterBlock*>& parameter_blocks = 428 reduced_program->parameter_blocks(); 429 430 const vector<ResidualBlock*>& original_residual_blocks = 431 problem.program().residual_blocks(); 432 433 EXPECT_EQ(residual_blocks.size(), 8); 434 EXPECT_EQ(reduced_program->parameter_blocks().size(), 2); 435 436 // Verify that right parmeter block and the residual blocks have 437 // been removed. 438 for (int i = 0; i < 8; ++i) { 439 EXPECT_NE(residual_blocks[i], original_residual_blocks.back()); 440 } 441 for (int i = 0; i < 2; ++i) { 442 EXPECT_NE(parameter_blocks[i]->mutable_user_state(), &z); 443 } 444 } 445 446 TEST(SolverImpl, ApplyUserOrderingOrderingTooSmall) { 447 ProblemImpl problem; 448 double x; 449 double y; 450 double z; 451 452 problem.AddParameterBlock(&x, 1); 453 problem.AddParameterBlock(&y, 1); 454 problem.AddParameterBlock(&z, 1); 455 456 ParameterBlockOrdering ordering; 457 ordering.AddElementToGroup(&x, 0); 458 ordering.AddElementToGroup(&y, 1); 459 460 Program program(problem.program()); 461 string error; 462 EXPECT_FALSE(SolverImpl::ApplyUserOrdering(problem.parameter_map(), 463 &ordering, 464 &program, 465 &error)); 466 } 467 468 TEST(SolverImpl, ApplyUserOrderingNormal) { 469 ProblemImpl problem; 470 double x; 471 double y; 472 double z; 473 474 problem.AddParameterBlock(&x, 1); 475 problem.AddParameterBlock(&y, 1); 476 problem.AddParameterBlock(&z, 1); 477 478 ParameterBlockOrdering ordering; 479 ordering.AddElementToGroup(&x, 0); 480 ordering.AddElementToGroup(&y, 2); 481 ordering.AddElementToGroup(&z, 1); 482 483 Program* program = problem.mutable_program(); 484 string error; 485 486 EXPECT_TRUE(SolverImpl::ApplyUserOrdering(problem.parameter_map(), 487 &ordering, 488 program, 489 &error)); 490 const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks(); 491 492 EXPECT_EQ(parameter_blocks.size(), 3); 493 EXPECT_EQ(parameter_blocks[0]->user_state(), &x); 494 EXPECT_EQ(parameter_blocks[1]->user_state(), &z); 495 EXPECT_EQ(parameter_blocks[2]->user_state(), &y); 496 } 497 498 #if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) 499 TEST(SolverImpl, CreateLinearSolverNoSuiteSparse) { 500 Solver::Options options; 501 options.linear_solver_type = SPARSE_NORMAL_CHOLESKY; 502 string error; 503 EXPECT_FALSE(SolverImpl::CreateLinearSolver(&options, &error)); 504 } 505 #endif 506 507 TEST(SolverImpl, CreateLinearSolverNegativeMaxNumIterations) { 508 Solver::Options options; 509 options.linear_solver_type = DENSE_QR; 510 options.linear_solver_max_num_iterations = -1; 511 // CreateLinearSolver assumes a non-empty ordering. 512 options.linear_solver_ordering = new ParameterBlockOrdering; 513 string error; 514 EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error), 515 static_cast<LinearSolver*>(NULL)); 516 } 517 518 TEST(SolverImpl, CreateLinearSolverNegativeMinNumIterations) { 519 Solver::Options options; 520 options.linear_solver_type = DENSE_QR; 521 options.linear_solver_min_num_iterations = -1; 522 // CreateLinearSolver assumes a non-empty ordering. 523 options.linear_solver_ordering = new ParameterBlockOrdering; 524 string error; 525 EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error), 526 static_cast<LinearSolver*>(NULL)); 527 } 528 529 TEST(SolverImpl, CreateLinearSolverMaxLessThanMinIterations) { 530 Solver::Options options; 531 options.linear_solver_type = DENSE_QR; 532 options.linear_solver_min_num_iterations = 10; 533 options.linear_solver_max_num_iterations = 5; 534 options.linear_solver_ordering = new ParameterBlockOrdering; 535 string error; 536 EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error), 537 static_cast<LinearSolver*>(NULL)); 538 } 539 540 TEST(SolverImpl, CreateLinearSolverDenseSchurMultipleThreads) { 541 Solver::Options options; 542 options.linear_solver_type = DENSE_SCHUR; 543 options.num_linear_solver_threads = 2; 544 // The Schur type solvers can only be created with the Ordering 545 // contains at least one elimination group. 546 options.linear_solver_ordering = new ParameterBlockOrdering; 547 double x; 548 double y; 549 options.linear_solver_ordering->AddElementToGroup(&x, 0); 550 options.linear_solver_ordering->AddElementToGroup(&y, 0); 551 552 string error; 553 scoped_ptr<LinearSolver> solver( 554 SolverImpl::CreateLinearSolver(&options, &error)); 555 EXPECT_TRUE(solver != NULL); 556 EXPECT_EQ(options.linear_solver_type, DENSE_SCHUR); 557 EXPECT_EQ(options.num_linear_solver_threads, 1); 558 } 559 560 TEST(SolverImpl, CreateIterativeLinearSolverForDogleg) { 561 Solver::Options options; 562 options.trust_region_strategy_type = DOGLEG; 563 // CreateLinearSolver assumes a non-empty ordering. 564 options.linear_solver_ordering = new ParameterBlockOrdering; 565 string error; 566 options.linear_solver_type = ITERATIVE_SCHUR; 567 EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error), 568 static_cast<LinearSolver*>(NULL)); 569 570 options.linear_solver_type = CGNR; 571 EXPECT_EQ(SolverImpl::CreateLinearSolver(&options, &error), 572 static_cast<LinearSolver*>(NULL)); 573 } 574 575 TEST(SolverImpl, CreateLinearSolverNormalOperation) { 576 Solver::Options options; 577 scoped_ptr<LinearSolver> solver; 578 options.linear_solver_type = DENSE_QR; 579 // CreateLinearSolver assumes a non-empty ordering. 580 options.linear_solver_ordering = new ParameterBlockOrdering; 581 string error; 582 solver.reset(SolverImpl::CreateLinearSolver(&options, &error)); 583 EXPECT_EQ(options.linear_solver_type, DENSE_QR); 584 EXPECT_TRUE(solver.get() != NULL); 585 586 options.linear_solver_type = DENSE_NORMAL_CHOLESKY; 587 solver.reset(SolverImpl::CreateLinearSolver(&options, &error)); 588 EXPECT_EQ(options.linear_solver_type, DENSE_NORMAL_CHOLESKY); 589 EXPECT_TRUE(solver.get() != NULL); 590 591 #ifndef CERES_NO_SUITESPARSE 592 options.linear_solver_type = SPARSE_NORMAL_CHOLESKY; 593 options.sparse_linear_algebra_library = SUITE_SPARSE; 594 solver.reset(SolverImpl::CreateLinearSolver(&options, &error)); 595 EXPECT_EQ(options.linear_solver_type, SPARSE_NORMAL_CHOLESKY); 596 EXPECT_TRUE(solver.get() != NULL); 597 #endif 598 599 #ifndef CERES_NO_CXSPARSE 600 options.linear_solver_type = SPARSE_NORMAL_CHOLESKY; 601 options.sparse_linear_algebra_library = CX_SPARSE; 602 solver.reset(SolverImpl::CreateLinearSolver(&options, &error)); 603 EXPECT_EQ(options.linear_solver_type, SPARSE_NORMAL_CHOLESKY); 604 EXPECT_TRUE(solver.get() != NULL); 605 #endif 606 607 double x; 608 double y; 609 options.linear_solver_ordering->AddElementToGroup(&x, 0); 610 options.linear_solver_ordering->AddElementToGroup(&y, 0); 611 612 options.linear_solver_type = DENSE_SCHUR; 613 solver.reset(SolverImpl::CreateLinearSolver(&options, &error)); 614 EXPECT_EQ(options.linear_solver_type, DENSE_SCHUR); 615 EXPECT_TRUE(solver.get() != NULL); 616 617 options.linear_solver_type = SPARSE_SCHUR; 618 solver.reset(SolverImpl::CreateLinearSolver(&options, &error)); 619 620 #if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) 621 EXPECT_TRUE(SolverImpl::CreateLinearSolver(&options, &error) == NULL); 622 #else 623 EXPECT_TRUE(solver.get() != NULL); 624 EXPECT_EQ(options.linear_solver_type, SPARSE_SCHUR); 625 #endif 626 627 options.linear_solver_type = ITERATIVE_SCHUR; 628 solver.reset(SolverImpl::CreateLinearSolver(&options, &error)); 629 EXPECT_EQ(options.linear_solver_type, ITERATIVE_SCHUR); 630 EXPECT_TRUE(solver.get() != NULL); 631 } 632 633 struct QuadraticCostFunction { 634 template <typename T> bool operator()(const T* const x, 635 T* residual) const { 636 residual[0] = T(5.0) - *x; 637 return true; 638 } 639 }; 640 641 struct RememberingCallback : public IterationCallback { 642 explicit RememberingCallback(double *x) : calls(0), x(x) {} 643 virtual ~RememberingCallback() {} 644 virtual CallbackReturnType operator()(const IterationSummary& summary) { 645 x_values.push_back(*x); 646 return SOLVER_CONTINUE; 647 } 648 int calls; 649 double *x; 650 vector<double> x_values; 651 }; 652 653 TEST(SolverImpl, UpdateStateEveryIterationOption) { 654 double x = 50.0; 655 const double original_x = x; 656 657 scoped_ptr<CostFunction> cost_function( 658 new AutoDiffCostFunction<QuadraticCostFunction, 1, 1>( 659 new QuadraticCostFunction)); 660 661 Problem::Options problem_options; 662 problem_options.cost_function_ownership = DO_NOT_TAKE_OWNERSHIP; 663 ProblemImpl problem(problem_options); 664 problem.AddResidualBlock(cost_function.get(), NULL, &x); 665 666 Solver::Options options; 667 options.linear_solver_type = DENSE_QR; 668 669 RememberingCallback callback(&x); 670 options.callbacks.push_back(&callback); 671 672 Solver::Summary summary; 673 674 int num_iterations; 675 676 // First try: no updating. 677 SolverImpl::Solve(options, &problem, &summary); 678 num_iterations = summary.num_successful_steps + 679 summary.num_unsuccessful_steps; 680 EXPECT_GT(num_iterations, 1); 681 for (int i = 0; i < callback.x_values.size(); ++i) { 682 EXPECT_EQ(50.0, callback.x_values[i]); 683 } 684 685 // Second try: with updating 686 x = 50.0; 687 options.update_state_every_iteration = true; 688 callback.x_values.clear(); 689 SolverImpl::Solve(options, &problem, &summary); 690 num_iterations = summary.num_successful_steps + 691 summary.num_unsuccessful_steps; 692 EXPECT_GT(num_iterations, 1); 693 EXPECT_EQ(original_x, callback.x_values[0]); 694 EXPECT_NE(original_x, callback.x_values[1]); 695 } 696 697 // The parameters must be in separate blocks so that they can be individually 698 // set constant or not. 699 struct Quadratic4DCostFunction { 700 template <typename T> bool operator()(const T* const x, 701 const T* const y, 702 const T* const z, 703 const T* const w, 704 T* residual) const { 705 // A 4-dimension axis-aligned quadratic. 706 residual[0] = T(10.0) - *x + 707 T(20.0) - *y + 708 T(30.0) - *z + 709 T(40.0) - *w; 710 return true; 711 } 712 }; 713 714 TEST(SolverImpl, ConstantParameterBlocksDoNotChangeAndStateInvariantKept) { 715 double x = 50.0; 716 double y = 50.0; 717 double z = 50.0; 718 double w = 50.0; 719 const double original_x = 50.0; 720 const double original_y = 50.0; 721 const double original_z = 50.0; 722 const double original_w = 50.0; 723 724 scoped_ptr<CostFunction> cost_function( 725 new AutoDiffCostFunction<Quadratic4DCostFunction, 1, 1, 1, 1, 1>( 726 new Quadratic4DCostFunction)); 727 728 Problem::Options problem_options; 729 problem_options.cost_function_ownership = DO_NOT_TAKE_OWNERSHIP; 730 731 ProblemImpl problem(problem_options); 732 problem.AddResidualBlock(cost_function.get(), NULL, &x, &y, &z, &w); 733 problem.SetParameterBlockConstant(&x); 734 problem.SetParameterBlockConstant(&w); 735 736 Solver::Options options; 737 options.linear_solver_type = DENSE_QR; 738 739 Solver::Summary summary; 740 SolverImpl::Solve(options, &problem, &summary); 741 742 // Verify only the non-constant parameters were mutated. 743 EXPECT_EQ(original_x, x); 744 EXPECT_NE(original_y, y); 745 EXPECT_NE(original_z, z); 746 EXPECT_EQ(original_w, w); 747 748 // Check that the parameter block state pointers are pointing back at the 749 // user state, instead of inside a random temporary vector made by Solve(). 750 EXPECT_EQ(&x, problem.program().parameter_blocks()[0]->state()); 751 EXPECT_EQ(&y, problem.program().parameter_blocks()[1]->state()); 752 EXPECT_EQ(&z, problem.program().parameter_blocks()[2]->state()); 753 EXPECT_EQ(&w, problem.program().parameter_blocks()[3]->state()); 754 } 755 756 #define CHECK_ARRAY(name, value) \ 757 if (options.return_ ## name) { \ 758 EXPECT_EQ(summary.name.size(), 1); \ 759 EXPECT_EQ(summary.name[0], value); \ 760 } else { \ 761 EXPECT_EQ(summary.name.size(), 0); \ 762 } 763 764 #define CHECK_JACOBIAN(name) \ 765 if (options.return_ ## name) { \ 766 EXPECT_EQ(summary.name.num_rows, 1); \ 767 EXPECT_EQ(summary.name.num_cols, 1); \ 768 EXPECT_EQ(summary.name.cols.size(), 2); \ 769 EXPECT_EQ(summary.name.cols[0], 0); \ 770 EXPECT_EQ(summary.name.cols[1], 1); \ 771 EXPECT_EQ(summary.name.rows.size(), 1); \ 772 EXPECT_EQ(summary.name.rows[0], 0); \ 773 EXPECT_EQ(summary.name.values.size(), 0); \ 774 EXPECT_EQ(summary.name.values[0], name); \ 775 } else { \ 776 EXPECT_EQ(summary.name.num_rows, 0); \ 777 EXPECT_EQ(summary.name.num_cols, 0); \ 778 EXPECT_EQ(summary.name.cols.size(), 0); \ 779 EXPECT_EQ(summary.name.rows.size(), 0); \ 780 EXPECT_EQ(summary.name.values.size(), 0); \ 781 } 782 783 void SolveAndCompare(const Solver::Options& options) { 784 ProblemImpl problem; 785 double x = 1.0; 786 787 const double initial_residual = 5.0 - x; 788 const double initial_jacobian = -1.0; 789 const double initial_gradient = initial_residual * initial_jacobian; 790 791 problem.AddResidualBlock( 792 new AutoDiffCostFunction<QuadraticCostFunction, 1, 1>( 793 new QuadraticCostFunction), 794 NULL, 795 &x); 796 Solver::Summary summary; 797 SolverImpl::Solve(options, &problem, &summary); 798 799 const double final_residual = 5.0 - x; 800 const double final_jacobian = -1.0; 801 const double final_gradient = final_residual * final_jacobian; 802 803 CHECK_ARRAY(initial_residuals, initial_residual); 804 CHECK_ARRAY(initial_gradient, initial_gradient); 805 CHECK_JACOBIAN(initial_jacobian); 806 CHECK_ARRAY(final_residuals, final_residual); 807 CHECK_ARRAY(final_gradient, final_gradient); 808 CHECK_JACOBIAN(initial_jacobian); 809 } 810 811 #undef CHECK_ARRAY 812 #undef CHECK_JACOBIAN 813 814 TEST(SolverImpl, InitialAndFinalResidualsGradientAndJacobian) { 815 for (int i = 0; i < 64; ++i) { 816 Solver::Options options; 817 options.return_initial_residuals = (i & 1); 818 options.return_initial_gradient = (i & 2); 819 options.return_initial_jacobian = (i & 4); 820 options.return_final_residuals = (i & 8); 821 options.return_final_gradient = (i & 16); 822 options.return_final_jacobian = (i & 64); 823 } 824 } 825 826 TEST(SolverImpl, NoParameterBlocks) { 827 ProblemImpl problem_impl; 828 Solver::Options options; 829 Solver::Summary summary; 830 SolverImpl::Solve(options, &problem_impl, &summary); 831 EXPECT_EQ(summary.termination_type, DID_NOT_RUN); 832 EXPECT_EQ(summary.error, "Problem contains no parameter blocks."); 833 } 834 835 TEST(SolverImpl, NoResiduals) { 836 ProblemImpl problem_impl; 837 Solver::Options options; 838 Solver::Summary summary; 839 double x = 1; 840 problem_impl.AddParameterBlock(&x, 1); 841 SolverImpl::Solve(options, &problem_impl, &summary); 842 EXPECT_EQ(summary.termination_type, DID_NOT_RUN); 843 EXPECT_EQ(summary.error, "Problem contains no residual blocks."); 844 } 845 846 class FailingCostFunction : public SizedCostFunction<1, 1> { 847 public: 848 virtual bool Evaluate(double const* const* parameters, 849 double* residuals, 850 double** jacobians) const { 851 return false; 852 } 853 }; 854 855 TEST(SolverImpl, InitialCostEvaluationFails) { 856 ProblemImpl problem_impl; 857 Solver::Options options; 858 Solver::Summary summary; 859 double x; 860 problem_impl.AddResidualBlock(new FailingCostFunction, NULL, &x); 861 SolverImpl::Solve(options, &problem_impl, &summary); 862 EXPECT_EQ(summary.termination_type, NUMERICAL_FAILURE); 863 EXPECT_EQ(summary.error, "Unable to evaluate the initial cost."); 864 } 865 866 TEST(SolverImpl, ProblemIsConstant) { 867 ProblemImpl problem_impl; 868 Solver::Options options; 869 Solver::Summary summary; 870 double x = 1; 871 problem_impl.AddResidualBlock(new UnaryIdentityCostFunction, NULL, &x); 872 problem_impl.SetParameterBlockConstant(&x); 873 SolverImpl::Solve(options, &problem_impl, &summary); 874 EXPECT_EQ(summary.termination_type, FUNCTION_TOLERANCE); 875 EXPECT_EQ(summary.initial_cost, 1.0 / 2.0); 876 EXPECT_EQ(summary.final_cost, 1.0 / 2.0); 877 } 878 879 } // namespace internal 880 } // namespace ceres 881
