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 // Generic loop for line search based optimization algorithms. 32 // 33 // This is primarily inpsired by the minFunc packaged written by Mark 34 // Schmidt. 35 // 36 // http://www.di.ens.fr/~mschmidt/Software/minFunc.html 37 // 38 // For details on the theory and implementation see "Numerical 39 // Optimization" by Nocedal & Wright. 40 41 #ifndef CERES_NO_LINE_SEARCH_MINIMIZER 42 43 #include "ceres/line_search_minimizer.h" 44 45 #include <algorithm> 46 #include <cstdlib> 47 #include <cmath> 48 #include <string> 49 #include <vector> 50 51 #include "Eigen/Dense" 52 #include "ceres/array_utils.h" 53 #include "ceres/evaluator.h" 54 #include "ceres/internal/eigen.h" 55 #include "ceres/internal/port.h" 56 #include "ceres/internal/scoped_ptr.h" 57 #include "ceres/line_search.h" 58 #include "ceres/line_search_direction.h" 59 #include "ceres/stringprintf.h" 60 #include "ceres/types.h" 61 #include "ceres/wall_time.h" 62 #include "glog/logging.h" 63 64 namespace ceres { 65 namespace internal { 66 namespace { 67 // Small constant for various floating point issues. 68 // TODO(sameeragarwal): Change to a better name if this has only one 69 // use. 70 const double kEpsilon = 1e-12; 71 72 bool Evaluate(Evaluator* evaluator, 73 const Vector& x, 74 LineSearchMinimizer::State* state) { 75 const bool status = evaluator->Evaluate(x.data(), 76 &(state->cost), 77 NULL, 78 state->gradient.data(), 79 NULL); 80 if (status) { 81 state->gradient_squared_norm = state->gradient.squaredNorm(); 82 state->gradient_max_norm = state->gradient.lpNorm<Eigen::Infinity>(); 83 } 84 85 return status; 86 } 87 88 } // namespace 89 90 void LineSearchMinimizer::Minimize(const Minimizer::Options& options, 91 double* parameters, 92 Solver::Summary* summary) { 93 double start_time = WallTimeInSeconds(); 94 double iteration_start_time = start_time; 95 96 Evaluator* evaluator = CHECK_NOTNULL(options.evaluator); 97 const int num_parameters = evaluator->NumParameters(); 98 const int num_effective_parameters = evaluator->NumEffectiveParameters(); 99 100 summary->termination_type = NO_CONVERGENCE; 101 summary->num_successful_steps = 0; 102 summary->num_unsuccessful_steps = 0; 103 104 VectorRef x(parameters, num_parameters); 105 106 State current_state(num_parameters, num_effective_parameters); 107 State previous_state(num_parameters, num_effective_parameters); 108 109 Vector delta(num_effective_parameters); 110 Vector x_plus_delta(num_parameters); 111 112 IterationSummary iteration_summary; 113 iteration_summary.iteration = 0; 114 iteration_summary.step_is_valid = false; 115 iteration_summary.step_is_successful = false; 116 iteration_summary.cost_change = 0.0; 117 iteration_summary.gradient_max_norm = 0.0; 118 iteration_summary.step_norm = 0.0; 119 iteration_summary.linear_solver_iterations = 0; 120 iteration_summary.step_solver_time_in_seconds = 0; 121 122 // Do initial cost and Jacobian evaluation. 123 if (!Evaluate(evaluator, x, ¤t_state)) { 124 LOG(WARNING) << "Terminating: Cost and gradient evaluation failed."; 125 summary->termination_type = NUMERICAL_FAILURE; 126 return; 127 } 128 129 summary->initial_cost = current_state.cost + summary->fixed_cost; 130 iteration_summary.cost = current_state.cost + summary->fixed_cost; 131 132 iteration_summary.gradient_max_norm = current_state.gradient_max_norm; 133 134 // The initial gradient max_norm is bounded from below so that we do 135 // not divide by zero. 136 const double initial_gradient_max_norm = 137 max(iteration_summary.gradient_max_norm, kEpsilon); 138 const double absolute_gradient_tolerance = 139 options.gradient_tolerance * initial_gradient_max_norm; 140 141 if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) { 142 summary->termination_type = GRADIENT_TOLERANCE; 143 VLOG(1) << "Terminating: Gradient tolerance reached." 144 << "Relative gradient max norm: " 145 << iteration_summary.gradient_max_norm / initial_gradient_max_norm 146 << " <= " << options.gradient_tolerance; 147 return; 148 } 149 150 iteration_summary.iteration_time_in_seconds = 151 WallTimeInSeconds() - iteration_start_time; 152 iteration_summary.cumulative_time_in_seconds = 153 WallTimeInSeconds() - start_time 154 + summary->preprocessor_time_in_seconds; 155 summary->iterations.push_back(iteration_summary); 156 157 LineSearchDirection::Options line_search_direction_options; 158 line_search_direction_options.num_parameters = num_effective_parameters; 159 line_search_direction_options.type = options.line_search_direction_type; 160 line_search_direction_options.nonlinear_conjugate_gradient_type = 161 options.nonlinear_conjugate_gradient_type; 162 line_search_direction_options.max_lbfgs_rank = options.max_lbfgs_rank; 163 line_search_direction_options.use_approximate_eigenvalue_bfgs_scaling = 164 options.use_approximate_eigenvalue_bfgs_scaling; 165 scoped_ptr<LineSearchDirection> line_search_direction( 166 LineSearchDirection::Create(line_search_direction_options)); 167 168 LineSearchFunction line_search_function(evaluator); 169 170 LineSearch::Options line_search_options; 171 line_search_options.interpolation_type = 172 options.line_search_interpolation_type; 173 line_search_options.min_step_size = options.min_line_search_step_size; 174 line_search_options.sufficient_decrease = 175 options.line_search_sufficient_function_decrease; 176 line_search_options.max_step_contraction = 177 options.max_line_search_step_contraction; 178 line_search_options.min_step_contraction = 179 options.min_line_search_step_contraction; 180 line_search_options.max_num_iterations = 181 options.max_num_line_search_step_size_iterations; 182 line_search_options.sufficient_curvature_decrease = 183 options.line_search_sufficient_curvature_decrease; 184 line_search_options.max_step_expansion = 185 options.max_line_search_step_expansion; 186 line_search_options.function = &line_search_function; 187 188 scoped_ptr<LineSearch> 189 line_search(LineSearch::Create(options.line_search_type, 190 line_search_options, 191 &summary->error)); 192 if (line_search.get() == NULL) { 193 LOG(ERROR) << "Ceres bug: Unable to create a LineSearch object, please " 194 << "contact the developers!, error: " << summary->error; 195 summary->termination_type = DID_NOT_RUN; 196 return; 197 } 198 199 LineSearch::Summary line_search_summary; 200 int num_line_search_direction_restarts = 0; 201 202 while (true) { 203 if (!RunCallbacks(options.callbacks, iteration_summary, summary)) { 204 return; 205 } 206 207 iteration_start_time = WallTimeInSeconds(); 208 if (iteration_summary.iteration >= options.max_num_iterations) { 209 summary->termination_type = NO_CONVERGENCE; 210 VLOG(1) << "Terminating: Maximum number of iterations reached."; 211 break; 212 } 213 214 const double total_solver_time = iteration_start_time - start_time + 215 summary->preprocessor_time_in_seconds; 216 if (total_solver_time >= options.max_solver_time_in_seconds) { 217 summary->termination_type = NO_CONVERGENCE; 218 VLOG(1) << "Terminating: Maximum solver time reached."; 219 break; 220 } 221 222 iteration_summary = IterationSummary(); 223 iteration_summary.iteration = summary->iterations.back().iteration + 1; 224 iteration_summary.step_is_valid = false; 225 iteration_summary.step_is_successful = false; 226 227 bool line_search_status = true; 228 if (iteration_summary.iteration == 1) { 229 current_state.search_direction = -current_state.gradient; 230 } else { 231 line_search_status = line_search_direction->NextDirection( 232 previous_state, 233 current_state, 234 ¤t_state.search_direction); 235 } 236 237 if (!line_search_status && 238 num_line_search_direction_restarts >= 239 options.max_num_line_search_direction_restarts) { 240 // Line search direction failed to generate a new direction, and we 241 // have already reached our specified maximum number of restarts, 242 // terminate optimization. 243 summary->error = 244 StringPrintf("Line search direction failure: specified " 245 "max_num_line_search_direction_restarts: %d reached.", 246 options.max_num_line_search_direction_restarts); 247 LOG(WARNING) << summary->error << " terminating optimization."; 248 summary->termination_type = NUMERICAL_FAILURE; 249 break; 250 251 } else if (!line_search_status) { 252 // Restart line search direction with gradient descent on first iteration 253 // as we have not yet reached our maximum number of restarts. 254 CHECK_LT(num_line_search_direction_restarts, 255 options.max_num_line_search_direction_restarts); 256 257 ++num_line_search_direction_restarts; 258 LOG(WARNING) 259 << "Line search direction algorithm: " 260 << LineSearchDirectionTypeToString(options.line_search_direction_type) 261 << ", failed to produce a valid new direction at iteration: " 262 << iteration_summary.iteration << ". Restarting, number of " 263 << "restarts: " << num_line_search_direction_restarts << " / " 264 << options.max_num_line_search_direction_restarts << " [max]."; 265 line_search_direction.reset( 266 LineSearchDirection::Create(line_search_direction_options)); 267 current_state.search_direction = -current_state.gradient; 268 } 269 270 line_search_function.Init(x, current_state.search_direction); 271 current_state.directional_derivative = 272 current_state.gradient.dot(current_state.search_direction); 273 274 // TODO(sameeragarwal): Refactor this into its own object and add 275 // explanations for the various choices. 276 // 277 // Note that we use !line_search_status to ensure that we treat cases when 278 // we restarted the line search direction equivalently to the first 279 // iteration. 280 const double initial_step_size = 281 (iteration_summary.iteration == 1 || !line_search_status) 282 ? min(1.0, 1.0 / current_state.gradient_max_norm) 283 : min(1.0, 2.0 * (current_state.cost - previous_state.cost) / 284 current_state.directional_derivative); 285 // By definition, we should only ever go forwards along the specified search 286 // direction in a line search, most likely cause for this being violated 287 // would be a numerical failure in the line search direction calculation. 288 if (initial_step_size < 0.0) { 289 summary->error = 290 StringPrintf("Numerical failure in line search, initial_step_size is " 291 "negative: %.5e, directional_derivative: %.5e, " 292 "(current_cost - previous_cost): %.5e", 293 initial_step_size, current_state.directional_derivative, 294 (current_state.cost - previous_state.cost)); 295 LOG(WARNING) << summary->error; 296 summary->termination_type = NUMERICAL_FAILURE; 297 break; 298 } 299 300 line_search->Search(initial_step_size, 301 current_state.cost, 302 current_state.directional_derivative, 303 &line_search_summary); 304 305 current_state.step_size = line_search_summary.optimal_step_size; 306 delta = current_state.step_size * current_state.search_direction; 307 308 previous_state = current_state; 309 iteration_summary.step_solver_time_in_seconds = 310 WallTimeInSeconds() - iteration_start_time; 311 312 // TODO(sameeragarwal): Collect stats. 313 if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data()) || 314 !Evaluate(evaluator, x_plus_delta, ¤t_state)) { 315 LOG(WARNING) << "Evaluation failed."; 316 } else { 317 x = x_plus_delta; 318 } 319 320 iteration_summary.gradient_max_norm = current_state.gradient_max_norm; 321 if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) { 322 summary->termination_type = GRADIENT_TOLERANCE; 323 VLOG(1) << "Terminating: Gradient tolerance reached." 324 << "Relative gradient max norm: " 325 << iteration_summary.gradient_max_norm / initial_gradient_max_norm 326 << " <= " << options.gradient_tolerance; 327 break; 328 } 329 330 iteration_summary.cost_change = previous_state.cost - current_state.cost; 331 const double absolute_function_tolerance = 332 options.function_tolerance * previous_state.cost; 333 if (fabs(iteration_summary.cost_change) < absolute_function_tolerance) { 334 VLOG(1) << "Terminating. Function tolerance reached. " 335 << "|cost_change|/cost: " 336 << fabs(iteration_summary.cost_change) / previous_state.cost 337 << " <= " << options.function_tolerance; 338 summary->termination_type = FUNCTION_TOLERANCE; 339 return; 340 } 341 342 iteration_summary.cost = current_state.cost + summary->fixed_cost; 343 iteration_summary.step_norm = delta.norm(); 344 iteration_summary.step_is_valid = true; 345 iteration_summary.step_is_successful = true; 346 iteration_summary.step_norm = delta.norm(); 347 iteration_summary.step_size = current_state.step_size; 348 iteration_summary.line_search_function_evaluations = 349 line_search_summary.num_function_evaluations; 350 iteration_summary.line_search_gradient_evaluations = 351 line_search_summary.num_gradient_evaluations; 352 iteration_summary.line_search_iterations = 353 line_search_summary.num_iterations; 354 iteration_summary.iteration_time_in_seconds = 355 WallTimeInSeconds() - iteration_start_time; 356 iteration_summary.cumulative_time_in_seconds = 357 WallTimeInSeconds() - start_time 358 + summary->preprocessor_time_in_seconds; 359 360 summary->iterations.push_back(iteration_summary); 361 ++summary->num_successful_steps; 362 } 363 } 364 365 } // namespace internal 366 } // namespace ceres 367 368 #endif // CERES_NO_LINE_SEARCH_MINIMIZER 369