1 // Ceres Solver - A fast non-linear least squares minimizer 2 // Copyright 2019 Google Inc. All rights reserved. 3 // http://ceres-solver.org/ 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: mierle@gmail.com (Keir Mierle) 30 // 31 // WARNING WARNING WARNING 32 // WARNING WARNING WARNING Tiny solver is experimental and will change. 33 // WARNING WARNING WARNING 34 // 35 // A tiny least squares solver using Levenberg-Marquardt, intended for solving 36 // small dense problems with low latency and low overhead. The implementation 37 // takes care to do all allocation up front, so that no memory is allocated 38 // during solving. This is especially useful when solving many similar problems; 39 // for example, inverse pixel distortion for every pixel on a grid. 40 // 41 // Note: This code has no dependencies beyond Eigen, including on other parts of 42 // Ceres, so it is possible to take this file alone and put it in another 43 // project without the rest of Ceres. 44 // 45 // Algorithm based off of: 46 // 47 // [1] K. Madsen, H. Nielsen, O. Tingleoff. 48 // Methods for Non-linear Least Squares Problems. 49 // http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf 50 51 #ifndef CERES_PUBLIC_TINY_SOLVER_H_ 52 #define CERES_PUBLIC_TINY_SOLVER_H_ 53 54 #include <cassert> 55 #include <cmath> 56 57 #include "Eigen/Dense" 58 59 namespace ceres { 60 61 // To use tiny solver, create a class or struct that allows computing the cost 62 // function (described below). This is similar to a ceres::CostFunction, but is 63 // different to enable statically allocating all memory for the solver 64 // (specifically, enum sizes). Key parts are the Scalar typedef, the enums to 65 // describe problem sizes (needed to remove all heap allocations), and the 66 // operator() overload to evaluate the cost and (optionally) jacobians. 67 // 68 // struct TinySolverCostFunctionTraits { 69 // typedef double Scalar; 70 // enum { 71 // NUM_RESIDUALS = <int> OR Eigen::Dynamic, 72 // NUM_PARAMETERS = <int> OR Eigen::Dynamic, 73 // }; 74 // bool operator()(const double* parameters, 75 // double* residuals, 76 // double* jacobian) const; 77 // 78 // int NumResiduals() const; -- Needed if NUM_RESIDUALS == Eigen::Dynamic. 79 // int NumParameters() const; -- Needed if NUM_PARAMETERS == Eigen::Dynamic. 80 // }; 81 // 82 // For operator(), the size of the objects is: 83 // 84 // double* parameters -- NUM_PARAMETERS or NumParameters() 85 // double* residuals -- NUM_RESIDUALS or NumResiduals() 86 // double* jacobian -- NUM_RESIDUALS * NUM_PARAMETERS in column-major format 87 // (Eigen's default); or NULL if no jacobian requested. 88 // 89 // An example (fully statically sized): 90 // 91 // struct MyCostFunctionExample { 92 // typedef double Scalar; 93 // enum { 94 // NUM_RESIDUALS = 2, 95 // NUM_PARAMETERS = 3, 96 // }; 97 // bool operator()(const double* parameters, 98 // double* residuals, 99 // double* jacobian) const { 100 // residuals[0] = x + 2*y + 4*z; 101 // residuals[1] = y * z; 102 // if (jacobian) { 103 // jacobian[0 * 2 + 0] = 1; // First column (x). 104 // jacobian[0 * 2 + 1] = 0; 105 // 106 // jacobian[1 * 2 + 0] = 2; // Second column (y). 107 // jacobian[1 * 2 + 1] = z; 108 // 109 // jacobian[2 * 2 + 0] = 4; // Third column (z). 110 // jacobian[2 * 2 + 1] = y; 111 // } 112 // return true; 113 // } 114 // }; 115 // 116 // The solver supports either statically or dynamically sized cost 117 // functions. If the number of residuals is dynamic then the Function 118 // must define: 119 // 120 // int NumResiduals() const; 121 // 122 // If the number of parameters is dynamic then the Function must 123 // define: 124 // 125 // int NumParameters() const; 126 // 127 template <typename Function, 128 typename LinearSolver = 129 Eigen::LDLT<Eigen::Matrix<typename Function::Scalar, 130 Function::NUM_PARAMETERS, 131 Function::NUM_PARAMETERS>>> 132 class TinySolver { 133 public: 134 // This class needs to have an Eigen aligned operator new as it contains 135 // fixed-size Eigen types. 136 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 137 138 enum { 139 NUM_RESIDUALS = Function::NUM_RESIDUALS, 140 NUM_PARAMETERS = Function::NUM_PARAMETERS 141 }; 142 typedef typename Function::Scalar Scalar; 143 typedef typename Eigen::Matrix<Scalar, NUM_PARAMETERS, 1> Parameters; 144 145 enum Status { 146 GRADIENT_TOO_SMALL, // eps > max(J'*f(x)) 147 RELATIVE_STEP_SIZE_TOO_SMALL, // eps > ||dx|| / (||x|| + eps) 148 COST_TOO_SMALL, // eps > ||f(x)||^2 / 2 149 HIT_MAX_ITERATIONS, 150 151 // TODO(sameeragarwal): Deal with numerical failures. 152 }; 153 154 struct Options { 155 Scalar gradient_tolerance = 1e-10; // eps > max(J'*f(x)) 156 Scalar parameter_tolerance = 1e-8; // eps > ||dx|| / ||x|| 157 Scalar cost_threshold = // eps > ||f(x)|| 158 std::numeric_limits<Scalar>::epsilon(); 159 Scalar initial_trust_region_radius = 1e4; 160 int max_num_iterations = 50; 161 }; 162 163 struct Summary { 164 Scalar initial_cost = -1; // 1/2 ||f(x)||^2 165 Scalar final_cost = -1; // 1/2 ||f(x)||^2 166 Scalar gradient_max_norm = -1; // max(J'f(x)) 167 int iterations = -1; 168 Status status = HIT_MAX_ITERATIONS; 169 }; 170 Update(const Function & function,const Parameters & x)171 bool Update(const Function& function, const Parameters& x) { 172 if (!function(x.data(), error_.data(), jacobian_.data())) { 173 return false; 174 } 175 176 error_ = -error_; 177 178 // On the first iteration, compute a diagonal (Jacobi) scaling 179 // matrix, which we store as a vector. 180 if (summary.iterations == 0) { 181 // jacobi_scaling = 1 / (1 + diagonal(J'J)) 182 // 183 // 1 is added to the denominator to regularize small diagonal 184 // entries. 185 jacobi_scaling_ = 1.0 / (1.0 + jacobian_.colwise().norm().array()); 186 } 187 188 // This explicitly computes the normal equations, which is numerically 189 // unstable. Nevertheless, it is often good enough and is fast. 190 // 191 // TODO(sameeragarwal): Refactor this to allow for DenseQR 192 // factorization. 193 jacobian_ = jacobian_ * jacobi_scaling_.asDiagonal(); 194 jtj_ = jacobian_.transpose() * jacobian_; 195 g_ = jacobian_.transpose() * error_; 196 summary.gradient_max_norm = g_.array().abs().maxCoeff(); 197 cost_ = error_.squaredNorm() / 2; 198 return true; 199 } 200 Solve(const Function & function,Parameters * x_and_min)201 const Summary& Solve(const Function& function, Parameters* x_and_min) { 202 Initialize<NUM_RESIDUALS, NUM_PARAMETERS>(function); 203 assert(x_and_min); 204 Parameters& x = *x_and_min; 205 summary = Summary(); 206 summary.iterations = 0; 207 208 // TODO(sameeragarwal): Deal with failure here. 209 Update(function, x); 210 summary.initial_cost = cost_; 211 summary.final_cost = cost_; 212 213 if (summary.gradient_max_norm < options.gradient_tolerance) { 214 summary.status = GRADIENT_TOO_SMALL; 215 return summary; 216 } 217 218 if (cost_ < options.cost_threshold) { 219 summary.status = COST_TOO_SMALL; 220 return summary; 221 } 222 223 Scalar u = 1.0 / options.initial_trust_region_radius; 224 Scalar v = 2; 225 226 for (summary.iterations = 1; 227 summary.iterations < options.max_num_iterations; 228 summary.iterations++) { 229 jtj_regularized_ = jtj_; 230 const Scalar min_diagonal = 1e-6; 231 const Scalar max_diagonal = 1e32; 232 for (int i = 0; i < lm_diagonal_.rows(); ++i) { 233 lm_diagonal_[i] = std::sqrt( 234 u * std::min(std::max(jtj_(i, i), min_diagonal), max_diagonal)); 235 jtj_regularized_(i, i) += lm_diagonal_[i] * lm_diagonal_[i]; 236 } 237 238 // TODO(sameeragarwal): Check for failure and deal with it. 239 linear_solver_.compute(jtj_regularized_); 240 lm_step_ = linear_solver_.solve(g_); 241 dx_ = jacobi_scaling_.asDiagonal() * lm_step_; 242 243 // Adding parameter_tolerance to x.norm() ensures that this 244 // works if x is near zero. 245 const Scalar parameter_tolerance = 246 options.parameter_tolerance * 247 (x.norm() + options.parameter_tolerance); 248 if (dx_.norm() < parameter_tolerance) { 249 summary.status = RELATIVE_STEP_SIZE_TOO_SMALL; 250 break; 251 } 252 x_new_ = x + dx_; 253 254 // TODO(keir): Add proper handling of errors from user eval of cost 255 // functions. 256 function(&x_new_[0], &f_x_new_[0], NULL); 257 258 const Scalar cost_change = (2 * cost_ - f_x_new_.squaredNorm()); 259 260 // TODO(sameeragarwal): Better more numerically stable evaluation. 261 const Scalar model_cost_change = lm_step_.dot(2 * g_ - jtj_ * lm_step_); 262 263 // rho is the ratio of the actual reduction in error to the reduction 264 // in error that would be obtained if the problem was linear. See [1] 265 // for details. 266 Scalar rho(cost_change / model_cost_change); 267 if (rho > 0) { 268 // Accept the Levenberg-Marquardt step because the linear 269 // model fits well. 270 x = x_new_; 271 272 // TODO(sameeragarwal): Deal with failure. 273 Update(function, x); 274 if (summary.gradient_max_norm < options.gradient_tolerance) { 275 summary.status = GRADIENT_TOO_SMALL; 276 break; 277 } 278 279 if (cost_ < options.cost_threshold) { 280 summary.status = COST_TOO_SMALL; 281 break; 282 } 283 284 Scalar tmp = Scalar(2 * rho - 1); 285 u = u * std::max(1 / 3., 1 - tmp * tmp * tmp); 286 v = 2; 287 continue; 288 } 289 290 // Reject the update because either the normal equations failed to solve 291 // or the local linear model was not good (rho < 0). Instead, increase u 292 // to move closer to gradient descent. 293 u *= v; 294 v *= 2; 295 } 296 297 summary.final_cost = cost_; 298 return summary; 299 } 300 301 Options options; 302 Summary summary; 303 304 private: 305 // Preallocate everything, including temporary storage needed for solving the 306 // linear system. This allows reusing the intermediate storage across solves. 307 LinearSolver linear_solver_; 308 Scalar cost_; 309 Parameters dx_, x_new_, g_, jacobi_scaling_, lm_diagonal_, lm_step_; 310 Eigen::Matrix<Scalar, NUM_RESIDUALS, 1> error_, f_x_new_; 311 Eigen::Matrix<Scalar, NUM_RESIDUALS, NUM_PARAMETERS> jacobian_; 312 Eigen::Matrix<Scalar, NUM_PARAMETERS, NUM_PARAMETERS> jtj_, jtj_regularized_; 313 314 // The following definitions are needed for template metaprogramming. 315 template <bool Condition, typename T> 316 struct enable_if; 317 318 template <typename T> 319 struct enable_if<true, T> { 320 typedef T type; 321 }; 322 323 // The number of parameters and residuals are dynamically sized. 324 template <int R, int P> 325 typename enable_if<(R == Eigen::Dynamic && P == Eigen::Dynamic), void>::type 326 Initialize(const Function& function) { 327 Initialize(function.NumResiduals(), function.NumParameters()); 328 } 329 330 // The number of parameters is dynamically sized and the number of 331 // residuals is statically sized. 332 template <int R, int P> 333 typename enable_if<(R == Eigen::Dynamic && P != Eigen::Dynamic), void>::type 334 Initialize(const Function& function) { 335 Initialize(function.NumResiduals(), P); 336 } 337 338 // The number of parameters is statically sized and the number of 339 // residuals is dynamically sized. 340 template <int R, int P> 341 typename enable_if<(R != Eigen::Dynamic && P == Eigen::Dynamic), void>::type 342 Initialize(const Function& function) { 343 Initialize(R, function.NumParameters()); 344 } 345 346 // The number of parameters and residuals are statically sized. 347 template <int R, int P> 348 typename enable_if<(R != Eigen::Dynamic && P != Eigen::Dynamic), void>::type 349 Initialize(const Function& /* function */) {} 350 351 void Initialize(int num_residuals, int num_parameters) { 352 dx_.resize(num_parameters); 353 x_new_.resize(num_parameters); 354 g_.resize(num_parameters); 355 jacobi_scaling_.resize(num_parameters); 356 lm_diagonal_.resize(num_parameters); 357 lm_step_.resize(num_parameters); 358 error_.resize(num_residuals); 359 f_x_new_.resize(num_residuals); 360 jacobian_.resize(num_residuals, num_parameters); 361 jtj_.resize(num_parameters, num_parameters); 362 jtj_regularized_.resize(num_parameters, num_parameters); 363 } 364 }; 365 366 } // namespace ceres 367 368 #endif // CERES_PUBLIC_TINY_SOLVER_H_ 369