Home | History | Annotate | Download | only in ceres
      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