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/levenberg_marquardt_strategy.h" 32 33 #include <cmath> 34 #include "Eigen/Core" 35 #include "ceres/array_utils.h" 36 #include "ceres/internal/eigen.h" 37 #include "ceres/linear_solver.h" 38 #include "ceres/sparse_matrix.h" 39 #include "ceres/trust_region_strategy.h" 40 #include "ceres/types.h" 41 #include "glog/logging.h" 42 43 namespace ceres { 44 namespace internal { 45 46 LevenbergMarquardtStrategy::LevenbergMarquardtStrategy( 47 const TrustRegionStrategy::Options& options) 48 : linear_solver_(options.linear_solver), 49 radius_(options.initial_radius), 50 max_radius_(options.max_radius), 51 min_diagonal_(options.lm_min_diagonal), 52 max_diagonal_(options.lm_max_diagonal), 53 decrease_factor_(2.0), 54 reuse_diagonal_(false) { 55 CHECK_NOTNULL(linear_solver_); 56 CHECK_GT(min_diagonal_, 0.0); 57 CHECK_LE(min_diagonal_, max_diagonal_); 58 CHECK_GT(max_radius_, 0.0); 59 } 60 61 LevenbergMarquardtStrategy::~LevenbergMarquardtStrategy() { 62 } 63 64 TrustRegionStrategy::Summary LevenbergMarquardtStrategy::ComputeStep( 65 const TrustRegionStrategy::PerSolveOptions& per_solve_options, 66 SparseMatrix* jacobian, 67 const double* residuals, 68 double* step) { 69 CHECK_NOTNULL(jacobian); 70 CHECK_NOTNULL(residuals); 71 CHECK_NOTNULL(step); 72 73 const int num_parameters = jacobian->num_cols(); 74 if (!reuse_diagonal_) { 75 if (diagonal_.rows() != num_parameters) { 76 diagonal_.resize(num_parameters, 1); 77 } 78 79 jacobian->SquaredColumnNorm(diagonal_.data()); 80 for (int i = 0; i < num_parameters; ++i) { 81 diagonal_[i] = min(max(diagonal_[i], min_diagonal_), max_diagonal_); 82 } 83 } 84 85 lm_diagonal_ = (diagonal_ / radius_).array().sqrt(); 86 87 LinearSolver::PerSolveOptions solve_options; 88 solve_options.D = lm_diagonal_.data(); 89 solve_options.q_tolerance = per_solve_options.eta; 90 // Disable r_tolerance checking. Since we only care about 91 // termination via the q_tolerance. As Nash and Sofer show, 92 // r_tolerance based termination is essentially useless in 93 // Truncated Newton methods. 94 solve_options.r_tolerance = -1.0; 95 96 // Invalidate the output array lm_step, so that we can detect if 97 // the linear solver generated numerical garbage. This is known 98 // to happen for the DENSE_QR and then DENSE_SCHUR solver when 99 // the Jacobin is severly rank deficient and mu is too small. 100 InvalidateArray(num_parameters, step); 101 102 // Instead of solving Jx = -r, solve Jy = r. 103 // Then x can be found as x = -y, but the inputs jacobian and residuals 104 // do not need to be modified. 105 LinearSolver::Summary linear_solver_summary = 106 linear_solver_->Solve(jacobian, residuals, solve_options, step); 107 if (linear_solver_summary.termination_type == FAILURE || 108 !IsArrayValid(num_parameters, step)) { 109 LOG(WARNING) << "Linear solver failure. Failed to compute a finite step."; 110 linear_solver_summary.termination_type = FAILURE; 111 } else { 112 VectorRef(step, num_parameters) *= -1.0; 113 } 114 115 reuse_diagonal_ = true; 116 117 TrustRegionStrategy::Summary summary; 118 summary.residual_norm = linear_solver_summary.residual_norm; 119 summary.num_iterations = linear_solver_summary.num_iterations; 120 summary.termination_type = linear_solver_summary.termination_type; 121 return summary; 122 } 123 124 void LevenbergMarquardtStrategy::StepAccepted(double step_quality) { 125 CHECK_GT(step_quality, 0.0); 126 radius_ = radius_ / std::max(1.0 / 3.0, 127 1.0 - pow(2.0 * step_quality - 1.0, 3)); 128 radius_ = std::min(max_radius_, radius_); 129 decrease_factor_ = 2.0; 130 reuse_diagonal_ = false; 131 } 132 133 void LevenbergMarquardtStrategy::StepRejected(double step_quality) { 134 radius_ = radius_ / decrease_factor_; 135 decrease_factor_ *= 2.0; 136 reuse_diagonal_ = true; 137 } 138 139 double LevenbergMarquardtStrategy::Radius() const { 140 return radius_; 141 } 142 143 } // namespace internal 144 } // namespace ceres 145
