1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2012 Google Inc. All rights reserved. 3 // http://code.google.com/p/ceres-solver/ 4 // 5 // Redistribution and use in source and binary forms, with or without 6 // modification, are permitted provided that the following conditions are met: 7 // 8 // * Redistributions of source code must retain the above copyright notice, 9 // this list of conditions and the following disclaimer. 10 // * Redistributions in binary form must reproduce the above copyright notice, 11 // this list of conditions and the following disclaimer in the documentation 12 // and/or other materials provided with the distribution. 13 // * Neither the name of Google Inc. nor the names of its contributors may be 14 // used to endorse or promote products derived from this software without 15 // specific prior written permission. 16 // 17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 // POSSIBILITY OF SUCH DAMAGE. 28 // 29 // Author: sameeragarwal (at) google.com (Sameer Agarwal) 30 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/ordered_groups.h" 44 #include "ceres/parameter_block.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/solver_impl.h" 50 #include "ceres/trust_region_minimizer.h" 51 #include "ceres/trust_region_strategy.h" 52 53 namespace ceres { 54 namespace internal { 55 56 CoordinateDescentMinimizer::~CoordinateDescentMinimizer() { 57 } 58 59 bool CoordinateDescentMinimizer::Init( 60 const Program& program, 61 const ProblemImpl::ParameterMap& parameter_map, 62 const ParameterBlockOrdering& ordering, 63 string* error) { 64 parameter_blocks_.clear(); 65 independent_set_offsets_.clear(); 66 independent_set_offsets_.push_back(0); 67 68 // Serialize the OrderedGroups into a vector of parameter block 69 // offsets for parallel access. 70 map<ParameterBlock*, int> parameter_block_index; 71 map<int, set<double*> > group_to_elements = ordering.group_to_elements(); 72 for (map<int, set<double*> >::const_iterator it = group_to_elements.begin(); 73 it != group_to_elements.end(); 74 ++it) { 75 for (set<double*>::const_iterator ptr_it = it->second.begin(); 76 ptr_it != it->second.end(); 77 ++ptr_it) { 78 parameter_blocks_.push_back(parameter_map.find(*ptr_it)->second); 79 parameter_block_index[parameter_blocks_.back()] = 80 parameter_blocks_.size() - 1; 81 } 82 independent_set_offsets_.push_back( 83 independent_set_offsets_.back() + it->second.size()); 84 } 85 86 // The ordering does not have to contain all parameter blocks, so 87 // assign zero offsets/empty independent sets to these parameter 88 // blocks. 89 const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); 90 for (int i = 0; i < parameter_blocks.size(); ++i) { 91 if (!ordering.IsMember(parameter_blocks[i]->mutable_user_state())) { 92 parameter_blocks_.push_back(parameter_blocks[i]); 93 independent_set_offsets_.push_back(independent_set_offsets_.back()); 94 } 95 } 96 97 // Compute the set of residual blocks that depend on each parameter 98 // block. 99 residual_blocks_.resize(parameter_block_index.size()); 100 const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); 101 for (int i = 0; i < residual_blocks.size(); ++i) { 102 ResidualBlock* residual_block = residual_blocks[i]; 103 const int num_parameter_blocks = residual_block->NumParameterBlocks(); 104 for (int j = 0; j < num_parameter_blocks; ++j) { 105 ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; 106 const map<ParameterBlock*, int>::const_iterator it = 107 parameter_block_index.find(parameter_block); 108 if (it != parameter_block_index.end()) { 109 residual_blocks_[it->second].push_back(residual_block); 110 } 111 } 112 } 113 114 evaluator_options_.linear_solver_type = DENSE_QR; 115 evaluator_options_.num_eliminate_blocks = 0; 116 evaluator_options_.num_threads = 1; 117 118 return true; 119 } 120 121 void CoordinateDescentMinimizer::Minimize( 122 const Minimizer::Options& options, 123 double* parameters, 124 Solver::Summary* summary) { 125 // Set the state and mark all parameter blocks constant. 126 for (int i = 0; i < parameter_blocks_.size(); ++i) { 127 ParameterBlock* parameter_block = parameter_blocks_[i]; 128 parameter_block->SetState(parameters + parameter_block->state_offset()); 129 parameter_block->SetConstant(); 130 } 131 132 scoped_array<LinearSolver*> linear_solvers(new LinearSolver*[options.num_threads]); 133 134 LinearSolver::Options linear_solver_options; 135 linear_solver_options.type = DENSE_QR; 136 137 for (int i = 0; i < options.num_threads; ++i) { 138 linear_solvers[i] = LinearSolver::Create(linear_solver_options); 139 } 140 141 for (int i = 0; i < independent_set_offsets_.size() - 1; ++i) { 142 // No point paying the price for an OpemMP call if the set if of 143 // size zero. 144 if (independent_set_offsets_[i] == independent_set_offsets_[i + 1]) { 145 continue; 146 } 147 148 // The parameter blocks in each independent set can be optimized 149 // in parallel, since they do not co-occur in any residual block. 150 #pragma omp parallel for num_threads(options.num_threads) 151 for (int j = independent_set_offsets_[i]; 152 j < independent_set_offsets_[i + 1]; 153 ++j) { 154 #ifdef CERES_USE_OPENMP 155 int thread_id = omp_get_thread_num(); 156 #else 157 int thread_id = 0; 158 #endif 159 160 ParameterBlock* parameter_block = parameter_blocks_[j]; 161 const int old_index = parameter_block->index(); 162 const int old_delta_offset = parameter_block->delta_offset(); 163 parameter_block->SetVarying(); 164 parameter_block->set_index(0); 165 parameter_block->set_delta_offset(0); 166 167 Program inner_program; 168 inner_program.mutable_parameter_blocks()->push_back(parameter_block); 169 *inner_program.mutable_residual_blocks() = residual_blocks_[j]; 170 171 // TODO(sameeragarwal): Better error handling. Right now we 172 // assume that this is not going to lead to problems of any 173 // sort. Basically we should be checking for numerical failure 174 // of some sort. 175 // 176 // On the other hand, if the optimization is a failure, that in 177 // some ways is fine, since it won't change the parameters and 178 // we are fine. 179 Solver::Summary inner_summary; 180 Solve(&inner_program, 181 linear_solvers[thread_id], 182 parameters + parameter_block->state_offset(), 183 &inner_summary); 184 185 parameter_block->set_index(old_index); 186 parameter_block->set_delta_offset(old_delta_offset); 187 parameter_block->SetState(parameters + parameter_block->state_offset()); 188 parameter_block->SetConstant(); 189 } 190 } 191 192 for (int i = 0; i < parameter_blocks_.size(); ++i) { 193 parameter_blocks_[i]->SetVarying(); 194 } 195 196 for (int i = 0; i < options.num_threads; ++i) { 197 delete linear_solvers[i]; 198 } 199 } 200 201 // Solve the optimization problem for one parameter block. 202 void CoordinateDescentMinimizer::Solve(Program* program, 203 LinearSolver* linear_solver, 204 double* parameter, 205 Solver::Summary* summary) { 206 *summary = Solver::Summary(); 207 summary->initial_cost = 0.0; 208 summary->fixed_cost = 0.0; 209 summary->final_cost = 0.0; 210 string error; 211 212 scoped_ptr<Evaluator> evaluator( 213 Evaluator::Create(evaluator_options_, program, &error)); 214 CHECK_NOTNULL(evaluator.get()); 215 216 scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian()); 217 CHECK_NOTNULL(jacobian.get()); 218 219 TrustRegionStrategy::Options trs_options; 220 trs_options.linear_solver = linear_solver; 221 222 scoped_ptr<TrustRegionStrategy>trust_region_strategy( 223 CHECK_NOTNULL(TrustRegionStrategy::Create(trs_options))); 224 225 Minimizer::Options minimizer_options; 226 minimizer_options.evaluator = evaluator.get(); 227 minimizer_options.jacobian = jacobian.get(); 228 minimizer_options.trust_region_strategy = trust_region_strategy.get(); 229 230 TrustRegionMinimizer minimizer; 231 minimizer.Minimize(minimizer_options, parameter, summary); 232 } 233 234 } // namespace internal 235 } // namespace ceres 236