1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2013 Google Inc. All rights reserved. 3 // http://code.google.com/p/ceres-solver/ 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are met: 7 // 8 // * Redistributions of source code must retain the above copyright notice, 9 // this list of conditions and the following disclaimer. 10 // * Redistributions in binary form must reproduce the above copyright notice, 11 // this list of conditions and the following disclaimer in the documentation 12 // and/or other materials provided with the distribution. 13 // * Neither the name of Google Inc. nor the names of its contributors may be 14 // used to endorse or promote products derived from this software without 15 // specific prior written permission. 16 // 17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 // POSSIBILITY OF SUCH DAMAGE. 28 // 29 // Author: sameeragarwal (at) google.com (Sameer Agarwal) 30 // mierle (at) gmail.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_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 void ProblemImpl::InternalRemoveResidualBlock(ResidualBlock* residual_block) { 154 CHECK_NOTNULL(residual_block); 155 // Perform no check on the validity of residual_block, that is handled in 156 // the public method: RemoveResidualBlock(). 157 158 // If needed, remove the parameter dependencies on this residual block. 159 if (options_.enable_fast_removal) { 160 const int num_parameter_blocks_for_residual = 161 residual_block->NumParameterBlocks(); 162 for (int i = 0; i < num_parameter_blocks_for_residual; ++i) { 163 residual_block->parameter_blocks()[i] 164 ->RemoveResidualBlock(residual_block); 165 } 166 167 ResidualBlockSet::iterator it = residual_block_set_.find(residual_block); 168 residual_block_set_.erase(it); 169 } 170 DeleteBlockInVector(program_->mutable_residual_blocks(), residual_block); 171 } 172 173 // Deletes the residual block in question, assuming there are no other 174 // references to it inside the problem (e.g. by another parameter). Referenced 175 // cost and loss functions are tucked away for future deletion, since it is not 176 // possible to know whether other parts of the problem depend on them without 177 // doing a full scan. 178 void ProblemImpl::DeleteBlock(ResidualBlock* residual_block) { 179 // The const casts here are legit, since ResidualBlock holds these 180 // pointers as const pointers but we have ownership of them and 181 // have the right to destroy them when the destructor is called. 182 if (options_.cost_function_ownership == TAKE_OWNERSHIP && 183 residual_block->cost_function() != NULL) { 184 cost_functions_to_delete_.push_back( 185 const_cast<CostFunction*>(residual_block->cost_function())); 186 } 187 if (options_.loss_function_ownership == TAKE_OWNERSHIP && 188 residual_block->loss_function() != NULL) { 189 loss_functions_to_delete_.push_back( 190 const_cast<LossFunction*>(residual_block->loss_function())); 191 } 192 delete residual_block; 193 } 194 195 // Deletes the parameter block in question, assuming there are no other 196 // references to it inside the problem (e.g. by any residual blocks). 197 // Referenced parameterizations are tucked away for future deletion, since it 198 // is not possible to know whether other parts of the problem depend on them 199 // without doing a full scan. 200 void ProblemImpl::DeleteBlock(ParameterBlock* parameter_block) { 201 if (options_.local_parameterization_ownership == TAKE_OWNERSHIP && 202 parameter_block->local_parameterization() != NULL) { 203 local_parameterizations_to_delete_.push_back( 204 parameter_block->mutable_local_parameterization()); 205 } 206 parameter_block_map_.erase(parameter_block->mutable_user_state()); 207 delete parameter_block; 208 } 209 210 ProblemImpl::ProblemImpl() : program_(new internal::Program) {} 211 ProblemImpl::ProblemImpl(const Problem::Options& options) 212 : options_(options), 213 program_(new internal::Program) {} 214 215 ProblemImpl::~ProblemImpl() { 216 // Collect the unique cost/loss functions and delete the residuals. 217 const int num_residual_blocks = program_->residual_blocks_.size(); 218 cost_functions_to_delete_.reserve(num_residual_blocks); 219 loss_functions_to_delete_.reserve(num_residual_blocks); 220 for (int i = 0; i < program_->residual_blocks_.size(); ++i) { 221 DeleteBlock(program_->residual_blocks_[i]); 222 } 223 224 // Collect the unique parameterizations and delete the parameters. 225 for (int i = 0; i < program_->parameter_blocks_.size(); ++i) { 226 DeleteBlock(program_->parameter_blocks_[i]); 227 } 228 229 // Delete the owned cost/loss functions and parameterizations. 230 STLDeleteUniqueContainerPointers(local_parameterizations_to_delete_.begin(), 231 local_parameterizations_to_delete_.end()); 232 STLDeleteUniqueContainerPointers(cost_functions_to_delete_.begin(), 233 cost_functions_to_delete_.end()); 234 STLDeleteUniqueContainerPointers(loss_functions_to_delete_.begin(), 235 loss_functions_to_delete_.end()); 236 } 237 238 ResidualBlock* ProblemImpl::AddResidualBlock( 239 CostFunction* cost_function, 240 LossFunction* loss_function, 241 const vector<double*>& parameter_blocks) { 242 CHECK_NOTNULL(cost_function); 243 CHECK_EQ(parameter_blocks.size(), 244 cost_function->parameter_block_sizes().size()); 245 246 // Check the sizes match. 247 const vector<int32>& parameter_block_sizes = 248 cost_function->parameter_block_sizes(); 249 250 if (!options_.disable_all_safety_checks) { 251 CHECK_EQ(parameter_block_sizes.size(), parameter_blocks.size()) 252 << "Number of blocks input is different than the number of blocks " 253 << "that the cost function expects."; 254 255 // Check for duplicate parameter blocks. 256 vector<double*> sorted_parameter_blocks(parameter_blocks); 257 sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end()); 258 vector<double*>::const_iterator duplicate_items = 259 unique(sorted_parameter_blocks.begin(), 260 sorted_parameter_blocks.end()); 261 if (duplicate_items != sorted_parameter_blocks.end()) { 262 string blocks; 263 for (int i = 0; i < parameter_blocks.size(); ++i) { 264 blocks += StringPrintf(" %p ", parameter_blocks[i]); 265 } 266 267 LOG(FATAL) << "Duplicate parameter blocks in a residual parameter " 268 << "are not allowed. Parameter block pointers: [" 269 << blocks << "]"; 270 } 271 } 272 273 // Add parameter blocks and convert the double*'s to parameter blocks. 274 vector<ParameterBlock*> parameter_block_ptrs(parameter_blocks.size()); 275 for (int i = 0; i < parameter_blocks.size(); ++i) { 276 parameter_block_ptrs[i] = 277 InternalAddParameterBlock(parameter_blocks[i], 278 parameter_block_sizes[i]); 279 } 280 281 if (!options_.disable_all_safety_checks) { 282 // Check that the block sizes match the block sizes expected by the 283 // cost_function. 284 for (int i = 0; i < parameter_block_ptrs.size(); ++i) { 285 CHECK_EQ(cost_function->parameter_block_sizes()[i], 286 parameter_block_ptrs[i]->Size()) 287 << "The cost function expects parameter block " << i 288 << " of size " << cost_function->parameter_block_sizes()[i] 289 << " but was given a block of size " 290 << parameter_block_ptrs[i]->Size(); 291 } 292 } 293 294 ResidualBlock* new_residual_block = 295 new ResidualBlock(cost_function, 296 loss_function, 297 parameter_block_ptrs, 298 program_->residual_blocks_.size()); 299 300 // Add dependencies on the residual to the parameter blocks. 301 if (options_.enable_fast_removal) { 302 for (int i = 0; i < parameter_blocks.size(); ++i) { 303 parameter_block_ptrs[i]->AddResidualBlock(new_residual_block); 304 } 305 } 306 307 program_->residual_blocks_.push_back(new_residual_block); 308 309 if (options_.enable_fast_removal) { 310 residual_block_set_.insert(new_residual_block); 311 } 312 313 return new_residual_block; 314 } 315 316 // Unfortunately, macros don't help much to reduce this code, and var args don't 317 // work because of the ambiguous case that there is no loss function. 318 ResidualBlock* ProblemImpl::AddResidualBlock( 319 CostFunction* cost_function, 320 LossFunction* loss_function, 321 double* x0) { 322 vector<double*> residual_parameters; 323 residual_parameters.push_back(x0); 324 return AddResidualBlock(cost_function, loss_function, residual_parameters); 325 } 326 327 ResidualBlock* ProblemImpl::AddResidualBlock( 328 CostFunction* cost_function, 329 LossFunction* loss_function, 330 double* x0, double* x1) { 331 vector<double*> residual_parameters; 332 residual_parameters.push_back(x0); 333 residual_parameters.push_back(x1); 334 return AddResidualBlock(cost_function, loss_function, residual_parameters); 335 } 336 337 ResidualBlock* ProblemImpl::AddResidualBlock( 338 CostFunction* cost_function, 339 LossFunction* loss_function, 340 double* x0, double* x1, double* x2) { 341 vector<double*> residual_parameters; 342 residual_parameters.push_back(x0); 343 residual_parameters.push_back(x1); 344 residual_parameters.push_back(x2); 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) { 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 return AddResidualBlock(cost_function, loss_function, residual_parameters); 358 } 359 360 ResidualBlock* ProblemImpl::AddResidualBlock( 361 CostFunction* cost_function, 362 LossFunction* loss_function, 363 double* x0, double* x1, double* x2, double* x3, double* x4) { 364 vector<double*> residual_parameters; 365 residual_parameters.push_back(x0); 366 residual_parameters.push_back(x1); 367 residual_parameters.push_back(x2); 368 residual_parameters.push_back(x3); 369 residual_parameters.push_back(x4); 370 return AddResidualBlock(cost_function, loss_function, residual_parameters); 371 } 372 373 ResidualBlock* ProblemImpl::AddResidualBlock( 374 CostFunction* cost_function, 375 LossFunction* loss_function, 376 double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) { 377 vector<double*> residual_parameters; 378 residual_parameters.push_back(x0); 379 residual_parameters.push_back(x1); 380 residual_parameters.push_back(x2); 381 residual_parameters.push_back(x3); 382 residual_parameters.push_back(x4); 383 residual_parameters.push_back(x5); 384 return AddResidualBlock(cost_function, loss_function, residual_parameters); 385 } 386 387 ResidualBlock* ProblemImpl::AddResidualBlock( 388 CostFunction* cost_function, 389 LossFunction* loss_function, 390 double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, 391 double* x6) { 392 vector<double*> residual_parameters; 393 residual_parameters.push_back(x0); 394 residual_parameters.push_back(x1); 395 residual_parameters.push_back(x2); 396 residual_parameters.push_back(x3); 397 residual_parameters.push_back(x4); 398 residual_parameters.push_back(x5); 399 residual_parameters.push_back(x6); 400 return AddResidualBlock(cost_function, loss_function, residual_parameters); 401 } 402 403 ResidualBlock* ProblemImpl::AddResidualBlock( 404 CostFunction* cost_function, 405 LossFunction* loss_function, 406 double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, 407 double* x6, double* x7) { 408 vector<double*> residual_parameters; 409 residual_parameters.push_back(x0); 410 residual_parameters.push_back(x1); 411 residual_parameters.push_back(x2); 412 residual_parameters.push_back(x3); 413 residual_parameters.push_back(x4); 414 residual_parameters.push_back(x5); 415 residual_parameters.push_back(x6); 416 residual_parameters.push_back(x7); 417 return AddResidualBlock(cost_function, loss_function, residual_parameters); 418 } 419 420 ResidualBlock* ProblemImpl::AddResidualBlock( 421 CostFunction* cost_function, 422 LossFunction* loss_function, 423 double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, 424 double* x6, double* x7, double* x8) { 425 vector<double*> residual_parameters; 426 residual_parameters.push_back(x0); 427 residual_parameters.push_back(x1); 428 residual_parameters.push_back(x2); 429 residual_parameters.push_back(x3); 430 residual_parameters.push_back(x4); 431 residual_parameters.push_back(x5); 432 residual_parameters.push_back(x6); 433 residual_parameters.push_back(x7); 434 residual_parameters.push_back(x8); 435 return AddResidualBlock(cost_function, loss_function, residual_parameters); 436 } 437 438 ResidualBlock* ProblemImpl::AddResidualBlock( 439 CostFunction* cost_function, 440 LossFunction* loss_function, 441 double* x0, double* x1, double* x2, double* x3, double* x4, double* x5, 442 double* x6, double* x7, double* x8, double* x9) { 443 vector<double*> residual_parameters; 444 residual_parameters.push_back(x0); 445 residual_parameters.push_back(x1); 446 residual_parameters.push_back(x2); 447 residual_parameters.push_back(x3); 448 residual_parameters.push_back(x4); 449 residual_parameters.push_back(x5); 450 residual_parameters.push_back(x6); 451 residual_parameters.push_back(x7); 452 residual_parameters.push_back(x8); 453 residual_parameters.push_back(x9); 454 return AddResidualBlock(cost_function, loss_function, residual_parameters); 455 } 456 457 void ProblemImpl::AddParameterBlock(double* values, int size) { 458 InternalAddParameterBlock(values, size); 459 } 460 461 void ProblemImpl::AddParameterBlock( 462 double* values, 463 int size, 464 LocalParameterization* local_parameterization) { 465 ParameterBlock* parameter_block = 466 InternalAddParameterBlock(values, size); 467 if (local_parameterization != NULL) { 468 parameter_block->SetParameterization(local_parameterization); 469 } 470 } 471 472 // Delete a block from a vector of blocks, maintaining the indexing invariant. 473 // This is done in constant time by moving an element from the end of the 474 // vector over the element to remove, then popping the last element. It 475 // destroys the ordering in the interest of speed. 476 template<typename Block> 477 void ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks, 478 Block* block_to_remove) { 479 CHECK_EQ((*mutable_blocks)[block_to_remove->index()], block_to_remove) 480 << "You found a Ceres bug! \n" 481 << "Block requested: " 482 << block_to_remove->ToString() << "\n" 483 << "Block present: " 484 << (*mutable_blocks)[block_to_remove->index()]->ToString(); 485 486 // Prepare the to-be-moved block for the new, lower-in-index position by 487 // setting the index to the blocks final location. 488 Block* tmp = mutable_blocks->back(); 489 tmp->set_index(block_to_remove->index()); 490 491 // Overwrite the to-be-deleted residual block with the one at the end. 492 (*mutable_blocks)[block_to_remove->index()] = tmp; 493 494 DeleteBlock(block_to_remove); 495 496 // The block is gone so shrink the vector of blocks accordingly. 497 mutable_blocks->pop_back(); 498 } 499 500 void ProblemImpl::RemoveResidualBlock(ResidualBlock* residual_block) { 501 CHECK_NOTNULL(residual_block); 502 503 // Verify that residual_block identifies a residual in the current problem. 504 const string residual_not_found_message = 505 StringPrintf("Residual block to remove: %p not found. This usually means " 506 "one of three things have happened:\n" 507 " 1) residual_block is uninitialised and points to a random " 508 "area in memory.\n" 509 " 2) residual_block represented a residual that was added to" 510 " the problem, but referred to a parameter block which has " 511 "since been removed, which removes all residuals which " 512 "depend on that parameter block, and was thus removed.\n" 513 " 3) residual_block referred to a residual that has already " 514 "been removed from the problem (by the user).", 515 residual_block); 516 if (options_.enable_fast_removal) { 517 CHECK(residual_block_set_.find(residual_block) != 518 residual_block_set_.end()) 519 << residual_not_found_message; 520 } else { 521 // Perform a full search over all current residuals. 522 CHECK(std::find(program_->residual_blocks().begin(), 523 program_->residual_blocks().end(), 524 residual_block) != program_->residual_blocks().end()) 525 << residual_not_found_message; 526 } 527 528 InternalRemoveResidualBlock(residual_block); 529 } 530 531 void ProblemImpl::RemoveParameterBlock(double* values) { 532 ParameterBlock* parameter_block = 533 FindParameterBlockOrDie(parameter_block_map_, values); 534 535 if (options_.enable_fast_removal) { 536 // Copy the dependent residuals from the parameter block because the set of 537 // dependents will change after each call to RemoveResidualBlock(). 538 vector<ResidualBlock*> residual_blocks_to_remove( 539 parameter_block->mutable_residual_blocks()->begin(), 540 parameter_block->mutable_residual_blocks()->end()); 541 for (int i = 0; i < residual_blocks_to_remove.size(); ++i) { 542 InternalRemoveResidualBlock(residual_blocks_to_remove[i]); 543 } 544 } else { 545 // Scan all the residual blocks to remove ones that depend on the parameter 546 // block. Do the scan backwards since the vector changes while iterating. 547 const int num_residual_blocks = NumResidualBlocks(); 548 for (int i = num_residual_blocks - 1; i >= 0; --i) { 549 ResidualBlock* residual_block = 550 (*(program_->mutable_residual_blocks()))[i]; 551 const int num_parameter_blocks = residual_block->NumParameterBlocks(); 552 for (int j = 0; j < num_parameter_blocks; ++j) { 553 if (residual_block->parameter_blocks()[j] == parameter_block) { 554 InternalRemoveResidualBlock(residual_block); 555 // The parameter blocks are guaranteed unique. 556 break; 557 } 558 } 559 } 560 } 561 DeleteBlockInVector(program_->mutable_parameter_blocks(), parameter_block); 562 } 563 564 void ProblemImpl::SetParameterBlockConstant(double* values) { 565 FindParameterBlockOrDie(parameter_block_map_, values)->SetConstant(); 566 } 567 568 void ProblemImpl::SetParameterBlockVariable(double* values) { 569 FindParameterBlockOrDie(parameter_block_map_, values)->SetVarying(); 570 } 571 572 void ProblemImpl::SetParameterization( 573 double* values, 574 LocalParameterization* local_parameterization) { 575 FindParameterBlockOrDie(parameter_block_map_, values) 576 ->SetParameterization(local_parameterization); 577 } 578 579 const LocalParameterization* ProblemImpl::GetParameterization( 580 double* values) const { 581 return FindParameterBlockOrDie(parameter_block_map_, values) 582 ->local_parameterization(); 583 } 584 585 void ProblemImpl::SetParameterLowerBound(double* values, 586 int index, 587 double lower_bound) { 588 FindParameterBlockOrDie(parameter_block_map_, values) 589 ->SetLowerBound(index, lower_bound); 590 } 591 592 void ProblemImpl::SetParameterUpperBound(double* values, 593 int index, 594 double upper_bound) { 595 FindParameterBlockOrDie(parameter_block_map_, values) 596 ->SetUpperBound(index, upper_bound); 597 } 598 599 bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options, 600 double* cost, 601 vector<double>* residuals, 602 vector<double>* gradient, 603 CRSMatrix* jacobian) { 604 if (cost == NULL && 605 residuals == NULL && 606 gradient == NULL && 607 jacobian == NULL) { 608 LOG(INFO) << "Nothing to do."; 609 return true; 610 } 611 612 // If the user supplied residual blocks, then use them, otherwise 613 // take the residual blocks from the underlying program. 614 Program program; 615 *program.mutable_residual_blocks() = 616 ((evaluate_options.residual_blocks.size() > 0) 617 ? evaluate_options.residual_blocks : program_->residual_blocks()); 618 619 const vector<double*>& parameter_block_ptrs = 620 evaluate_options.parameter_blocks; 621 622 vector<ParameterBlock*> variable_parameter_blocks; 623 vector<ParameterBlock*>& parameter_blocks = 624 *program.mutable_parameter_blocks(); 625 626 if (parameter_block_ptrs.size() == 0) { 627 // The user did not provide any parameter blocks, so default to 628 // using all the parameter blocks in the order that they are in 629 // the underlying program object. 630 parameter_blocks = program_->parameter_blocks(); 631 } else { 632 // The user supplied a vector of parameter blocks. Using this list 633 // requires a number of steps. 634 635 // 1. Convert double* into ParameterBlock* 636 parameter_blocks.resize(parameter_block_ptrs.size()); 637 for (int i = 0; i < parameter_block_ptrs.size(); ++i) { 638 parameter_blocks[i] = 639 FindParameterBlockOrDie(parameter_block_map_, 640 parameter_block_ptrs[i]); 641 } 642 643 // 2. The user may have only supplied a subset of parameter 644 // blocks, so identify the ones that are not supplied by the user 645 // and are NOT constant. These parameter blocks are stored in 646 // variable_parameter_blocks. 647 // 648 // To ensure that the parameter blocks are not included in the 649 // columns of the jacobian, we need to make sure that they are 650 // constant during evaluation and then make them variable again 651 // after we are done. 652 vector<ParameterBlock*> all_parameter_blocks(program_->parameter_blocks()); 653 vector<ParameterBlock*> included_parameter_blocks( 654 program.parameter_blocks()); 655 656 vector<ParameterBlock*> excluded_parameter_blocks; 657 sort(all_parameter_blocks.begin(), all_parameter_blocks.end()); 658 sort(included_parameter_blocks.begin(), included_parameter_blocks.end()); 659 set_difference(all_parameter_blocks.begin(), 660 all_parameter_blocks.end(), 661 included_parameter_blocks.begin(), 662 included_parameter_blocks.end(), 663 back_inserter(excluded_parameter_blocks)); 664 665 variable_parameter_blocks.reserve(excluded_parameter_blocks.size()); 666 for (int i = 0; i < excluded_parameter_blocks.size(); ++i) { 667 ParameterBlock* parameter_block = excluded_parameter_blocks[i]; 668 if (!parameter_block->IsConstant()) { 669 variable_parameter_blocks.push_back(parameter_block); 670 parameter_block->SetConstant(); 671 } 672 } 673 } 674 675 // Setup the Parameter indices and offsets before an evaluator can 676 // be constructed and used. 677 program.SetParameterOffsetsAndIndex(); 678 679 Evaluator::Options evaluator_options; 680 681 // Even though using SPARSE_NORMAL_CHOLESKY requires SuiteSparse or 682 // CXSparse, here it just being used for telling the evaluator to 683 // use a SparseRowCompressedMatrix for the jacobian. This is because 684 // the Evaluator decides the storage for the Jacobian based on the 685 // type of linear solver being used. 686 evaluator_options.linear_solver_type = SPARSE_NORMAL_CHOLESKY; 687 evaluator_options.num_threads = evaluate_options.num_threads; 688 689 string error; 690 scoped_ptr<Evaluator> evaluator( 691 Evaluator::Create(evaluator_options, &program, &error)); 692 if (evaluator.get() == NULL) { 693 LOG(ERROR) << "Unable to create an Evaluator object. " 694 << "Error: " << error 695 << "This is a Ceres bug; please contact the developers!"; 696 697 // Make the parameter blocks that were temporarily marked 698 // constant, variable again. 699 for (int i = 0; i < variable_parameter_blocks.size(); ++i) { 700 variable_parameter_blocks[i]->SetVarying(); 701 } 702 703 program_->SetParameterBlockStatePtrsToUserStatePtrs(); 704 program_->SetParameterOffsetsAndIndex(); 705 return false; 706 } 707 708 if (residuals !=NULL) { 709 residuals->resize(evaluator->NumResiduals()); 710 } 711 712 if (gradient != NULL) { 713 gradient->resize(evaluator->NumEffectiveParameters()); 714 } 715 716 scoped_ptr<CompressedRowSparseMatrix> tmp_jacobian; 717 if (jacobian != NULL) { 718 tmp_jacobian.reset( 719 down_cast<CompressedRowSparseMatrix*>(evaluator->CreateJacobian())); 720 } 721 722 // Point the state pointers to the user state pointers. This is 723 // needed so that we can extract a parameter vector which is then 724 // passed to Evaluator::Evaluate. 725 program.SetParameterBlockStatePtrsToUserStatePtrs(); 726 727 // Copy the value of the parameter blocks into a vector, since the 728 // Evaluate::Evaluate method needs its input as such. The previous 729 // call to SetParameterBlockStatePtrsToUserStatePtrs ensures that 730 // these values are the ones corresponding to the actual state of 731 // the parameter blocks, rather than the temporary state pointer 732 // used for evaluation. 733 Vector parameters(program.NumParameters()); 734 program.ParameterBlocksToStateVector(parameters.data()); 735 736 double tmp_cost = 0; 737 738 Evaluator::EvaluateOptions evaluator_evaluate_options; 739 evaluator_evaluate_options.apply_loss_function = 740 evaluate_options.apply_loss_function; 741 bool status = evaluator->Evaluate(evaluator_evaluate_options, 742 parameters.data(), 743 &tmp_cost, 744 residuals != NULL ? &(*residuals)[0] : NULL, 745 gradient != NULL ? &(*gradient)[0] : NULL, 746 tmp_jacobian.get()); 747 748 // Make the parameter blocks that were temporarily marked constant, 749 // variable again. 750 for (int i = 0; i < variable_parameter_blocks.size(); ++i) { 751 variable_parameter_blocks[i]->SetVarying(); 752 } 753 754 if (status) { 755 if (cost != NULL) { 756 *cost = tmp_cost; 757 } 758 if (jacobian != NULL) { 759 tmp_jacobian->ToCRSMatrix(jacobian); 760 } 761 } 762 763 program_->SetParameterBlockStatePtrsToUserStatePtrs(); 764 program_->SetParameterOffsetsAndIndex(); 765 return status; 766 } 767 768 int ProblemImpl::NumParameterBlocks() const { 769 return program_->NumParameterBlocks(); 770 } 771 772 int ProblemImpl::NumParameters() const { 773 return program_->NumParameters(); 774 } 775 776 int ProblemImpl::NumResidualBlocks() const { 777 return program_->NumResidualBlocks(); 778 } 779 780 int ProblemImpl::NumResiduals() const { 781 return program_->NumResiduals(); 782 } 783 784 int ProblemImpl::ParameterBlockSize(const double* parameter_block) const { 785 return FindParameterBlockOrDie(parameter_block_map_, 786 const_cast<double*>(parameter_block))->Size(); 787 }; 788 789 int ProblemImpl::ParameterBlockLocalSize(const double* parameter_block) const { 790 return FindParameterBlockOrDie( 791 parameter_block_map_, const_cast<double*>(parameter_block))->LocalSize(); 792 }; 793 794 bool ProblemImpl::HasParameterBlock(const double* parameter_block) const { 795 return (parameter_block_map_.find(const_cast<double*>(parameter_block)) != 796 parameter_block_map_.end()); 797 } 798 799 void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const { 800 CHECK_NOTNULL(parameter_blocks); 801 parameter_blocks->resize(0); 802 for (ParameterMap::const_iterator it = parameter_block_map_.begin(); 803 it != parameter_block_map_.end(); 804 ++it) { 805 parameter_blocks->push_back(it->first); 806 } 807 } 808 809 void ProblemImpl::GetResidualBlocks( 810 vector<ResidualBlockId>* residual_blocks) const { 811 CHECK_NOTNULL(residual_blocks); 812 *residual_blocks = program().residual_blocks(); 813 } 814 815 void ProblemImpl::GetParameterBlocksForResidualBlock( 816 const ResidualBlockId residual_block, 817 vector<double*>* parameter_blocks) const { 818 int num_parameter_blocks = residual_block->NumParameterBlocks(); 819 CHECK_NOTNULL(parameter_blocks)->resize(num_parameter_blocks); 820 for (int i = 0; i < num_parameter_blocks; ++i) { 821 (*parameter_blocks)[i] = 822 residual_block->parameter_blocks()[i]->mutable_user_state(); 823 } 824 } 825 826 void ProblemImpl::GetResidualBlocksForParameterBlock( 827 const double* values, 828 vector<ResidualBlockId>* residual_blocks) const { 829 ParameterBlock* parameter_block = 830 FindParameterBlockOrDie(parameter_block_map_, 831 const_cast<double*>(values)); 832 833 if (options_.enable_fast_removal) { 834 // In this case the residual blocks that depend on the parameter block are 835 // stored in the parameter block already, so just copy them out. 836 CHECK_NOTNULL(residual_blocks)->resize( 837 parameter_block->mutable_residual_blocks()->size()); 838 std::copy(parameter_block->mutable_residual_blocks()->begin(), 839 parameter_block->mutable_residual_blocks()->end(), 840 residual_blocks->begin()); 841 return; 842 } 843 844 // Find residual blocks that depend on the parameter block. 845 CHECK_NOTNULL(residual_blocks)->clear(); 846 const int num_residual_blocks = NumResidualBlocks(); 847 for (int i = 0; i < num_residual_blocks; ++i) { 848 ResidualBlock* residual_block = 849 (*(program_->mutable_residual_blocks()))[i]; 850 const int num_parameter_blocks = residual_block->NumParameterBlocks(); 851 for (int j = 0; j < num_parameter_blocks; ++j) { 852 if (residual_block->parameter_blocks()[j] == parameter_block) { 853 residual_blocks->push_back(residual_block); 854 // The parameter blocks are guaranteed unique. 855 break; 856 } 857 } 858 } 859 } 860 861 } // namespace internal 862 } // namespace ceres 863
