internal/ceres/coordinate_descent_minimizer.cc

   1 // Ceres Solver - A fast non-linear least squares minimizer
   2 // Copyright 2015 Google Inc. All rights reserved.
   3 // http://ceres-solver.org/
   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@google.com (Sameer Agarwal)
  30
  31 #include "ceres/coordinate_descent_minimizer.h"
  32
  33 #ifdef CERES_USE_OPENMP
  34 #include <omp.h>
  35 #endif
  36
  37 #include <iterator>
  38 #include <numeric>
  39 #include <vector>
  40 #include "ceres/evaluator.h"
  41 #include "ceres/linear_solver.h"
  42 #include "ceres/minimizer.h"
  43 #include "ceres/parameter_block.h"
  44 #include "ceres/parameter_block_ordering.h"
  45 #include "ceres/problem_impl.h"
  46 #include "ceres/program.h"
  47 #include "ceres/residual_block.h"
  48 #include "ceres/solver.h"
  49 #include "ceres/trust_region_minimizer.h"
  50 #include "ceres/trust_region_strategy.h"
  51
  52 namespace ceres {
  53 namespace internal {
  54
  55 using std::map;
  56 using std::max;
  57 using std::min;
  58 using std::set;
  59 using std::string;
  60 using std::vector;
  61
  62 CoordinateDescentMinimizer::~CoordinateDescentMinimizer() {
  63 }
  64
  65 bool CoordinateDescentMinimizer::Init(
  66     const Program& program,
  67     const ProblemImpl::ParameterMap& parameter_map,
  68     const ParameterBlockOrdering& ordering,
  69     string* error) {
  70   parameter_blocks_.clear();
  71   independent_set_offsets_.clear();
  72   independent_set_offsets_.push_back(0);
  73
  74   // Serialize the OrderedGroups into a vector of parameter block
  75   // offsets for parallel access.
  76   map<ParameterBlock*, int> parameter_block_index;
  77   map<int, set<double*> > group_to_elements = ordering.group_to_elements();
  78   for (map<int, set<double*> >::const_iterator it = group_to_elements.begin();
  79        it != group_to_elements.end();
  80        ++it) {
  81     for (set<double*>::const_iterator ptr_it = it->second.begin();
  82          ptr_it != it->second.end();
  83          ++ptr_it) {
  84       parameter_blocks_.push_back(parameter_map.find(*ptr_it)->second);
  85       parameter_block_index[parameter_blocks_.back()] =
  86           parameter_blocks_.size() - 1;
  87     }
  88     independent_set_offsets_.push_back(
  89         independent_set_offsets_.back() + it->second.size());
  90   }
  91
  92   // The ordering does not have to contain all parameter blocks, so
  93   // assign zero offsets/empty independent sets to these parameter
  94   // blocks.
  95   const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
  96   for (int i = 0; i < parameter_blocks.size(); ++i) {
  97     if (!ordering.IsMember(parameter_blocks[i]->mutable_user_state())) {
  98       parameter_blocks_.push_back(parameter_blocks[i]);
  99       independent_set_offsets_.push_back(independent_set_offsets_.back());
 100     }
 101   }
 102
 103   // Compute the set of residual blocks that depend on each parameter
 104   // block.
 105   residual_blocks_.resize(parameter_block_index.size());
 106   const vector<ResidualBlock*>& residual_blocks = program.residual_blocks();
 107   for (int i = 0; i < residual_blocks.size(); ++i) {
 108     ResidualBlock* residual_block = residual_blocks[i];
 109     const int num_parameter_blocks = residual_block->NumParameterBlocks();
 110     for (int j = 0; j < num_parameter_blocks; ++j) {
 111       ParameterBlock* parameter_block = residual_block->parameter_blocks()[j];
 112       const map<ParameterBlock*, int>::const_iterator it =
 113           parameter_block_index.find(parameter_block);
 114       if (it != parameter_block_index.end()) {
 115         residual_blocks_[it->second].push_back(residual_block);
 116       }
 117     }
 118   }
 119
 120   evaluator_options_.linear_solver_type = DENSE_QR;
 121   evaluator_options_.num_eliminate_blocks = 0;
 122   evaluator_options_.num_threads = 1;
 123
 124   return true;
 125 }
 126
 127 void CoordinateDescentMinimizer::Minimize(
 128     const Minimizer::Options& options,
 129     double* parameters,
 130     Solver::Summary* summary) {
 131   // Set the state and mark all parameter blocks constant.
 132   for (int i = 0; i < parameter_blocks_.size(); ++i) {
 133     ParameterBlock* parameter_block = parameter_blocks_[i];
 134     parameter_block->SetState(parameters + parameter_block->state_offset());
 135     parameter_block->SetConstant();
 136   }
 137
 138   scoped_array<LinearSolver*> linear_solvers(
 139       new LinearSolver*[options.num_threads]);
 140
 141   LinearSolver::Options linear_solver_options;
 142   linear_solver_options.type = DENSE_QR;
 143
 144   for (int i = 0; i < options.num_threads; ++i) {
 145     linear_solvers[i] = LinearSolver::Create(linear_solver_options);
 146   }
 147
 148   for (int i = 0; i < independent_set_offsets_.size() - 1; ++i) {
 149     const int num_problems =
 150         independent_set_offsets_[i + 1] - independent_set_offsets_[i];
 151     // No point paying the price for an OpemMP call if the set is of
 152     // size zero.
 153     if (num_problems == 0) {
 154       continue;
 155     }
 156
 157 #ifdef CERES_USE_OPENMP
 158     const int num_inner_iteration_threads =
 159         min(options.num_threads, num_problems);
 160     evaluator_options_.num_threads =
 161         max(1, options.num_threads / num_inner_iteration_threads);
 162
 163     // The parameter blocks in each independent set can be optimized
 164     // in parallel, since they do not co-occur in any residual block.
 165 #pragma omp parallel for num_threads(num_inner_iteration_threads)
 166 #endif
 167     for (int j = independent_set_offsets_[i];
 168          j < independent_set_offsets_[i + 1];
 169          ++j) {
 170 #ifdef CERES_USE_OPENMP
 171       int thread_id = omp_get_thread_num();
 172 #else
 173       int thread_id = 0;
 174 #endif
 175
 176       ParameterBlock* parameter_block = parameter_blocks_[j];
 177       const int old_index = parameter_block->index();
 178       const int old_delta_offset = parameter_block->delta_offset();
 179       parameter_block->SetVarying();
 180       parameter_block->set_index(0);
 181       parameter_block->set_delta_offset(0);
 182
 183       Program inner_program;
 184       inner_program.mutable_parameter_blocks()->push_back(parameter_block);
 185       *inner_program.mutable_residual_blocks() = residual_blocks_[j];
 186
 187       // TODO(sameeragarwal): Better error handling. Right now we
 188       // assume that this is not going to lead to problems of any
 189       // sort. Basically we should be checking for numerical failure
 190       // of some sort.
 191       //
 192       // On the other hand, if the optimization is a failure, that in
 193       // some ways is fine, since it won't change the parameters and
 194       // we are fine.
 195       Solver::Summary inner_summary;
 196       Solve(&inner_program,
 197             linear_solvers[thread_id],
 198             parameters + parameter_block->state_offset(),
 199             &inner_summary);
 200
 201       parameter_block->set_index(old_index);
 202       parameter_block->set_delta_offset(old_delta_offset);
 203       parameter_block->SetState(parameters + parameter_block->state_offset());
 204       parameter_block->SetConstant();
 205     }
 206   }
 207
 208   for (int i =  0; i < parameter_blocks_.size(); ++i) {
 209     parameter_blocks_[i]->SetVarying();
 210   }
 211
 212   for (int i = 0; i < options.num_threads; ++i) {
 213     delete linear_solvers[i];
 214   }
 215 }
 216
 217 // Solve the optimization problem for one parameter block.
 218 void CoordinateDescentMinimizer::Solve(Program* program,
 219                                        LinearSolver* linear_solver,
 220                                        double* parameter,
 221                                        Solver::Summary* summary) {
 222   *summary = Solver::Summary();
 223   summary->initial_cost = 0.0;
 224   summary->fixed_cost = 0.0;
 225   summary->final_cost = 0.0;
 226   string error;
 227
 228   Minimizer::Options minimizer_options;
 229   minimizer_options.evaluator.reset(
 230       CHECK_NOTNULL(Evaluator::Create(evaluator_options_, program,  &error)));
 231   minimizer_options.jacobian.reset(
 232       CHECK_NOTNULL(minimizer_options.evaluator->CreateJacobian()));
 233
 234   TrustRegionStrategy::Options trs_options;
 235   trs_options.linear_solver = linear_solver;
 236   minimizer_options.trust_region_strategy.reset(
 237       CHECK_NOTNULL(TrustRegionStrategy::Create(trs_options)));
 238   minimizer_options.is_silent = true;
 239
 240   TrustRegionMinimizer minimizer;
 241   minimizer.Minimize(minimizer_options, parameter, summary);
 242 }
 243
 244 bool CoordinateDescentMinimizer::IsOrderingValid(
 245     const Program& program,
 246     const ParameterBlockOrdering& ordering,
 247     string* message) {
 248   const map<int, set<double*> >& group_to_elements =
 249       ordering.group_to_elements();
 250
 251   // Verify that each group is an independent set
 252   map<int, set<double*> >::const_iterator it = group_to_elements.begin();
 253   for (; it != group_to_elements.end(); ++it) {
 254     if (!program.IsParameterBlockSetIndependent(it->second)) {
 255       *message =
 256           StringPrintf("The user-provided "
 257                        "parameter_blocks_for_inner_iterations does not "
 258                        "form an independent set. Group Id: %d", it->first);
 259       return false;
 260     }
 261   }
 262   return true;
 263 }
 264
 265 // Find a recursive decomposition of the Hessian matrix as a set
 266 // of independent sets of decreasing size and invert it. This
 267 // seems to work better in practice, i.e., Cameras before
 268 // points.
 269 ParameterBlockOrdering* CoordinateDescentMinimizer::CreateOrdering(
 270     const Program& program) {
 271   scoped_ptr<ParameterBlockOrdering> ordering(new ParameterBlockOrdering);
 272   ComputeRecursiveIndependentSetOrdering(program, ordering.get());
 273   ordering->Reverse();
 274   return ordering.release();
 275 }
 276
 277 }  // namespace internal
 278 }  // namespace ceres