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/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 #include "ceres/parameter_block_ordering.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( 133 new LinearSolver*[options.num_threads]); 134 135 LinearSolver::Options linear_solver_options; 136 linear_solver_options.type = DENSE_QR; 137 138 for (int i = 0; i < options.num_threads; ++i) { 139 linear_solvers[i] = LinearSolver::Create(linear_solver_options); 140 } 141 142 for (int i = 0; i < independent_set_offsets_.size() - 1; ++i) { 143 // No point paying the price for an OpemMP call if the set if of 144 // size zero. 145 if (independent_set_offsets_[i] == independent_set_offsets_[i + 1]) { 146 continue; 147 } 148 149 // The parameter blocks in each independent set can be optimized 150 // in parallel, since they do not co-occur in any residual block. 151 #pragma omp parallel for num_threads(options.num_threads) 152 for (int j = independent_set_offsets_[i]; 153 j < independent_set_offsets_[i + 1]; 154 ++j) { 155 #ifdef CERES_USE_OPENMP 156 int thread_id = omp_get_thread_num(); 157 #else 158 int thread_id = 0; 159 #endif 160 161 ParameterBlock* parameter_block = parameter_blocks_[j]; 162 const int old_index = parameter_block->index(); 163 const int old_delta_offset = parameter_block->delta_offset(); 164 parameter_block->SetVarying(); 165 parameter_block->set_index(0); 166 parameter_block->set_delta_offset(0); 167 168 Program inner_program; 169 inner_program.mutable_parameter_blocks()->push_back(parameter_block); 170 *inner_program.mutable_residual_blocks() = residual_blocks_[j]; 171 172 // TODO(sameeragarwal): Better error handling. Right now we 173 // assume that this is not going to lead to problems of any 174 // sort. Basically we should be checking for numerical failure 175 // of some sort. 176 // 177 // On the other hand, if the optimization is a failure, that in 178 // some ways is fine, since it won't change the parameters and 179 // we are fine. 180 Solver::Summary inner_summary; 181 Solve(&inner_program, 182 linear_solvers[thread_id], 183 parameters + parameter_block->state_offset(), 184 &inner_summary); 185 186 parameter_block->set_index(old_index); 187 parameter_block->set_delta_offset(old_delta_offset); 188 parameter_block->SetState(parameters + parameter_block->state_offset()); 189 parameter_block->SetConstant(); 190 } 191 } 192 193 for (int i = 0; i < parameter_blocks_.size(); ++i) { 194 parameter_blocks_[i]->SetVarying(); 195 } 196 197 for (int i = 0; i < options.num_threads; ++i) { 198 delete linear_solvers[i]; 199 } 200 } 201 202 // Solve the optimization problem for one parameter block. 203 void CoordinateDescentMinimizer::Solve(Program* program, 204 LinearSolver* linear_solver, 205 double* parameter, 206 Solver::Summary* summary) { 207 *summary = Solver::Summary(); 208 summary->initial_cost = 0.0; 209 summary->fixed_cost = 0.0; 210 summary->final_cost = 0.0; 211 string error; 212 213 scoped_ptr<Evaluator> evaluator( 214 Evaluator::Create(evaluator_options_, program, &error)); 215 CHECK_NOTNULL(evaluator.get()); 216 217 scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian()); 218 CHECK_NOTNULL(jacobian.get()); 219 220 TrustRegionStrategy::Options trs_options; 221 trs_options.linear_solver = linear_solver; 222 223 scoped_ptr<TrustRegionStrategy>trust_region_strategy( 224 CHECK_NOTNULL(TrustRegionStrategy::Create(trs_options))); 225 226 Minimizer::Options minimizer_options; 227 minimizer_options.evaluator = evaluator.get(); 228 minimizer_options.jacobian = jacobian.get(); 229 minimizer_options.trust_region_strategy = trust_region_strategy.get(); 230 minimizer_options.is_silent = true; 231 232 TrustRegionMinimizer minimizer; 233 minimizer.Minimize(minimizer_options, parameter, summary); 234 } 235 236 bool CoordinateDescentMinimizer::IsOrderingValid( 237 const Program& program, 238 const ParameterBlockOrdering& ordering, 239 string* message) { 240 const map<int, set<double*> >& group_to_elements = 241 ordering.group_to_elements(); 242 243 // Verify that each group is an independent set 244 map<int, set<double*> >::const_iterator it = group_to_elements.begin(); 245 for ( ; it != group_to_elements.end(); ++it) { 246 if (!program.IsParameterBlockSetIndependent(it->second)) { 247 *message = 248 StringPrintf("The user-provided " 249 "parameter_blocks_for_inner_iterations does not " 250 "form an independent set. Group Id: %d", it->first); 251 return false; 252 } 253 } 254 return true; 255 } 256 257 // Find a recursive decomposition of the Hessian matrix as a set 258 // of independent sets of decreasing size and invert it. This 259 // seems to work better in practice, i.e., Cameras before 260 // points. 261 ParameterBlockOrdering* CoordinateDescentMinimizer::CreateOrdering( 262 const Program& program) { 263 scoped_ptr<ParameterBlockOrdering> ordering(new ParameterBlockOrdering); 264 ComputeRecursiveIndependentSetOrdering(program, ordering.get()); 265 ordering->Reverse(); 266 return ordering.release(); 267 } 268 269 } // namespace internal 270 } // namespace ceres 271
