Austin Schuh | 70cc955 | 2019-01-21 19:46:48 -0800 | [diff] [blame^] | 1 | // Ceres Solver - A fast non-linear least squares minimizer |
| 2 | // Copyright 2017 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 = Eigen::LDLT< |
| 129 | Eigen::Matrix<typename Function::Scalar, |
| 130 | Function::NUM_PARAMETERS, |
| 131 | Function::NUM_PARAMETERS>>> |
| 132 | class TinySolver { |
| 133 | public: |
| 134 | enum { |
| 135 | NUM_RESIDUALS = Function::NUM_RESIDUALS, |
| 136 | NUM_PARAMETERS = Function::NUM_PARAMETERS |
| 137 | }; |
| 138 | typedef typename Function::Scalar Scalar; |
| 139 | typedef typename Eigen::Matrix<Scalar, NUM_PARAMETERS, 1> Parameters; |
| 140 | |
| 141 | enum Status { |
| 142 | GRADIENT_TOO_SMALL, // eps > max(J'*f(x)) |
| 143 | RELATIVE_STEP_SIZE_TOO_SMALL, // eps > ||dx|| / (||x|| + eps) |
| 144 | COST_TOO_SMALL, // eps > ||f(x)||^2 / 2 |
| 145 | HIT_MAX_ITERATIONS, |
| 146 | |
| 147 | // TODO(sameeragarwal): Deal with numerical failures. |
| 148 | }; |
| 149 | |
| 150 | struct Options { |
| 151 | Scalar gradient_tolerance = 1e-10; // eps > max(J'*f(x)) |
| 152 | Scalar parameter_tolerance = 1e-8; // eps > ||dx|| / ||x|| |
| 153 | Scalar cost_threshold = // eps > ||f(x)|| |
| 154 | std::numeric_limits<Scalar>::epsilon(); |
| 155 | Scalar initial_trust_region_radius = 1e4; |
| 156 | int max_num_iterations = 50; |
| 157 | }; |
| 158 | |
| 159 | struct Summary { |
| 160 | Scalar initial_cost = -1; // 1/2 ||f(x)||^2 |
| 161 | Scalar final_cost = -1; // 1/2 ||f(x)||^2 |
| 162 | Scalar gradient_max_norm = -1; // max(J'f(x)) |
| 163 | int iterations = -1; |
| 164 | Status status = HIT_MAX_ITERATIONS; |
| 165 | }; |
| 166 | |
| 167 | bool Update(const Function& function, const Parameters &x) { |
| 168 | if (!function(x.data(), error_.data(), jacobian_.data())) { |
| 169 | return false; |
| 170 | } |
| 171 | |
| 172 | error_ = -error_; |
| 173 | |
| 174 | // On the first iteration, compute a diagonal (Jacobi) scaling |
| 175 | // matrix, which we store as a vector. |
| 176 | if (summary.iterations == 0) { |
| 177 | // jacobi_scaling = 1 / (1 + diagonal(J'J)) |
| 178 | // |
| 179 | // 1 is added to the denominator to regularize small diagonal |
| 180 | // entries. |
| 181 | jacobi_scaling_ = 1.0 / (1.0 + jacobian_.colwise().norm().array()); |
| 182 | } |
| 183 | |
| 184 | // This explicitly computes the normal equations, which is numerically |
| 185 | // unstable. Nevertheless, it is often good enough and is fast. |
| 186 | // |
| 187 | // TODO(sameeragarwal): Refactor this to allow for DenseQR |
| 188 | // factorization. |
| 189 | jacobian_ = jacobian_ * jacobi_scaling_.asDiagonal(); |
| 190 | jtj_ = jacobian_.transpose() * jacobian_; |
| 191 | g_ = jacobian_.transpose() * error_; |
| 192 | summary.gradient_max_norm = g_.array().abs().maxCoeff(); |
| 193 | cost_ = error_.squaredNorm() / 2; |
| 194 | return true; |
| 195 | } |
| 196 | |
| 197 | const Summary& Solve(const Function& function, Parameters* x_and_min) { |
| 198 | Initialize<NUM_RESIDUALS, NUM_PARAMETERS>(function); |
| 199 | assert(x_and_min); |
| 200 | Parameters& x = *x_and_min; |
| 201 | summary = Summary(); |
| 202 | summary.iterations = 0; |
| 203 | |
| 204 | // TODO(sameeragarwal): Deal with failure here. |
| 205 | Update(function, x); |
| 206 | summary.initial_cost = cost_; |
| 207 | summary.final_cost = cost_; |
| 208 | |
| 209 | if (summary.gradient_max_norm < options.gradient_tolerance) { |
| 210 | summary.status = GRADIENT_TOO_SMALL; |
| 211 | return summary; |
| 212 | } |
| 213 | |
| 214 | if (cost_ < options.cost_threshold) { |
| 215 | summary.status = COST_TOO_SMALL; |
| 216 | return summary; |
| 217 | } |
| 218 | |
| 219 | Scalar u = 1.0 / options.initial_trust_region_radius; |
| 220 | Scalar v = 2; |
| 221 | |
| 222 | for (summary.iterations = 1; |
| 223 | summary.iterations < options.max_num_iterations; |
| 224 | summary.iterations++) { |
| 225 | jtj_regularized_ = jtj_; |
| 226 | const Scalar min_diagonal = 1e-6; |
| 227 | const Scalar max_diagonal = 1e32; |
| 228 | for (int i = 0; i < lm_diagonal_.rows(); ++i) { |
| 229 | lm_diagonal_[i] = std::sqrt( |
| 230 | u * std::min(std::max(jtj_(i, i), min_diagonal), max_diagonal)); |
| 231 | jtj_regularized_(i, i) += lm_diagonal_[i] * lm_diagonal_[i]; |
| 232 | } |
| 233 | |
| 234 | // TODO(sameeragarwal): Check for failure and deal with it. |
| 235 | linear_solver_.compute(jtj_regularized_); |
| 236 | lm_step_ = linear_solver_.solve(g_); |
| 237 | dx_ = jacobi_scaling_.asDiagonal() * lm_step_; |
| 238 | |
| 239 | // Adding parameter_tolerance to x.norm() ensures that this |
| 240 | // works if x is near zero. |
| 241 | const Scalar parameter_tolerance = |
| 242 | options.parameter_tolerance * |
| 243 | (x.norm() + options.parameter_tolerance); |
| 244 | if (dx_.norm() < parameter_tolerance) { |
| 245 | summary.status = RELATIVE_STEP_SIZE_TOO_SMALL; |
| 246 | break; |
| 247 | } |
| 248 | x_new_ = x + dx_; |
| 249 | |
| 250 | // TODO(keir): Add proper handling of errors from user eval of cost |
| 251 | // functions. |
| 252 | function(&x_new_[0], &f_x_new_[0], NULL); |
| 253 | |
| 254 | const Scalar cost_change = (2 * cost_ - f_x_new_.squaredNorm()); |
| 255 | |
| 256 | // TODO(sameeragarwal): Better more numerically stable evaluation. |
| 257 | const Scalar model_cost_change = lm_step_.dot(2 * g_ - jtj_ * lm_step_); |
| 258 | |
| 259 | // rho is the ratio of the actual reduction in error to the reduction |
| 260 | // in error that would be obtained if the problem was linear. See [1] |
| 261 | // for details. |
| 262 | Scalar rho(cost_change / model_cost_change); |
| 263 | if (rho > 0) { |
| 264 | // Accept the Levenberg-Marquardt step because the linear |
| 265 | // model fits well. |
| 266 | x = x_new_; |
| 267 | |
| 268 | // TODO(sameeragarwal): Deal with failure. |
| 269 | Update(function, x); |
| 270 | if (summary.gradient_max_norm < options.gradient_tolerance) { |
| 271 | summary.status = GRADIENT_TOO_SMALL; |
| 272 | break; |
| 273 | } |
| 274 | |
| 275 | if (cost_ < options.cost_threshold) { |
| 276 | summary.status = COST_TOO_SMALL; |
| 277 | break; |
| 278 | } |
| 279 | |
| 280 | Scalar tmp = Scalar(2 * rho - 1); |
| 281 | u = u * std::max(1 / 3., 1 - tmp * tmp * tmp); |
| 282 | v = 2; |
| 283 | continue; |
| 284 | } |
| 285 | |
| 286 | // Reject the update because either the normal equations failed to solve |
| 287 | // or the local linear model was not good (rho < 0). Instead, increase u |
| 288 | // to move closer to gradient descent. |
| 289 | u *= v; |
| 290 | v *= 2; |
| 291 | } |
| 292 | |
| 293 | summary.final_cost = cost_; |
| 294 | return summary; |
| 295 | } |
| 296 | |
| 297 | Options options; |
| 298 | Summary summary; |
| 299 | |
| 300 | private: |
| 301 | // Preallocate everything, including temporary storage needed for solving the |
| 302 | // linear system. This allows reusing the intermediate storage across solves. |
| 303 | LinearSolver linear_solver_; |
| 304 | Scalar cost_; |
| 305 | Parameters dx_, x_new_, g_, jacobi_scaling_, lm_diagonal_, lm_step_; |
| 306 | Eigen::Matrix<Scalar, NUM_RESIDUALS, 1> error_, f_x_new_; |
| 307 | Eigen::Matrix<Scalar, NUM_RESIDUALS, NUM_PARAMETERS> jacobian_; |
| 308 | Eigen::Matrix<Scalar, NUM_PARAMETERS, NUM_PARAMETERS> jtj_, jtj_regularized_; |
| 309 | |
| 310 | // The following definitions are needed for template metaprogramming. |
| 311 | template <bool Condition, typename T> |
| 312 | struct enable_if; |
| 313 | |
| 314 | template <typename T> |
| 315 | struct enable_if<true, T> { |
| 316 | typedef T type; |
| 317 | }; |
| 318 | |
| 319 | // The number of parameters and residuals are dynamically sized. |
| 320 | template <int R, int P> |
| 321 | typename enable_if<(R == Eigen::Dynamic && P == Eigen::Dynamic), void>::type |
| 322 | Initialize(const Function& function) { |
| 323 | Initialize(function.NumResiduals(), function.NumParameters()); |
| 324 | } |
| 325 | |
| 326 | // The number of parameters is dynamically sized and the number of |
| 327 | // residuals is statically sized. |
| 328 | template <int R, int P> |
| 329 | typename enable_if<(R == Eigen::Dynamic && P != Eigen::Dynamic), void>::type |
| 330 | Initialize(const Function& function) { |
| 331 | Initialize(function.NumResiduals(), P); |
| 332 | } |
| 333 | |
| 334 | // The number of parameters is statically sized and the number of |
| 335 | // residuals is dynamically sized. |
| 336 | template <int R, int P> |
| 337 | typename enable_if<(R != Eigen::Dynamic && P == Eigen::Dynamic), void>::type |
| 338 | Initialize(const Function& function) { |
| 339 | Initialize(R, function.NumParameters()); |
| 340 | } |
| 341 | |
| 342 | // The number of parameters and residuals are statically sized. |
| 343 | template <int R, int P> |
| 344 | typename enable_if<(R != Eigen::Dynamic && P != Eigen::Dynamic), void>::type |
| 345 | Initialize(const Function& /* function */) {} |
| 346 | |
| 347 | void Initialize(int num_residuals, int num_parameters) { |
| 348 | dx_.resize(num_parameters); |
| 349 | x_new_.resize(num_parameters); |
| 350 | g_.resize(num_parameters); |
| 351 | jacobi_scaling_.resize(num_parameters); |
| 352 | lm_diagonal_.resize(num_parameters); |
| 353 | lm_step_.resize(num_parameters); |
| 354 | error_.resize(num_residuals); |
| 355 | f_x_new_.resize(num_residuals); |
| 356 | jacobian_.resize(num_residuals, num_parameters); |
| 357 | jtj_.resize(num_parameters, num_parameters); |
| 358 | jtj_regularized_.resize(num_parameters, num_parameters); |
| 359 | } |
| 360 | }; |
| 361 | |
| 362 | } // namespace ceres |
| 363 | |
| 364 | #endif // CERES_PUBLIC_TINY_SOLVER_H_ |