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_impl.h" 33 34 #include <algorithm> 35 #include <cstddef> 36 #include <iterator> 37 #include <set> 38 #include <string> 39 #include <utility> 40 #include <vector> 41 #include "ceres/casts.h" 42 #include "ceres/compressed_row_sparse_matrix.h" 43 #include "ceres/cost_function.h" 44 #include "ceres/crs_matrix.h" 45 #include "ceres/evaluator.h" 46 #include "ceres/loss_function.h" 47 #include "ceres/map_util.h" 48 #include "ceres/parameter_block.h" 49 #include "ceres/program.h" 50 #include "ceres/residual_block.h" 51 #include "ceres/stl_util.h" 52 #include "ceres/stringprintf.h" 53 #include "glog/logging.h" 54 55 namespace ceres { 56 namespace internal { 57 58 typedef map<double*, internal::ParameterBlock*> ParameterMap; 59 60 namespace { 61 internal::ParameterBlock* FindParameterBlockOrDie( 62 const ParameterMap& parameter_map, 63 double* parameter_block) { 64 ParameterMap::const_iterator it = parameter_map.find(parameter_block); 65 CHECK(it != parameter_map.end()) 66 << "Parameter block not found: " << parameter_block; 67 return it->second; 68 } 69 70 // Returns true if two regions of memory, a and b, with sizes size_a and size_b 71 // respectively, overlap. 72 bool RegionsAlias(const double* a, int size_a, 73 const double* b, int size_b) { 74 return (a < b) ? b < (a + size_a) 75 : a < (b + size_b); 76 } 77 78 void CheckForNoAliasing(double* existing_block, 79 int existing_block_size, 80 double* new_block, 81 int new_block_size) { 82 CHECK(!RegionsAlias(existing_block, existing_block_size, 83 new_block, new_block_size)) 84 << "Aliasing detected between existing parameter block at memory " 85 << "location " << existing_block 86 << " and has size " << existing_block_size << " with new parameter " 87 << "block that has memory address " << new_block << " and would have " 88 << "size " << new_block_size << "."; 89 } 90 91 } // namespace 92 93 ParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values, 94 int size) { 95 CHECK(values != NULL) << "Null pointer passed to AddParameterBlock " 96 << "for a parameter with size " << size; 97 98 // Ignore the request if there is a block for the given pointer already. 99 ParameterMap::iterator it = parameter_block_map_.find(values); 100 if (it != parameter_block_map_.end()) { 101 if (!options_.disable_all_safety_checks) { 102 int existing_size = it->second->Size(); 103 CHECK(size == existing_size) 104 << "Tried adding a parameter block with the same double pointer, " 105 << values << ", twice, but with different block sizes. Original " 106 << "size was " << existing_size << " but new size is " 107 << size; 108 } 109 return it->second; 110 } 111 112 if (!options_.disable_all_safety_checks) { 113 // Before adding the parameter block, also check that it doesn't alias any 114 // other parameter blocks. 115 if (!parameter_block_map_.empty()) { 116 ParameterMap::iterator lb = parameter_block_map_.lower_bound(values); 117 118 // If lb is not the first block, check the previous block for aliasing. 119 if (lb != parameter_block_map_.begin()) { 120 ParameterMap::iterator previous = lb; 121 --previous; 122 CheckForNoAliasing(previous->first, 123 previous->second->Size(), 124 values, 125 size); 126 } 127 128 // If lb is not off the end, check lb for aliasing. 129 if (lb != parameter_block_map_.end()) { 130 CheckForNoAliasing(lb->first, 131 lb->second->Size(), 132 values, 133 size); 134 } 135 } 136 } 137 138 // Pass the index of the new parameter block as well to keep the index in 139 // sync with the position of the parameter in the program's parameter vector. 140 ParameterBlock* new_parameter_block = 141 new ParameterBlock(values, size, program_->parameter_blocks_.size()); 142 143 // For dynamic problems, add the list of dependent residual blocks, which is 144 // empty to start. 145 if (options_.enable_fast_parameter_block_removal) { 146 new_parameter_block->EnableResidualBlockDependencies(); 147 } 148 parameter_block_map_[values] = new_parameter_block; 149 program_->parameter_blocks_.push_back(new_parameter_block); 150 return new_parameter_block; 151 } 152 153 // Deletes the residual block in question, assuming there are no other 154 // references to it inside the problem (e.g. by another parameter). Referenced 155 // cost and loss functions are tucked away for future deletion, since it is not 156 // possible to know whether other parts of the problem depend on them without 157 // doing a full scan. 158 void ProblemImpl::DeleteBlock(ResidualBlock* residual_block) { 159 // The const casts here are legit, since ResidualBlock holds these 160 // pointers as const pointers but we have ownership of them and 161 // have the right to destroy them when the destructor is called. 162 if (options_.cost_function_ownership == TAKE_OWNERSHIP && 163 residual_block->cost_function() != NULL) { 164 cost_functions_to_delete_.push_back( 165 const_cast<CostFunction*>(residual_block->cost_function())); 166 } 167 if (options_.loss_function_ownership == TAKE_OWNERSHIP && 168 residual_block->loss_function() != NULL) { 169 loss_functions_to_delete_.push_back( 170 const_cast<LossFunction*>(residual_block->loss_function())); 171 } 172 delete residual_block; 173 } 174 175 // Deletes the parameter block in question, assuming there are no other 176 // references to it inside the problem (e.g. by any residual blocks). 177 // Referenced parameterizations are tucked away for future deletion, since it 178 // is not possible to know whether other parts of the problem depend on them 179 // without doing a full scan. 180 void ProblemImpl::DeleteBlock(ParameterBlock* parameter_block) { 181 if (options_.local_parameterization_ownership == TAKE_OWNERSHIP && 182 parameter_block->local_parameterization() != NULL) { 183 local_parameterizations_to_delete_.push_back( 184 parameter_block->mutable_local_parameterization()); 185 } 186 parameter_block_map_.erase(parameter_block->mutable_user_state()); 187 delete parameter_block; 188 } 189 190 ProblemImpl::ProblemImpl() : program_(new internal::Program) {} 191 ProblemImpl::ProblemImpl(const Problem::Options& options) 192 : options_(options), 193 program_(new internal::Program) {} 194 195 ProblemImpl::~ProblemImpl() { 196 // Collect the unique cost/loss functions and delete the residuals. 197 const int num_residual_blocks = program_->residual_blocks_.size(); 198 cost_functions_to_delete_.reserve(num_residual_blocks); 199 loss_functions_to_delete_.reserve(num_residual_blocks); 200 for (int i = 0; i < program_->residual_blocks_.size(); ++i) { 201 DeleteBlock(program_->residual_blocks_[i]); 202 } 203 204 // Collect the unique parameterizations and delete the parameters. 205 for (int i = 0; i < program_->parameter_blocks_.size(); ++i) { 206 DeleteBlock(program_->parameter_blocks_[i]); 207 } 208 209 // Delete the owned cost/loss functions and parameterizations. 210 STLDeleteUniqueContainerPointers(local_parameterizations_to_delete_.begin(), 211 local_parameterizations_to_delete_.end()); 212 STLDeleteUniqueContainerPointers(cost_functions_to_delete_.begin(), 213 cost_functions_to_delete_.end()); 214 STLDeleteUniqueContainerPointers(loss_functions_to_delete_.begin(), 215 loss_functions_to_delete_.end()); 216 } 217 218 ResidualBlock* ProblemImpl::AddResidualBlock( 219 CostFunction* cost_function, 220 LossFunction* loss_function, 221 const vector<double*>& parameter_blocks) { 222 CHECK_NOTNULL(cost_function); 223 CHECK_EQ(parameter_blocks.size(), 224 cost_function->parameter_block_sizes().size()); 225 226 // Check the sizes match. 227 const vector<int16>& parameter_block_sizes = 228 cost_function->parameter_block_sizes(); 229 230 if (!options_.disable_all_safety_checks) { 231 CHECK_EQ(parameter_block_sizes.size(), parameter_blocks.size()) 232 << "Number of blocks input is different than the number of blocks " 233 << "that the cost function expects."; 234 235 // Check for duplicate parameter blocks. 236 vector<double*> sorted_parameter_blocks(parameter_blocks); 237 sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end()); 238 vector<double*>::const_iterator duplicate_items = 239 unique(sorted_parameter_blocks.begin(), 240 sorted_parameter_blocks.end()); 241 if (duplicate_items != sorted_parameter_blocks.end()) { 242 string blocks; 243 for (int i = 0; i < parameter_blocks.size(); ++i) { 244 blocks += StringPrintf(" %p ", parameter_blocks[i]); 245 } 246 247 LOG(FATAL) << "Duplicate parameter blocks in a residual parameter " 248 << "are not allowed. Parameter block pointers: [" 249 << blocks << "]"; 250 } 251 } 252 253 // Add parameter blocks and convert the double*'s to parameter blocks. 254 vector<ParameterBlock*> parameter_block_ptrs(parameter_blocks.size()); 255 for (int i = 0; i < parameter_blocks.size(); ++i) { 256 parameter_block_ptrs[i] = 257 InternalAddParameterBlock(parameter_blocks[i], 258 parameter_block_sizes[i]); 259 } 260 261 if (!options_.disable_all_safety_checks) { 262 // Check that the block sizes match the block sizes expected by the 263 // cost_function. 264 for (int i = 0; i < parameter_block_ptrs.size(); ++i) { 265 CHECK_EQ(cost_function->parameter_block_sizes()[i], 266 parameter_block_ptrs[i]->Size()) 267 << "The cost function expects parameter block " << i 268 << " of size " << cost_function->parameter_block_sizes()[i] 269 << " but was given a block of size " 270 << parameter_block_ptrs[i]->Size(); 271 } 272 } 273 274 ResidualBlock* new_residual_block = 275 new ResidualBlock(cost_function, 276 loss_function, 277 parameter_block_ptrs, 278 program_->residual_blocks_.size()); 279 280 // Add dependencies on the residual to the parameter blocks. 281 if (options_.enable_fast_parameter_block_removal) { 282 for (int i = 0; i < parameter_blocks.size(); ++i) { 283 parameter_block_ptrs[i]->AddResidualBlock(new_residual_block); 284 } 285 } 286 287 program_->residual_blocks_.push_back(new_residual_block); 288 return new_residual_block; 289 } 290 291 // Unfortunately, macros don't help much to reduce this code, and var args don't 292 // work because of the ambiguous case that there is no loss function. 293 ResidualBlock* ProblemImpl::AddResidualBlock( 294 CostFunction* cost_function, 295 LossFunction* loss_function, 296 double* x0) { 297 vector<double*> residual_parameters; 298 residual_parameters.push_back(x0); 299 return AddResidualBlock(cost_function, loss_function, residual_parameters); 300 } 301 302 ResidualBlock* ProblemImpl::AddResidualBlock( 303 CostFunction* cost_function, 304 LossFunction* loss_function, 305 double* x0, double* x1) { 306 vector<double*> residual_parameters; 307 residual_parameters.push_back(x0); 308 residual_parameters.push_back(x1); 309 return AddResidualBlock(cost_function, loss_function, residual_parameters); 310 } 311 312 ResidualBlock* ProblemImpl::AddResidualBlock( 313 CostFunction* cost_function, 314 LossFunction* loss_function, 315 double* x0, double* x1, double* x2) { 316 vector<double*> residual_parameters; 317 residual_parameters.push_back(x0); 318 residual_parameters.push_back(x1); 319 residual_parameters.push_back(x2); 320 return AddResidualBlock(cost_function, loss_function, residual_parameters); 321 } 322 323 ResidualBlock* ProblemImpl::AddResidualBlock( 324 CostFunction* cost_function, 325 LossFunction* loss_function, 326 double* x0, double* x1, double* x2, double* x3) { 327 vector<double*> residual_parameters; 328 residual_parameters.push_back(x0); 329 residual_parameters.push_back(x1); 330 residual_parameters.push_back(x2); 331 residual_parameters.push_back(x3); 332 return AddResidualBlock(cost_function, loss_function, residual_parameters); 333 } 334 335 ResidualBlock* ProblemImpl::AddResidualBlock( 336 CostFunction* cost_function, 337 LossFunction* loss_function, 338 double* x0, double* x1, double* x2, double* x3, double* x4) { 339 vector<double*> residual_parameters; 340 residual_parameters.push_back(x0); 341 residual_parameters.push_back(x1); 342 residual_parameters.push_back(x2); 343 residual_parameters.push_back(x3); 344 residual_parameters.push_back(x4); 345 return AddResidualBlock(cost_function, loss_function, residual_parameters); 346 } 347 348 ResidualBlock* ProblemImpl::AddResidualBlock( 349 CostFunction* cost_function, 350 LossFunction* loss_function, 351 double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) { 352 vector<double*> residual_parameters; 353 residual_parameters.push_back(x0); 354 residual_parameters.push_back(x1); 355 residual_parameters.push_back(x2); 356 residual_parameters.push_back(x3); 357 residual_parameters.push_back(x4); 358 residual_parameters.push_back(x5); 359 return AddResidualBlock(cost_function, loss_function, residual_parameters); 360 } 361 362 ResidualBlock* ProblemImpl::AddResidualBlock( 363 CostFunction* cost_function, 364 LossFunction* loss_function, 365 double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, 366 double* x6) { 367 vector<double*> residual_parameters; 368 residual_parameters.push_back(x0); 369 residual_parameters.push_back(x1); 370 residual_parameters.push_back(x2); 371 residual_parameters.push_back(x3); 372 residual_parameters.push_back(x4); 373 residual_parameters.push_back(x5); 374 residual_parameters.push_back(x6); 375 return AddResidualBlock(cost_function, loss_function, residual_parameters); 376 } 377 378 ResidualBlock* ProblemImpl::AddResidualBlock( 379 CostFunction* cost_function, 380 LossFunction* loss_function, 381 double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, 382 double* x6, double* x7) { 383 vector<double*> residual_parameters; 384 residual_parameters.push_back(x0); 385 residual_parameters.push_back(x1); 386 residual_parameters.push_back(x2); 387 residual_parameters.push_back(x3); 388 residual_parameters.push_back(x4); 389 residual_parameters.push_back(x5); 390 residual_parameters.push_back(x6); 391 residual_parameters.push_back(x7); 392 return AddResidualBlock(cost_function, loss_function, residual_parameters); 393 } 394 395 ResidualBlock* ProblemImpl::AddResidualBlock( 396 CostFunction* cost_function, 397 LossFunction* loss_function, 398 double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, 399 double* x6, double* x7, double* x8) { 400 vector<double*> residual_parameters; 401 residual_parameters.push_back(x0); 402 residual_parameters.push_back(x1); 403 residual_parameters.push_back(x2); 404 residual_parameters.push_back(x3); 405 residual_parameters.push_back(x4); 406 residual_parameters.push_back(x5); 407 residual_parameters.push_back(x6); 408 residual_parameters.push_back(x7); 409 residual_parameters.push_back(x8); 410 return AddResidualBlock(cost_function, loss_function, residual_parameters); 411 } 412 413 ResidualBlock* ProblemImpl::AddResidualBlock( 414 CostFunction* cost_function, 415 LossFunction* loss_function, 416 double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, 417 double* x6, double* x7, double* x8, double* x9) { 418 vector<double*> residual_parameters; 419 residual_parameters.push_back(x0); 420 residual_parameters.push_back(x1); 421 residual_parameters.push_back(x2); 422 residual_parameters.push_back(x3); 423 residual_parameters.push_back(x4); 424 residual_parameters.push_back(x5); 425 residual_parameters.push_back(x6); 426 residual_parameters.push_back(x7); 427 residual_parameters.push_back(x8); 428 residual_parameters.push_back(x9); 429 return AddResidualBlock(cost_function, loss_function, residual_parameters); 430 } 431 432 void ProblemImpl::AddParameterBlock(double* values, int size) { 433 InternalAddParameterBlock(values, size); 434 } 435 436 void ProblemImpl::AddParameterBlock( 437 double* values, 438 int size, 439 LocalParameterization* local_parameterization) { 440 ParameterBlock* parameter_block = 441 InternalAddParameterBlock(values, size); 442 if (local_parameterization != NULL) { 443 parameter_block->SetParameterization(local_parameterization); 444 } 445 } 446 447 // Delete a block from a vector of blocks, maintaining the indexing invariant. 448 // This is done in constant time by moving an element from the end of the 449 // vector over the element to remove, then popping the last element. It 450 // destroys the ordering in the interest of speed. 451 template<typename Block> 452 void ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks, 453 Block* block_to_remove) { 454 CHECK_EQ((*mutable_blocks)[block_to_remove->index()], block_to_remove) 455 << "You found a Ceres bug! Block: " << block_to_remove->ToString(); 456 457 // Prepare the to-be-moved block for the new, lower-in-index position by 458 // setting the index to the blocks final location. 459 Block* tmp = mutable_blocks->back(); 460 tmp->set_index(block_to_remove->index()); 461 462 // Overwrite the to-be-deleted residual block with the one at the end. 463 (*mutable_blocks)[block_to_remove->index()] = tmp; 464 465 DeleteBlock(block_to_remove); 466 467 // The block is gone so shrink the vector of blocks accordingly. 468 mutable_blocks->pop_back(); 469 } 470 471 void ProblemImpl::RemoveResidualBlock(ResidualBlock* residual_block) { 472 CHECK_NOTNULL(residual_block); 473 474 // If needed, remove the parameter dependencies on this residual block. 475 if (options_.enable_fast_parameter_block_removal) { 476 const int num_parameter_blocks_for_residual = 477 residual_block->NumParameterBlocks(); 478 for (int i = 0; i < num_parameter_blocks_for_residual; ++i) { 479 residual_block->parameter_blocks()[i] 480 ->RemoveResidualBlock(residual_block); 481 } 482 } 483 DeleteBlockInVector(program_->mutable_residual_blocks(), residual_block); 484 } 485 486 void ProblemImpl::RemoveParameterBlock(double* values) { 487 ParameterBlock* parameter_block = 488 FindParameterBlockOrDie(parameter_block_map_, values); 489 490 if (options_.enable_fast_parameter_block_removal) { 491 // Copy the dependent residuals from the parameter block because the set of 492 // dependents will change after each call to RemoveResidualBlock(). 493 vector<ResidualBlock*> residual_blocks_to_remove( 494 parameter_block->mutable_residual_blocks()->begin(), 495 parameter_block->mutable_residual_blocks()->end()); 496 for (int i = 0; i < residual_blocks_to_remove.size(); ++i) { 497 RemoveResidualBlock(residual_blocks_to_remove[i]); 498 } 499 } else { 500 // Scan all the residual blocks to remove ones that depend on the parameter 501 // block. Do the scan backwards since the vector changes while iterating. 502 const int num_residual_blocks = NumResidualBlocks(); 503 for (int i = num_residual_blocks - 1; i >= 0; --i) { 504 ResidualBlock* residual_block = 505 (*(program_->mutable_residual_blocks()))[i]; 506 const int num_parameter_blocks = residual_block->NumParameterBlocks(); 507 for (int j = 0; j < num_parameter_blocks; ++j) { 508 if (residual_block->parameter_blocks()[j] == parameter_block) { 509 RemoveResidualBlock(residual_block); 510 // The parameter blocks are guaranteed unique. 511 break; 512 } 513 } 514 } 515 } 516 DeleteBlockInVector(program_->mutable_parameter_blocks(), parameter_block); 517 } 518 519 void ProblemImpl::SetParameterBlockConstant(double* values) { 520 FindParameterBlockOrDie(parameter_block_map_, values)->SetConstant(); 521 } 522 523 void ProblemImpl::SetParameterBlockVariable(double* values) { 524 FindParameterBlockOrDie(parameter_block_map_, values)->SetVarying(); 525 } 526 527 void ProblemImpl::SetParameterization( 528 double* values, 529 LocalParameterization* local_parameterization) { 530 FindParameterBlockOrDie(parameter_block_map_, values) 531 ->SetParameterization(local_parameterization); 532 } 533 534 bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, 535 double* cost, 536 vector<double>* residuals, 537 vector<double>* gradient, 538 CRSMatrix* jacobian) { 539 if (cost == NULL && 540 residuals == NULL && 541 gradient == NULL && 542 jacobian == NULL) { 543 LOG(INFO) << "Nothing to do."; 544 return true; 545 } 546 547 // If the user supplied residual blocks, then use them, otherwise 548 // take the residual blocks from the underlying program. 549 Program program; 550 *program.mutable_residual_blocks() = 551 ((evaluate_options.residual_blocks.size() > 0) 552 ? evaluate_options.residual_blocks : program_->residual_blocks()); 553 554 const vector<double*>& parameter_block_ptrs = 555 evaluate_options.parameter_blocks; 556 557 vector<ParameterBlock*> variable_parameter_blocks; 558 vector<ParameterBlock*>& parameter_blocks = 559 *program.mutable_parameter_blocks(); 560 561 if (parameter_block_ptrs.size() == 0) { 562 // The user did not provide any parameter blocks, so default to 563 // using all the parameter blocks in the order that they are in 564 // the underlying program object. 565 parameter_blocks = program_->parameter_blocks(); 566 } else { 567 // The user supplied a vector of parameter blocks. Using this list 568 // requires a number of steps. 569 570 // 1. Convert double* into ParameterBlock* 571 parameter_blocks.resize(parameter_block_ptrs.size()); 572 for (int i = 0; i < parameter_block_ptrs.size(); ++i) { 573 parameter_blocks[i] = 574 FindParameterBlockOrDie(parameter_block_map_, 575 parameter_block_ptrs[i]); 576 } 577 578 // 2. The user may have only supplied a subset of parameter 579 // blocks, so identify the ones that are not supplied by the user 580 // and are NOT constant. These parameter blocks are stored in 581 // variable_parameter_blocks. 582 // 583 // To ensure that the parameter blocks are not included in the 584 // columns of the jacobian, we need to make sure that they are 585 // constant during evaluation and then make them variable again 586 // after we are done. 587 vector<ParameterBlock*> all_parameter_blocks(program_->parameter_blocks()); 588 vector<ParameterBlock*> included_parameter_blocks( 589 program.parameter_blocks()); 590 591 vector<ParameterBlock*> excluded_parameter_blocks; 592 sort(all_parameter_blocks.begin(), all_parameter_blocks.end()); 593 sort(included_parameter_blocks.begin(), included_parameter_blocks.end()); 594 set_difference(all_parameter_blocks.begin(), 595 all_parameter_blocks.end(), 596 included_parameter_blocks.begin(), 597 included_parameter_blocks.end(), 598 back_inserter(excluded_parameter_blocks)); 599 600 variable_parameter_blocks.reserve(excluded_parameter_blocks.size()); 601 for (int i = 0; i < excluded_parameter_blocks.size(); ++i) { 602 ParameterBlock* parameter_block = excluded_parameter_blocks[i]; 603 if (!parameter_block->IsConstant()) { 604 variable_parameter_blocks.push_back(parameter_block); 605 parameter_block->SetConstant(); 606 } 607 } 608 } 609 610 // Setup the Parameter indices and offsets before an evaluator can 611 // be constructed and used. 612 program.SetParameterOffsetsAndIndex(); 613 614 Evaluator::Options evaluator_options; 615 616 // Even though using SPARSE_NORMAL_CHOLESKY requires SuiteSparse or 617 // CXSparse, here it just being used for telling the evaluator to 618 // use a SparseRowCompressedMatrix for the jacobian. This is because 619 // the Evaluator decides the storage for the Jacobian based on the 620 // type of linear solver being used. 621 evaluator_options.linear_solver_type = SPARSE_NORMAL_CHOLESKY; 622 evaluator_options.num_threads = evaluate_options.num_threads; 623 624 string error; 625 scoped_ptr<Evaluator> evaluator( 626 Evaluator::Create(evaluator_options, &program, &error)); 627 if (evaluator.get() == NULL) { 628 LOG(ERROR) << "Unable to create an Evaluator object. " 629 << "Error: " << error 630 << "This is a Ceres bug; please contact the developers!"; 631 632 // Make the parameter blocks that were temporarily marked 633 // constant, variable again. 634 for (int i = 0; i < variable_parameter_blocks.size(); ++i) { 635 variable_parameter_blocks[i]->SetVarying(); 636 } 637 return false; 638 } 639 640 if (residuals !=NULL) { 641 residuals->resize(evaluator->NumResiduals()); 642 } 643 644 if (gradient != NULL) { 645 gradient->resize(evaluator->NumEffectiveParameters()); 646 } 647 648 scoped_ptr<CompressedRowSparseMatrix> tmp_jacobian; 649 if (jacobian != NULL) { 650 tmp_jacobian.reset( 651 down_cast<CompressedRowSparseMatrix*>(evaluator->CreateJacobian())); 652 } 653 654 // Point the state pointers to the user state pointers. This is 655 // needed so that we can extract a parameter vector which is then 656 // passed to Evaluator::Evaluate. 657 program.SetParameterBlockStatePtrsToUserStatePtrs(); 658 659 // Copy the value of the parameter blocks into a vector, since the 660 // Evaluate::Evaluate method needs its input as such. The previous 661 // call to SetParameterBlockStatePtrsToUserStatePtrs ensures that 662 // these values are the ones corresponding to the actual state of 663 // the parameter blocks, rather than the temporary state pointer 664 // used for evaluation. 665 Vector parameters(program.NumParameters()); 666 program.ParameterBlocksToStateVector(parameters.data()); 667 668 double tmp_cost = 0; 669 670 Evaluator::EvaluateOptions evaluator_evaluate_options; 671 evaluator_evaluate_options.apply_loss_function = 672 evaluate_options.apply_loss_function; 673 bool status = evaluator->Evaluate(evaluator_evaluate_options, 674 parameters.data(), 675 &tmp_cost, 676 residuals != NULL ? &(*residuals)[0] : NULL, 677 gradient != NULL ? &(*gradient)[0] : NULL, 678 tmp_jacobian.get()); 679 680 // Make the parameter blocks that were temporarily marked constant, 681 // variable again. 682 for (int i = 0; i < variable_parameter_blocks.size(); ++i) { 683 variable_parameter_blocks[i]->SetVarying(); 684 } 685 686 if (status) { 687 if (cost != NULL) { 688 *cost = tmp_cost; 689 } 690 if (jacobian != NULL) { 691 tmp_jacobian->ToCRSMatrix(jacobian); 692 } 693 } 694 695 return status; 696 } 697 698 int ProblemImpl::NumParameterBlocks() const { 699 return program_->NumParameterBlocks(); 700 } 701 702 int ProblemImpl::NumParameters() const { 703 return program_->NumParameters(); 704 } 705 706 int ProblemImpl::NumResidualBlocks() const { 707 return program_->NumResidualBlocks(); 708 } 709 710 int ProblemImpl::NumResiduals() const { 711 return program_->NumResiduals(); 712 } 713 714 int ProblemImpl::ParameterBlockSize(const double* parameter_block) const { 715 return FindParameterBlockOrDie(parameter_block_map_, 716 const_cast<double*>(parameter_block))->Size(); 717 }; 718 719 int ProblemImpl::ParameterBlockLocalSize(const double* parameter_block) const { 720 return FindParameterBlockOrDie( 721 parameter_block_map_, const_cast<double*>(parameter_block))->LocalSize(); 722 }; 723 724 void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const { 725 CHECK_NOTNULL(parameter_blocks); 726 parameter_blocks->resize(0); 727 for (ParameterMap::const_iterator it = parameter_block_map_.begin(); 728 it != parameter_block_map_.end(); 729 ++it) { 730 parameter_blocks->push_back(it->first); 731 } 732 } 733 734 735 } // namespace internal 736 } // namespace ceres 737
