| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011 | // Ceres Solver - A fast non-linear least squares minimizer// Copyright 2015 Google Inc. All rights reserved.// http://ceres-solver.org///// Redistribution and use in source and binary forms, with or without// modification, are permitted provided that the following conditions are met://// * Redistributions of source code must retain the above copyright notice,//   this list of conditions and the following disclaimer.// * Redistributions in binary form must reproduce the above copyright notice,//   this list of conditions and the following disclaimer in the documentation//   and/or other materials provided with the distribution.// * Neither the name of Google Inc. nor the names of its contributors may be//   used to endorse or promote products derived from this software without//   specific prior written permission.//// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE// POSSIBILITY OF SUCH DAMAGE.//// Author: sameeragarwal@google.com (Sameer Agarwal)//         mierle@gmail.com (Keir Mierle)#include "ceres/problem_impl.h"#include <algorithm>#include <cstddef>#include <iterator>#include <set>#include <string>#include <utility>#include <vector>#include "ceres/casts.h"#include "ceres/compressed_row_jacobian_writer.h"#include "ceres/compressed_row_sparse_matrix.h"#include "ceres/context_impl.h"#include "ceres/cost_function.h"#include "ceres/crs_matrix.h"#include "ceres/evaluator.h"#include "ceres/internal/port.h"#include "ceres/loss_function.h"#include "ceres/map_util.h"#include "ceres/parameter_block.h"#include "ceres/program.h"#include "ceres/program_evaluator.h"#include "ceres/residual_block.h"#include "ceres/scratch_evaluate_preparer.h"#include "ceres/stl_util.h"#include "ceres/stringprintf.h"#include "glog/logging.h"namespace ceres {namespace internal {using std::map;using std::string;using std::vector;namespace {// Returns true if two regions of memory, a and b, with sizes size_a and size_b// respectively, overlap.bool RegionsAlias(const double* a, int size_a,                  const double* b, int size_b) {  return (a < b) ? b < (a + size_a)                 : a < (b + size_b);}void CheckForNoAliasing(double* existing_block,                        int existing_block_size,                        double* new_block,                        int new_block_size) {  CHECK(!RegionsAlias(existing_block, existing_block_size,                      new_block, new_block_size))      << "Aliasing detected between existing parameter block at memory "      << "location " << existing_block      << " and has size " << existing_block_size << " with new parameter "      << "block that has memory address " << new_block << " and would have "      << "size " << new_block_size << ".";}template <typename KeyType>void DecrementValueOrDeleteKey(const KeyType key,                               std::map<KeyType, int>* container) {  auto it = container->find(key);  if (it->second == 1) {    delete key;    container->erase(it);  } else {    --it->second;  }}template <typename ForwardIterator>void STLDeleteContainerPairFirstPointers(ForwardIterator begin,                                         ForwardIterator end) {  while (begin != end) {    delete begin->first;    ++begin;  }}void InitializeContext(Context* context,                       ContextImpl** context_impl,                       bool* context_impl_owned) {  if (context == NULL) {    *context_impl_owned = true;    *context_impl = new ContextImpl;  } else {    *context_impl_owned = false;    *context_impl = down_cast<ContextImpl*>(context);  }}}  // namespaceParameterBlock* ProblemImpl::InternalAddParameterBlock(double* values,                                                       int size) {  CHECK(values != NULL) << "Null pointer passed to AddParameterBlock "                        << "for a parameter with size " << size;  // Ignore the request if there is a block for the given pointer already.  ParameterMap::iterator it = parameter_block_map_.find(values);  if (it != parameter_block_map_.end()) {    if (!options_.disable_all_safety_checks) {      int existing_size = it->second->Size();      CHECK(size == existing_size)          << "Tried adding a parameter block with the same double pointer, "          << values << ", twice, but with different block sizes. Original "          << "size was " << existing_size << " but new size is "          << size;    }    return it->second;  }  if (!options_.disable_all_safety_checks) {    // Before adding the parameter block, also check that it doesn't alias any    // other parameter blocks.    if (!parameter_block_map_.empty()) {      ParameterMap::iterator lb = parameter_block_map_.lower_bound(values);      // If lb is not the first block, check the previous block for aliasing.      if (lb != parameter_block_map_.begin()) {        ParameterMap::iterator previous = lb;        --previous;        CheckForNoAliasing(previous->first,                           previous->second->Size(),                           values,                           size);      }      // If lb is not off the end, check lb for aliasing.      if (lb != parameter_block_map_.end()) {        CheckForNoAliasing(lb->first,                           lb->second->Size(),                           values,                           size);      }    }  }  // Pass the index of the new parameter block as well to keep the index in  // sync with the position of the parameter in the program's parameter vector.  ParameterBlock* new_parameter_block =      new ParameterBlock(values, size, program_->parameter_blocks_.size());  // For dynamic problems, add the list of dependent residual blocks, which is  // empty to start.  if (options_.enable_fast_removal) {    new_parameter_block->EnableResidualBlockDependencies();  }  parameter_block_map_[values] = new_parameter_block;  program_->parameter_blocks_.push_back(new_parameter_block);  return new_parameter_block;}void ProblemImpl::InternalRemoveResidualBlock(ResidualBlock* residual_block) {  CHECK_NOTNULL(residual_block);  // Perform no check on the validity of residual_block, that is handled in  // the public method: RemoveResidualBlock().  // If needed, remove the parameter dependencies on this residual block.  if (options_.enable_fast_removal) {    const int num_parameter_blocks_for_residual =        residual_block->NumParameterBlocks();    for (int i = 0; i < num_parameter_blocks_for_residual; ++i) {      residual_block->parameter_blocks()[i]          ->RemoveResidualBlock(residual_block);    }    ResidualBlockSet::iterator it = residual_block_set_.find(residual_block);    residual_block_set_.erase(it);  }  DeleteBlockInVector(program_->mutable_residual_blocks(), residual_block);}// Deletes the residual block in question, assuming there are no other// references to it inside the problem (e.g. by another parameter). Referenced// cost and loss functions are tucked away for future deletion, since it is not// possible to know whether other parts of the problem depend on them without// doing a full scan.void ProblemImpl::DeleteBlock(ResidualBlock* residual_block) {  // The const casts here are legit, since ResidualBlock holds these  // pointers as const pointers but we have ownership of them and  // have the right to destroy them when the destructor is called.  CostFunction* cost_function =      const_cast<CostFunction*>(residual_block->cost_function());  if (options_.cost_function_ownership == TAKE_OWNERSHIP) {    DecrementValueOrDeleteKey(cost_function, &cost_function_ref_count_);  }  LossFunction* loss_function =      const_cast<LossFunction*>(residual_block->loss_function());  if (options_.loss_function_ownership == TAKE_OWNERSHIP &&      loss_function != NULL) {    DecrementValueOrDeleteKey(loss_function, &loss_function_ref_count_);  }  delete residual_block;}// Deletes the parameter block in question, assuming there are no other// references to it inside the problem (e.g. by any residual blocks).// Referenced parameterizations are tucked away for future deletion, since it// is not possible to know whether other parts of the problem depend on them// without doing a full scan.void ProblemImpl::DeleteBlock(ParameterBlock* parameter_block) {  if (options_.local_parameterization_ownership == TAKE_OWNERSHIP &&      parameter_block->local_parameterization() != NULL) {    local_parameterizations_to_delete_.push_back(        parameter_block->mutable_local_parameterization());  }  parameter_block_map_.erase(parameter_block->mutable_user_state());  delete parameter_block;}ProblemImpl::ProblemImpl()    : options_(Problem::Options()),      program_(new internal::Program) {  residual_parameters_.reserve(10);  InitializeContext(options_.context, &context_impl_, &context_impl_owned_);}ProblemImpl::ProblemImpl(const Problem::Options& options)    : options_(options),      program_(new internal::Program) {  residual_parameters_.reserve(10);  InitializeContext(options_.context, &context_impl_, &context_impl_owned_);}ProblemImpl::~ProblemImpl() {  STLDeleteContainerPointers(program_->residual_blocks_.begin(),                             program_->residual_blocks_.end());  if (options_.cost_function_ownership == TAKE_OWNERSHIP) {    STLDeleteContainerPairFirstPointers(cost_function_ref_count_.begin(),                                        cost_function_ref_count_.end());  }  if (options_.loss_function_ownership == TAKE_OWNERSHIP) {    STLDeleteContainerPairFirstPointers(loss_function_ref_count_.begin(),                                        loss_function_ref_count_.end());  }  // Collect the unique parameterizations and delete the parameters.  for (int i = 0; i < program_->parameter_blocks_.size(); ++i) {    DeleteBlock(program_->parameter_blocks_[i]);  }  // Delete the owned parameterizations.  STLDeleteUniqueContainerPointers(local_parameterizations_to_delete_.begin(),                                   local_parameterizations_to_delete_.end());  if (context_impl_owned_) {    delete context_impl_;  }}ResidualBlock* ProblemImpl::AddResidualBlock(    CostFunction* cost_function,    LossFunction* loss_function,    const vector<double*>& parameter_blocks) {  CHECK_NOTNULL(cost_function);  CHECK_EQ(parameter_blocks.size(),           cost_function->parameter_block_sizes().size());  // Check the sizes match.  const vector<int32>& parameter_block_sizes =      cost_function->parameter_block_sizes();  if (!options_.disable_all_safety_checks) {    CHECK_EQ(parameter_block_sizes.size(), parameter_blocks.size())        << "Number of blocks input is different than the number of blocks "        << "that the cost function expects.";    // Check for duplicate parameter blocks.    vector<double*> sorted_parameter_blocks(parameter_blocks);    sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end());    const bool has_duplicate_items =        (std::adjacent_find(sorted_parameter_blocks.begin(),                            sorted_parameter_blocks.end())         != sorted_parameter_blocks.end());    if (has_duplicate_items) {      string blocks;      for (int i = 0; i < parameter_blocks.size(); ++i) {        blocks += StringPrintf(" %p ", parameter_blocks[i]);      }      LOG(FATAL) << "Duplicate parameter blocks in a residual parameter "                 << "are not allowed. Parameter block pointers: ["                 << blocks << "]";    }  }  // Add parameter blocks and convert the double*'s to parameter blocks.  vector<ParameterBlock*> parameter_block_ptrs(parameter_blocks.size());  for (int i = 0; i < parameter_blocks.size(); ++i) {    parameter_block_ptrs[i] =        InternalAddParameterBlock(parameter_blocks[i],                                  parameter_block_sizes[i]);  }  if (!options_.disable_all_safety_checks) {    // Check that the block sizes match the block sizes expected by the    // cost_function.    for (int i = 0; i < parameter_block_ptrs.size(); ++i) {      CHECK_EQ(cost_function->parameter_block_sizes()[i],               parameter_block_ptrs[i]->Size())          << "The cost function expects parameter block " << i          << " of size " << cost_function->parameter_block_sizes()[i]          << " but was given a block of size "          << parameter_block_ptrs[i]->Size();    }  }  ResidualBlock* new_residual_block =      new ResidualBlock(cost_function,                        loss_function,                        parameter_block_ptrs,                        program_->residual_blocks_.size());  // Add dependencies on the residual to the parameter blocks.  if (options_.enable_fast_removal) {    for (int i = 0; i < parameter_blocks.size(); ++i) {      parameter_block_ptrs[i]->AddResidualBlock(new_residual_block);    }  }  program_->residual_blocks_.push_back(new_residual_block);  if (options_.enable_fast_removal) {    residual_block_set_.insert(new_residual_block);  }  if (options_.cost_function_ownership == TAKE_OWNERSHIP) {    // Increment the reference count, creating an entry in the table if    // needed. Note: C++ maps guarantee that new entries have default    // constructed values; this implies integers are zero initialized.    ++cost_function_ref_count_[cost_function];  }  if (options_.loss_function_ownership == TAKE_OWNERSHIP &&      loss_function != NULL) {    ++loss_function_ref_count_[loss_function];  }  return new_residual_block;}// Unfortunately, macros don't help much to reduce this code, and var args don't// work because of the ambiguous case that there is no loss function.ResidualBlock* ProblemImpl::AddResidualBlock(    CostFunction* cost_function,    LossFunction* loss_function,    double* x0) {  residual_parameters_.clear();  residual_parameters_.push_back(x0);  return AddResidualBlock(cost_function, loss_function, residual_parameters_);}ResidualBlock* ProblemImpl::AddResidualBlock(    CostFunction* cost_function,    LossFunction* loss_function,    double* x0, double* x1) {  residual_parameters_.clear();  residual_parameters_.push_back(x0);  residual_parameters_.push_back(x1);  return AddResidualBlock(cost_function, loss_function, residual_parameters_);}ResidualBlock* ProblemImpl::AddResidualBlock(    CostFunction* cost_function,    LossFunction* loss_function,    double* x0, double* x1, double* x2) {  residual_parameters_.clear();  residual_parameters_.push_back(x0);  residual_parameters_.push_back(x1);  residual_parameters_.push_back(x2);  return AddResidualBlock(cost_function, loss_function, residual_parameters_);}ResidualBlock* ProblemImpl::AddResidualBlock(    CostFunction* cost_function,    LossFunction* loss_function,    double* x0, double* x1, double* x2, double* x3) {  residual_parameters_.clear();  residual_parameters_.push_back(x0);  residual_parameters_.push_back(x1);  residual_parameters_.push_back(x2);  residual_parameters_.push_back(x3);  return AddResidualBlock(cost_function, loss_function, residual_parameters_);}ResidualBlock* ProblemImpl::AddResidualBlock(    CostFunction* cost_function,    LossFunction* loss_function,    double* x0, double* x1, double* x2, double* x3, double* x4) {  residual_parameters_.clear();  residual_parameters_.push_back(x0);  residual_parameters_.push_back(x1);  residual_parameters_.push_back(x2);  residual_parameters_.push_back(x3);  residual_parameters_.push_back(x4);  return AddResidualBlock(cost_function, loss_function, residual_parameters_);}ResidualBlock* ProblemImpl::AddResidualBlock(    CostFunction* cost_function,    LossFunction* loss_function,    double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) {  residual_parameters_.clear();  residual_parameters_.push_back(x0);  residual_parameters_.push_back(x1);  residual_parameters_.push_back(x2);  residual_parameters_.push_back(x3);  residual_parameters_.push_back(x4);  residual_parameters_.push_back(x5);  return AddResidualBlock(cost_function, loss_function, residual_parameters_);}ResidualBlock* ProblemImpl::AddResidualBlock(    CostFunction* cost_function,    LossFunction* loss_function,    double* x0, double* x1, double* x2, double* x3, double* x4, double* x5,    double* x6) {  residual_parameters_.clear();  residual_parameters_.push_back(x0);  residual_parameters_.push_back(x1);  residual_parameters_.push_back(x2);  residual_parameters_.push_back(x3);  residual_parameters_.push_back(x4);  residual_parameters_.push_back(x5);  residual_parameters_.push_back(x6);  return AddResidualBlock(cost_function, loss_function, residual_parameters_);}ResidualBlock* ProblemImpl::AddResidualBlock(    CostFunction* cost_function,    LossFunction* loss_function,    double* x0, double* x1, double* x2, double* x3, double* x4, double* x5,    double* x6, double* x7) {  residual_parameters_.clear();  residual_parameters_.push_back(x0);  residual_parameters_.push_back(x1);  residual_parameters_.push_back(x2);  residual_parameters_.push_back(x3);  residual_parameters_.push_back(x4);  residual_parameters_.push_back(x5);  residual_parameters_.push_back(x6);  residual_parameters_.push_back(x7);  return AddResidualBlock(cost_function, loss_function, residual_parameters_);}ResidualBlock* ProblemImpl::AddResidualBlock(    CostFunction* cost_function,    LossFunction* loss_function,    double* x0, double* x1, double* x2, double* x3, double* x4, double* x5,    double* x6, double* x7, double* x8) {  residual_parameters_.clear();  residual_parameters_.push_back(x0);  residual_parameters_.push_back(x1);  residual_parameters_.push_back(x2);  residual_parameters_.push_back(x3);  residual_parameters_.push_back(x4);  residual_parameters_.push_back(x5);  residual_parameters_.push_back(x6);  residual_parameters_.push_back(x7);  residual_parameters_.push_back(x8);  return AddResidualBlock(cost_function, loss_function, residual_parameters_);}ResidualBlock* ProblemImpl::AddResidualBlock(    CostFunction* cost_function,    LossFunction* loss_function,    double* x0, double* x1, double* x2, double* x3, double* x4, double* x5,    double* x6, double* x7, double* x8, double* x9) {  residual_parameters_.clear();  residual_parameters_.push_back(x0);  residual_parameters_.push_back(x1);  residual_parameters_.push_back(x2);  residual_parameters_.push_back(x3);  residual_parameters_.push_back(x4);  residual_parameters_.push_back(x5);  residual_parameters_.push_back(x6);  residual_parameters_.push_back(x7);  residual_parameters_.push_back(x8);  residual_parameters_.push_back(x9);  return AddResidualBlock(cost_function, loss_function, residual_parameters_);}void ProblemImpl::AddParameterBlock(double* values, int size) {  InternalAddParameterBlock(values, size);}void ProblemImpl::AddParameterBlock(    double* values,    int size,    LocalParameterization* local_parameterization) {  ParameterBlock* parameter_block =      InternalAddParameterBlock(values, size);  if (local_parameterization != NULL) {    parameter_block->SetParameterization(local_parameterization);  }}// Delete a block from a vector of blocks, maintaining the indexing invariant.// This is done in constant time by moving an element from the end of the// vector over the element to remove, then popping the last element. It// destroys the ordering in the interest of speed.template<typename Block>void ProblemImpl::DeleteBlockInVector(vector<Block*>* mutable_blocks,                                      Block* block_to_remove) {  CHECK_EQ((*mutable_blocks)[block_to_remove->index()], block_to_remove)      << "You found a Ceres bug! \n"      << "Block requested: "      << block_to_remove->ToString() << "\n"      << "Block present: "      << (*mutable_blocks)[block_to_remove->index()]->ToString();  // Prepare the to-be-moved block for the new, lower-in-index position by  // setting the index to the blocks final location.  Block* tmp = mutable_blocks->back();  tmp->set_index(block_to_remove->index());  // Overwrite the to-be-deleted residual block with the one at the end.  (*mutable_blocks)[block_to_remove->index()] = tmp;  DeleteBlock(block_to_remove);  // The block is gone so shrink the vector of blocks accordingly.  mutable_blocks->pop_back();}void ProblemImpl::RemoveResidualBlock(ResidualBlock* residual_block) {  CHECK_NOTNULL(residual_block);  // Verify that residual_block identifies a residual in the current problem.  const string residual_not_found_message =      StringPrintf("Residual block to remove: %p not found. This usually means "                   "one of three things have happened:\n"                   " 1) residual_block is uninitialised and points to a random "                   "area in memory.\n"                   " 2) residual_block represented a residual that was added to"                   " the problem, but referred to a parameter block which has "                   "since been removed, which removes all residuals which "                   "depend on that parameter block, and was thus removed.\n"                   " 3) residual_block referred to a residual that has already "                   "been removed from the problem (by the user).",                   residual_block);  if (options_.enable_fast_removal) {    CHECK(residual_block_set_.find(residual_block) !=          residual_block_set_.end())        << residual_not_found_message;  } else {    // Perform a full search over all current residuals.    CHECK(std::find(program_->residual_blocks().begin(),                    program_->residual_blocks().end(),                    residual_block) != program_->residual_blocks().end())        << residual_not_found_message;  }  InternalRemoveResidualBlock(residual_block);}void ProblemImpl::RemoveParameterBlock(double* values) {  ParameterBlock* parameter_block =      FindWithDefault(parameter_block_map_, values, NULL);  if (parameter_block == NULL) {    LOG(FATAL) << "Parameter block not found: " << values               << ". You must add the parameter block to the problem before "               << "it can be removed.";  }  if (options_.enable_fast_removal) {    // Copy the dependent residuals from the parameter block because the set of    // dependents will change after each call to RemoveResidualBlock().    vector<ResidualBlock*> residual_blocks_to_remove(        parameter_block->mutable_residual_blocks()->begin(),        parameter_block->mutable_residual_blocks()->end());    for (int i = 0; i < residual_blocks_to_remove.size(); ++i) {      InternalRemoveResidualBlock(residual_blocks_to_remove[i]);    }  } else {    // Scan all the residual blocks to remove ones that depend on the parameter    // block. Do the scan backwards since the vector changes while iterating.    const int num_residual_blocks = NumResidualBlocks();    for (int i = num_residual_blocks - 1; i >= 0; --i) {      ResidualBlock* residual_block =          (*(program_->mutable_residual_blocks()))[i];      const int num_parameter_blocks = residual_block->NumParameterBlocks();      for (int j = 0; j < num_parameter_blocks; ++j) {        if (residual_block->parameter_blocks()[j] == parameter_block) {          InternalRemoveResidualBlock(residual_block);          // The parameter blocks are guaranteed unique.          break;        }      }    }  }  DeleteBlockInVector(program_->mutable_parameter_blocks(), parameter_block);}void ProblemImpl::SetParameterBlockConstant(double* values) {  ParameterBlock* parameter_block =      FindWithDefault(parameter_block_map_, values, NULL);  if (parameter_block == NULL) {    LOG(FATAL) << "Parameter block not found: " << values               << ". You must add the parameter block to the problem before "               << "it can be set constant.";  }  parameter_block->SetConstant();}bool ProblemImpl::IsParameterBlockConstant(double* values) const {  const ParameterBlock* parameter_block =      FindWithDefault(parameter_block_map_, values, NULL);  CHECK(parameter_block != NULL)    << "Parameter block not found: " << values << ". You must add the "    << "parameter block to the problem before it can be queried.";  return parameter_block->IsConstant();}void ProblemImpl::SetParameterBlockVariable(double* values) {  ParameterBlock* parameter_block =      FindWithDefault(parameter_block_map_, values, NULL);  if (parameter_block == NULL) {    LOG(FATAL) << "Parameter block not found: " << values               << ". You must add the parameter block to the problem before "               << "it can be set varying.";  }  parameter_block->SetVarying();}void ProblemImpl::SetParameterization(    double* values,    LocalParameterization* local_parameterization) {  ParameterBlock* parameter_block =      FindWithDefault(parameter_block_map_, values, NULL);  if (parameter_block == NULL) {    LOG(FATAL) << "Parameter block not found: " << values               << ". You must add the parameter block to the problem before "               << "you can set its local parameterization.";  }  parameter_block->SetParameterization(local_parameterization);}const LocalParameterization* ProblemImpl::GetParameterization(    double* values) const {  ParameterBlock* parameter_block =      FindWithDefault(parameter_block_map_, values, NULL);  if (parameter_block == NULL) {    LOG(FATAL) << "Parameter block not found: " << values               << ". You must add the parameter block to the problem before "               << "you can get its local parameterization.";  }  return parameter_block->local_parameterization();}void ProblemImpl::SetParameterLowerBound(double* values,                                         int index,                                         double lower_bound) {  ParameterBlock* parameter_block =      FindWithDefault(parameter_block_map_, values, NULL);  if (parameter_block == NULL) {    LOG(FATAL) << "Parameter block not found: " << values               << ". You must add the parameter block to the problem before "               << "you can set a lower bound on one of its components.";  }  parameter_block->SetLowerBound(index, lower_bound);}void ProblemImpl::SetParameterUpperBound(double* values,                                         int index,                                         double upper_bound) {  ParameterBlock* parameter_block =      FindWithDefault(parameter_block_map_, values, NULL);  if (parameter_block == NULL) {    LOG(FATAL) << "Parameter block not found: " << values               << ". You must add the parameter block to the problem before "               << "you can set an upper bound on one of its components.";  }  parameter_block->SetUpperBound(index, upper_bound);}bool ProblemImpl::Evaluate(const Problem::EvaluateOptions& evaluate_options,                           double* cost,                           vector<double>* residuals,                           vector<double>* gradient,                           CRSMatrix* jacobian) {  if (cost == NULL &&      residuals == NULL &&      gradient == NULL &&      jacobian == NULL) {    LOG(INFO) << "Nothing to do.";    return true;  }  // If the user supplied residual blocks, then use them, otherwise  // take the residual blocks from the underlying program.  Program program;  *program.mutable_residual_blocks() =      ((evaluate_options.residual_blocks.size() > 0)       ? evaluate_options.residual_blocks : program_->residual_blocks());  const vector<double*>& parameter_block_ptrs =      evaluate_options.parameter_blocks;  vector<ParameterBlock*> variable_parameter_blocks;  vector<ParameterBlock*>& parameter_blocks =      *program.mutable_parameter_blocks();  if (parameter_block_ptrs.size() == 0) {    // The user did not provide any parameter blocks, so default to    // using all the parameter blocks in the order that they are in    // the underlying program object.    parameter_blocks = program_->parameter_blocks();  } else {    // The user supplied a vector of parameter blocks. Using this list    // requires a number of steps.    // 1. Convert double* into ParameterBlock*    parameter_blocks.resize(parameter_block_ptrs.size());    for (int i = 0; i < parameter_block_ptrs.size(); ++i) {      parameter_blocks[i] = FindWithDefault(parameter_block_map_,                                            parameter_block_ptrs[i],                                            NULL);      if (parameter_blocks[i] == NULL) {        LOG(FATAL) << "No known parameter block for "                   << "Problem::Evaluate::Options.parameter_blocks[" << i << "]"                   << " = " << parameter_block_ptrs[i];      }    }    // 2. The user may have only supplied a subset of parameter    // blocks, so identify the ones that are not supplied by the user    // and are NOT constant. These parameter blocks are stored in    // variable_parameter_blocks.    //    // To ensure that the parameter blocks are not included in the    // columns of the jacobian, we need to make sure that they are    // constant during evaluation and then make them variable again    // after we are done.    vector<ParameterBlock*> all_parameter_blocks(program_->parameter_blocks());    vector<ParameterBlock*> included_parameter_blocks(        program.parameter_blocks());    vector<ParameterBlock*> excluded_parameter_blocks;    sort(all_parameter_blocks.begin(), all_parameter_blocks.end());    sort(included_parameter_blocks.begin(), included_parameter_blocks.end());    set_difference(all_parameter_blocks.begin(),                   all_parameter_blocks.end(),                   included_parameter_blocks.begin(),                   included_parameter_blocks.end(),                   back_inserter(excluded_parameter_blocks));    variable_parameter_blocks.reserve(excluded_parameter_blocks.size());    for (int i = 0; i < excluded_parameter_blocks.size(); ++i) {      ParameterBlock* parameter_block = excluded_parameter_blocks[i];      if (!parameter_block->IsConstant()) {        variable_parameter_blocks.push_back(parameter_block);        parameter_block->SetConstant();      }    }  }  // Setup the Parameter indices and offsets before an evaluator can  // be constructed and used.  program.SetParameterOffsetsAndIndex();  Evaluator::Options evaluator_options;  // Even though using SPARSE_NORMAL_CHOLESKY requires SuiteSparse or  // CXSparse, here it just being used for telling the evaluator to  // use a SparseRowCompressedMatrix for the jacobian. This is because  // the Evaluator decides the storage for the Jacobian based on the  // type of linear solver being used.  evaluator_options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;#ifdef CERES_NO_THREADS  LOG_IF(WARNING, evaluate_options.num_threads > 1)      << "Neither OpenMP nor TBB support is compiled into this binary; "      << "only evaluate_options.num_threads = 1 is supported. Switching "      << "to single threaded mode.";  evaluator_options.num_threads = 1;#else  evaluator_options.num_threads = evaluate_options.num_threads;#endif  // CERES_NO_THREADS  // The main thread also does work so we only need to launch num_threads - 1.  context_impl_->EnsureMinimumThreads(evaluator_options.num_threads - 1);  evaluator_options.context = context_impl_;  scoped_ptr<Evaluator> evaluator(      new ProgramEvaluator<ScratchEvaluatePreparer,                           CompressedRowJacobianWriter>(evaluator_options,                                                        &program));  if (residuals !=NULL) {    residuals->resize(evaluator->NumResiduals());  }  if (gradient != NULL) {    gradient->resize(evaluator->NumEffectiveParameters());  }  scoped_ptr<CompressedRowSparseMatrix> tmp_jacobian;  if (jacobian != NULL) {    tmp_jacobian.reset(        down_cast<CompressedRowSparseMatrix*>(evaluator->CreateJacobian()));  }  // Point the state pointers to the user state pointers. This is  // needed so that we can extract a parameter vector which is then  // passed to Evaluator::Evaluate.  program.SetParameterBlockStatePtrsToUserStatePtrs();  // Copy the value of the parameter blocks into a vector, since the  // Evaluate::Evaluate method needs its input as such. The previous  // call to SetParameterBlockStatePtrsToUserStatePtrs ensures that  // these values are the ones corresponding to the actual state of  // the parameter blocks, rather than the temporary state pointer  // used for evaluation.  Vector parameters(program.NumParameters());  program.ParameterBlocksToStateVector(parameters.data());  double tmp_cost = 0;  Evaluator::EvaluateOptions evaluator_evaluate_options;  evaluator_evaluate_options.apply_loss_function =      evaluate_options.apply_loss_function;  bool status = evaluator->Evaluate(evaluator_evaluate_options,                                    parameters.data(),                                    &tmp_cost,                                    residuals != NULL ? &(*residuals)[0] : NULL,                                    gradient != NULL ? &(*gradient)[0] : NULL,                                    tmp_jacobian.get());  // Make the parameter blocks that were temporarily marked constant,  // variable again.  for (int i = 0; i < variable_parameter_blocks.size(); ++i) {    variable_parameter_blocks[i]->SetVarying();  }  if (status) {    if (cost != NULL) {      *cost = tmp_cost;    }    if (jacobian != NULL) {      tmp_jacobian->ToCRSMatrix(jacobian);    }  }  program_->SetParameterBlockStatePtrsToUserStatePtrs();  program_->SetParameterOffsetsAndIndex();  return status;}int ProblemImpl::NumParameterBlocks() const {  return program_->NumParameterBlocks();}int ProblemImpl::NumParameters() const {  return program_->NumParameters();}int ProblemImpl::NumResidualBlocks() const {  return program_->NumResidualBlocks();}int ProblemImpl::NumResiduals() const {  return program_->NumResiduals();}int ProblemImpl::ParameterBlockSize(const double* values) const {  ParameterBlock* parameter_block =      FindWithDefault(parameter_block_map_, const_cast<double*>(values), NULL);  if (parameter_block == NULL) {    LOG(FATAL) << "Parameter block not found: " << values               << ". You must add the parameter block to the problem before "               << "you can get its size.";  }  return parameter_block->Size();}int ProblemImpl::ParameterBlockLocalSize(const double* values) const {  ParameterBlock* parameter_block =      FindWithDefault(parameter_block_map_, const_cast<double*>(values), NULL);  if (parameter_block == NULL) {    LOG(FATAL) << "Parameter block not found: " << values               << ". You must add the parameter block to the problem before "               << "you can get its local size.";  }  return parameter_block->LocalSize();}bool ProblemImpl::HasParameterBlock(const double* parameter_block) const {  return (parameter_block_map_.find(const_cast<double*>(parameter_block)) !=          parameter_block_map_.end());}void ProblemImpl::GetParameterBlocks(vector<double*>* parameter_blocks) const {  CHECK_NOTNULL(parameter_blocks);  parameter_blocks->resize(0);  parameter_blocks->reserve(parameter_block_map_.size());  for (const auto& entry : parameter_block_map_) {    parameter_blocks->push_back(entry.first);  }}void ProblemImpl::GetResidualBlocks(    vector<ResidualBlockId>* residual_blocks) const {  CHECK_NOTNULL(residual_blocks);  *residual_blocks = program().residual_blocks();}void ProblemImpl::GetParameterBlocksForResidualBlock(    const ResidualBlockId residual_block,    vector<double*>* parameter_blocks) const {  int num_parameter_blocks = residual_block->NumParameterBlocks();  CHECK_NOTNULL(parameter_blocks)->resize(num_parameter_blocks);  for (int i = 0; i < num_parameter_blocks; ++i) {    (*parameter_blocks)[i] =        residual_block->parameter_blocks()[i]->mutable_user_state();  }}const CostFunction* ProblemImpl::GetCostFunctionForResidualBlock(    const ResidualBlockId residual_block) const {  return residual_block->cost_function();}const LossFunction* ProblemImpl::GetLossFunctionForResidualBlock(    const ResidualBlockId residual_block) const {  return residual_block->loss_function();}void ProblemImpl::GetResidualBlocksForParameterBlock(    const double* values,    vector<ResidualBlockId>* residual_blocks) const {  ParameterBlock* parameter_block =      FindWithDefault(parameter_block_map_, const_cast<double*>(values), NULL);  if (parameter_block == NULL) {    LOG(FATAL) << "Parameter block not found: " << values               << ". You must add the parameter block to the problem before "               << "you can get the residual blocks that depend on it.";  }  if (options_.enable_fast_removal) {    // In this case the residual blocks that depend on the parameter block are    // stored in the parameter block already, so just copy them out.    CHECK_NOTNULL(residual_blocks)->resize(        parameter_block->mutable_residual_blocks()->size());    std::copy(parameter_block->mutable_residual_blocks()->begin(),              parameter_block->mutable_residual_blocks()->end(),              residual_blocks->begin());    return;  }  // Find residual blocks that depend on the parameter block.  CHECK_NOTNULL(residual_blocks)->clear();  const int num_residual_blocks = NumResidualBlocks();  for (int i = 0; i < num_residual_blocks; ++i) {    ResidualBlock* residual_block =        (*(program_->mutable_residual_blocks()))[i];    const int num_parameter_blocks = residual_block->NumParameterBlocks();    for (int j = 0; j < num_parameter_blocks; ++j) {      if (residual_block->parameter_blocks()[j] == parameter_block) {        residual_blocks->push_back(residual_block);        // The parameter blocks are guaranteed unique.        break;      }    }  }}}  // namespace internal}  // namespace ceres
 |