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@google.com (Sameer Agarwal) 30 31#include "ceres/trust_region_minimizer.h" 32 33#include <algorithm> 34#include <cstdlib> 35#include <cmath> 36#include <cstring> 37#include <limits> 38#include <string> 39#include <vector> 40 41#include "Eigen/Core" 42#include "ceres/array_utils.h" 43#include "ceres/evaluator.h" 44#include "ceres/internal/eigen.h" 45#include "ceres/internal/scoped_ptr.h" 46#include "ceres/linear_least_squares_problems.h" 47#include "ceres/sparse_matrix.h" 48#include "ceres/stringprintf.h" 49#include "ceres/trust_region_strategy.h" 50#include "ceres/types.h" 51#include "ceres/wall_time.h" 52#include "glog/logging.h" 53 54namespace ceres { 55namespace internal { 56namespace { 57// Small constant for various floating point issues. 58const double kEpsilon = 1e-12; 59} // namespace 60 61// Execute the list of IterationCallbacks sequentially. If any one of 62// the callbacks does not return SOLVER_CONTINUE, then stop and return 63// its status. 64CallbackReturnType TrustRegionMinimizer::RunCallbacks( 65 const IterationSummary& iteration_summary) { 66 for (int i = 0; i < options_.callbacks.size(); ++i) { 67 const CallbackReturnType status = 68 (*options_.callbacks[i])(iteration_summary); 69 if (status != SOLVER_CONTINUE) { 70 return status; 71 } 72 } 73 return SOLVER_CONTINUE; 74} 75 76// Compute a scaling vector that is used to improve the conditioning 77// of the Jacobian. 78void TrustRegionMinimizer::EstimateScale(const SparseMatrix& jacobian, 79 double* scale) const { 80 jacobian.SquaredColumnNorm(scale); 81 for (int i = 0; i < jacobian.num_cols(); ++i) { 82 scale[i] = 1.0 / (1.0 + sqrt(scale[i])); 83 } 84} 85 86void TrustRegionMinimizer::Init(const Minimizer::Options& options) { 87 options_ = options; 88 sort(options_.lsqp_iterations_to_dump.begin(), 89 options_.lsqp_iterations_to_dump.end()); 90} 91 92bool TrustRegionMinimizer::MaybeDumpLinearLeastSquaresProblem( 93 const int iteration, 94 const SparseMatrix* jacobian, 95 const double* residuals, 96 const double* step) const { 97 // TODO(sameeragarwal): Since the use of trust_region_radius has 98 // moved inside TrustRegionStrategy, its not clear how we dump the 99 // regularization vector/matrix anymore. 100 // 101 // Also num_eliminate_blocks is not visible to the trust region 102 // minimizer either. 103 // 104 // Both of these indicate that this is the wrong place for this 105 // code, and going forward this should needs fixing/refactoring. 106 return true; 107} 108 109void TrustRegionMinimizer::Minimize(const Minimizer::Options& options, 110 double* parameters, 111 Solver::Summary* summary) { 112 double start_time = WallTimeInSeconds(); 113 double iteration_start_time = start_time; 114 Init(options); 115 116 summary->termination_type = NO_CONVERGENCE; 117 summary->num_successful_steps = 0; 118 summary->num_unsuccessful_steps = 0; 119 120 Evaluator* evaluator = CHECK_NOTNULL(options_.evaluator); 121 SparseMatrix* jacobian = CHECK_NOTNULL(options_.jacobian); 122 TrustRegionStrategy* strategy = CHECK_NOTNULL(options_.trust_region_strategy); 123 124 const int num_parameters = evaluator->NumParameters(); 125 const int num_effective_parameters = evaluator->NumEffectiveParameters(); 126 const int num_residuals = evaluator->NumResiduals(); 127 128 VectorRef x_min(parameters, num_parameters); 129 Vector x = x_min; 130 double x_norm = x.norm(); 131 132 Vector residuals(num_residuals); 133 Vector trust_region_step(num_effective_parameters); 134 Vector delta(num_effective_parameters); 135 Vector x_plus_delta(num_parameters); 136 Vector gradient(num_effective_parameters); 137 Vector model_residuals(num_residuals); 138 Vector scale(num_effective_parameters); 139 140 IterationSummary iteration_summary; 141 iteration_summary.iteration = 0; 142 iteration_summary.step_is_valid = false; 143 iteration_summary.step_is_successful = false; 144 iteration_summary.cost_change = 0.0; 145 iteration_summary.gradient_max_norm = 0.0; 146 iteration_summary.step_norm = 0.0; 147 iteration_summary.relative_decrease = 0.0; 148 iteration_summary.trust_region_radius = strategy->Radius(); 149 // TODO(sameeragarwal): Rename eta to linear_solver_accuracy or 150 // something similar across the board. 151 iteration_summary.eta = options_.eta; 152 iteration_summary.linear_solver_iterations = 0; 153 iteration_summary.step_solver_time_in_seconds = 0; 154 155 // Do initial cost and Jacobian evaluation. 156 double cost = 0.0; 157 if (!evaluator->Evaluate(x.data(), &cost, residuals.data(), NULL, jacobian)) { 158 LOG(WARNING) << "Terminating: Residual and Jacobian evaluation failed."; 159 summary->termination_type = NUMERICAL_FAILURE; 160 return; 161 } 162 163 iteration_summary.cost = cost + summary->fixed_cost; 164 165 int num_consecutive_nonmonotonic_steps = 0; 166 double minimum_cost = cost; 167 double reference_cost = cost; 168 double accumulated_reference_model_cost_change = 0.0; 169 double candidate_cost = cost; 170 double accumulated_candidate_model_cost_change = 0.0; 171 172 gradient.setZero(); 173 jacobian->LeftMultiply(residuals.data(), gradient.data()); 174 iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>(); 175 176 if (options_.jacobi_scaling) { 177 EstimateScale(*jacobian, scale.data()); 178 jacobian->ScaleColumns(scale.data()); 179 } else { 180 scale.setOnes(); 181 } 182 183 // The initial gradient max_norm is bounded from below so that we do 184 // not divide by zero. 185 const double gradient_max_norm_0 = 186 max(iteration_summary.gradient_max_norm, kEpsilon); 187 const double absolute_gradient_tolerance = 188 options_.gradient_tolerance * gradient_max_norm_0; 189 190 if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) { 191 summary->termination_type = GRADIENT_TOLERANCE; 192 VLOG(1) << "Terminating: Gradient tolerance reached." 193 << "Relative gradient max norm: " 194 << iteration_summary.gradient_max_norm / gradient_max_norm_0 195 << " <= " << options_.gradient_tolerance; 196 return; 197 } 198 199 iteration_summary.iteration_time_in_seconds = 200 WallTimeInSeconds() - iteration_start_time; 201 iteration_summary.cumulative_time_in_seconds = 202 WallTimeInSeconds() - start_time 203 + summary->preprocessor_time_in_seconds; 204 summary->iterations.push_back(iteration_summary); 205 206 // Call the various callbacks. 207 switch (RunCallbacks(iteration_summary)) { 208 case SOLVER_TERMINATE_SUCCESSFULLY: 209 summary->termination_type = USER_SUCCESS; 210 VLOG(1) << "Terminating: User callback returned USER_SUCCESS."; 211 return; 212 case SOLVER_ABORT: 213 summary->termination_type = USER_ABORT; 214 VLOG(1) << "Terminating: User callback returned USER_ABORT."; 215 return; 216 case SOLVER_CONTINUE: 217 break; 218 default: 219 LOG(FATAL) << "Unknown type of user callback status"; 220 } 221 222 int num_consecutive_invalid_steps = 0; 223 while (true) { 224 iteration_start_time = WallTimeInSeconds(); 225 if (iteration_summary.iteration >= options_.max_num_iterations) { 226 summary->termination_type = NO_CONVERGENCE; 227 VLOG(1) << "Terminating: Maximum number of iterations reached."; 228 break; 229 } 230 231 const double total_solver_time = iteration_start_time - start_time + 232 summary->preprocessor_time_in_seconds; 233 if (total_solver_time >= options_.max_solver_time_in_seconds) { 234 summary->termination_type = NO_CONVERGENCE; 235 VLOG(1) << "Terminating: Maximum solver time reached."; 236 break; 237 } 238 239 iteration_summary = IterationSummary(); 240 iteration_summary = summary->iterations.back(); 241 iteration_summary.iteration = summary->iterations.back().iteration + 1; 242 iteration_summary.step_is_valid = false; 243 iteration_summary.step_is_successful = false; 244 245 const double strategy_start_time = WallTimeInSeconds(); 246 TrustRegionStrategy::PerSolveOptions per_solve_options; 247 per_solve_options.eta = options_.eta; 248 TrustRegionStrategy::Summary strategy_summary = 249 strategy->ComputeStep(per_solve_options, 250 jacobian, 251 residuals.data(), 252 trust_region_step.data()); 253 254 iteration_summary.step_solver_time_in_seconds = 255 WallTimeInSeconds() - strategy_start_time; 256 iteration_summary.linear_solver_iterations = 257 strategy_summary.num_iterations; 258 259 if (!MaybeDumpLinearLeastSquaresProblem(iteration_summary.iteration, 260 jacobian, 261 residuals.data(), 262 trust_region_step.data())) { 263 LOG(FATAL) << "Tried writing linear least squares problem: " 264 << options.lsqp_dump_directory << "but failed."; 265 } 266 267 double model_cost_change = 0.0; 268 if (strategy_summary.termination_type != FAILURE) { 269 // new_model_cost 270 // = 1/2 [f + J * step]^2 271 // = 1/2 [ f'f + 2f'J * step + step' * J' * J * step ] 272 // model_cost_change 273 // = cost - new_model_cost 274 // = f'f/2 - 1/2 [ f'f + 2f'J * step + step' * J' * J * step] 275 // = -f'J * step - step' * J' * J * step / 2 276 model_residuals.setZero(); 277 jacobian->RightMultiply(trust_region_step.data(), model_residuals.data()); 278 model_cost_change = -(residuals.dot(model_residuals) + 279 model_residuals.squaredNorm() / 2.0); 280 281 if (model_cost_change < 0.0) { 282 VLOG(1) << "Invalid step: current_cost: " << cost 283 << " absolute difference " << model_cost_change 284 << " relative difference " << (model_cost_change / cost); 285 } else { 286 iteration_summary.step_is_valid = true; 287 } 288 } 289 290 if (!iteration_summary.step_is_valid) { 291 // Invalid steps can happen due to a number of reasons, and we 292 // allow a limited number of successive failures, and return with 293 // NUMERICAL_FAILURE if this limit is exceeded. 294 if (++num_consecutive_invalid_steps >= 295 options_.max_num_consecutive_invalid_steps) { 296 summary->termination_type = NUMERICAL_FAILURE; 297 summary->error = StringPrintf( 298 "Terminating. Number of successive invalid steps more " 299 "than Solver::Options::max_num_consecutive_invalid_steps: %d", 300 options_.max_num_consecutive_invalid_steps); 301 302 LOG(WARNING) << summary->error; 303 return; 304 } 305 306 // We are going to try and reduce the trust region radius and 307 // solve again. To do this, we are going to treat this iteration 308 // as an unsuccessful iteration. Since the various callbacks are 309 // still executed, we are going to fill the iteration summary 310 // with data that assumes a step of length zero and no progress. 311 iteration_summary.cost = cost + summary->fixed_cost; 312 iteration_summary.cost_change = 0.0; 313 iteration_summary.gradient_max_norm = 314 summary->iterations.back().gradient_max_norm; 315 iteration_summary.step_norm = 0.0; 316 iteration_summary.relative_decrease = 0.0; 317 iteration_summary.eta = options_.eta; 318 } else { 319 // The step is numerically valid, so now we can judge its quality. 320 num_consecutive_invalid_steps = 0; 321 322 // Undo the Jacobian column scaling. 323 delta = (trust_region_step.array() * scale.array()).matrix(); 324 if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) { 325 summary->termination_type = NUMERICAL_FAILURE; 326 summary->error = 327 "Terminating. Failed to compute Plus(x, delta, x_plus_delta)."; 328 329 LOG(WARNING) << summary->error; 330 return; 331 } 332 333 // Try this step. 334 double new_cost = numeric_limits<double>::max(); 335 if (!evaluator->Evaluate(x_plus_delta.data(), 336 &new_cost, 337 NULL, NULL, NULL)) { 338 // If the evaluation of the new cost fails, treat it as a step 339 // with high cost. 340 LOG(WARNING) << "Step failed to evaluate. " 341 << "Treating it as step with infinite cost"; 342 new_cost = numeric_limits<double>::max(); 343 } else { 344 // Check if performing an inner iteration will make it better. 345 if (options.inner_iteration_minimizer != NULL) { 346 const double x_plus_delta_cost = new_cost; 347 Vector inner_iteration_x = x_plus_delta; 348 Solver::Summary inner_iteration_summary; 349 options.inner_iteration_minimizer->Minimize(options, 350 inner_iteration_x.data(), 351 &inner_iteration_summary); 352 if(!evaluator->Evaluate(inner_iteration_x.data(), 353 &new_cost, 354 NULL, NULL, NULL)) { 355 VLOG(2) << "Inner iteration failed."; 356 new_cost = x_plus_delta_cost; 357 } else { 358 x_plus_delta = inner_iteration_x; 359 // Bost the model_cost_change, since the inner iteration 360 // improvements are not accounted for by the trust region. 361 model_cost_change += x_plus_delta_cost - new_cost; 362 VLOG(2) << "Inner iteration succeeded; current cost: " << cost 363 << " x_plus_delta_cost: " << x_plus_delta_cost 364 << " new_cost: " << new_cost; 365 } 366 } 367 } 368 369 iteration_summary.step_norm = (x - x_plus_delta).norm(); 370 371 // Convergence based on parameter_tolerance. 372 const double step_size_tolerance = options_.parameter_tolerance * 373 (x_norm + options_.parameter_tolerance); 374 if (iteration_summary.step_norm <= step_size_tolerance) { 375 VLOG(1) << "Terminating. Parameter tolerance reached. " 376 << "relative step_norm: " 377 << iteration_summary.step_norm / 378 (x_norm + options_.parameter_tolerance) 379 << " <= " << options_.parameter_tolerance; 380 summary->termination_type = PARAMETER_TOLERANCE; 381 return; 382 } 383 384 VLOG(2) << "old cost: " << cost << " new cost: " << new_cost; 385 iteration_summary.cost_change = cost - new_cost; 386 const double absolute_function_tolerance = 387 options_.function_tolerance * cost; 388 if (fabs(iteration_summary.cost_change) < absolute_function_tolerance) { 389 VLOG(1) << "Terminating. Function tolerance reached. " 390 << "|cost_change|/cost: " 391 << fabs(iteration_summary.cost_change) / cost 392 << " <= " << options_.function_tolerance; 393 summary->termination_type = FUNCTION_TOLERANCE; 394 return; 395 } 396 397 const double relative_decrease = 398 iteration_summary.cost_change / model_cost_change; 399 400 const double historical_relative_decrease = 401 (reference_cost - new_cost) / 402 (accumulated_reference_model_cost_change + model_cost_change); 403 404 // If monotonic steps are being used, then the relative_decrease 405 // is the usual ratio of the change in objective function value 406 // divided by the change in model cost. 407 // 408 // If non-monotonic steps are allowed, then we take the maximum 409 // of the relative_decrease and the 410 // historical_relative_decrease, which measures the increase 411 // from a reference iteration. The model cost change is 412 // estimated by accumulating the model cost changes since the 413 // reference iteration. The historical relative_decrease offers 414 // a boost to a step which is not too bad compared to the 415 // reference iteration, allowing for non-monotonic steps. 416 iteration_summary.relative_decrease = 417 options.use_nonmonotonic_steps 418 ? max(relative_decrease, historical_relative_decrease) 419 : relative_decrease; 420 421 iteration_summary.step_is_successful = 422 iteration_summary.relative_decrease > options_.min_relative_decrease; 423 424 if (iteration_summary.step_is_successful) { 425 accumulated_candidate_model_cost_change += model_cost_change; 426 accumulated_reference_model_cost_change += model_cost_change; 427 if (relative_decrease <= options_.min_relative_decrease) { 428 iteration_summary.step_is_nonmonotonic = true; 429 VLOG(2) << "Non-monotonic step! " 430 << " relative_decrease: " << relative_decrease 431 << " historical_relative_decrease: " 432 << historical_relative_decrease; 433 } 434 } 435 } 436 437 if (iteration_summary.step_is_successful) { 438 ++summary->num_successful_steps; 439 strategy->StepAccepted(iteration_summary.relative_decrease); 440 x = x_plus_delta; 441 x_norm = x.norm(); 442 443 // Step looks good, evaluate the residuals and Jacobian at this 444 // point. 445 if (!evaluator->Evaluate(x.data(), 446 &cost, 447 residuals.data(), 448 NULL, 449 jacobian)) { 450 summary->termination_type = NUMERICAL_FAILURE; 451 summary->error = "Terminating: Residual and Jacobian evaluation failed."; 452 LOG(WARNING) << summary->error; 453 return; 454 } 455 456 gradient.setZero(); 457 jacobian->LeftMultiply(residuals.data(), gradient.data()); 458 iteration_summary.gradient_max_norm = gradient.lpNorm<Eigen::Infinity>(); 459 460 if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) { 461 summary->termination_type = GRADIENT_TOLERANCE; 462 VLOG(1) << "Terminating: Gradient tolerance reached." 463 << "Relative gradient max norm: " 464 << iteration_summary.gradient_max_norm / gradient_max_norm_0 465 << " <= " << options_.gradient_tolerance; 466 return; 467 } 468 469 if (options_.jacobi_scaling) { 470 jacobian->ScaleColumns(scale.data()); 471 } 472 473 // Update the best, reference and candidate iterates. 474 // 475 // Based on algorithm 10.1.2 (page 357) of "Trust Region 476 // Methods" by Conn Gould & Toint, or equations 33-40 of 477 // "Non-monotone trust-region algorithms for nonlinear 478 // optimization subject to convex constraints" by Phil Toint, 479 // Mathematical Programming, 77, 1997. 480 if (cost < minimum_cost) { 481 // A step that improves solution quality was found. 482 x_min = x; 483 minimum_cost = cost; 484 // Set the candidate iterate to the current point. 485 candidate_cost = cost; 486 num_consecutive_nonmonotonic_steps = 0; 487 accumulated_candidate_model_cost_change = 0.0; 488 } else { 489 ++num_consecutive_nonmonotonic_steps; 490 if (cost > candidate_cost) { 491 // The current iterate is has a higher cost than the 492 // candidate iterate. Set the candidate to this point. 493 VLOG(2) << "Updating the candidate iterate to the current point."; 494 candidate_cost = cost; 495 accumulated_candidate_model_cost_change = 0.0; 496 } 497 498 // At this point we have made too many non-monotonic steps and 499 // we are going to reset the value of the reference iterate so 500 // as to force the algorithm to descend. 501 // 502 // This is the case because the candidate iterate has a value 503 // greater than minimum_cost but smaller than the reference 504 // iterate. 505 if (num_consecutive_nonmonotonic_steps == 506 options.max_consecutive_nonmonotonic_steps) { 507 VLOG(2) << "Resetting the reference point to the candidate point"; 508 reference_cost = candidate_cost; 509 accumulated_reference_model_cost_change = 510 accumulated_candidate_model_cost_change; 511 } 512 } 513 } else { 514 ++summary->num_unsuccessful_steps; 515 if (iteration_summary.step_is_valid) { 516 strategy->StepRejected(iteration_summary.relative_decrease); 517 } else { 518 strategy->StepIsInvalid(); 519 } 520 } 521 522 iteration_summary.cost = cost + summary->fixed_cost; 523 iteration_summary.trust_region_radius = strategy->Radius(); 524 if (iteration_summary.trust_region_radius < 525 options_.min_trust_region_radius) { 526 summary->termination_type = PARAMETER_TOLERANCE; 527 VLOG(1) << "Termination. Minimum trust region radius reached."; 528 return; 529 } 530 531 iteration_summary.iteration_time_in_seconds = 532 WallTimeInSeconds() - iteration_start_time; 533 iteration_summary.cumulative_time_in_seconds = 534 WallTimeInSeconds() - start_time 535 + summary->preprocessor_time_in_seconds; 536 summary->iterations.push_back(iteration_summary); 537 538 switch (RunCallbacks(iteration_summary)) { 539 case SOLVER_TERMINATE_SUCCESSFULLY: 540 summary->termination_type = USER_SUCCESS; 541 VLOG(1) << "Terminating: User callback returned USER_SUCCESS."; 542 return; 543 case SOLVER_ABORT: 544 summary->termination_type = USER_ABORT; 545 VLOG(1) << "Terminating: User callback returned USER_ABORT."; 546 return; 547 case SOLVER_CONTINUE: 548 break; 549 default: 550 LOG(FATAL) << "Unknown type of user callback status"; 551 } 552 } 553} 554 555 556} // namespace internal 557} // namespace ceres 558