| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368 | // Ceres Solver - A fast non-linear least squares minimizer// Copyright 2019 Google Inc. All rights reserved.// http://ceres-solver.org///// Redistribution and use in source and binary forms, with or without// modification, are permitted provided that the following conditions are met://// * Redistributions of source code must retain the above copyright notice,//   this list of conditions and the following disclaimer.// * Redistributions in binary form must reproduce the above copyright notice,//   this list of conditions and the following disclaimer in the documentation//   and/or other materials provided with the distribution.// * Neither the name of Google Inc. nor the names of its contributors may be//   used to endorse or promote products derived from this software without//   specific prior written permission.//// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE// POSSIBILITY OF SUCH DAMAGE.//// Author: mierle@gmail.com (Keir Mierle)//// WARNING WARNING WARNING// WARNING WARNING WARNING  Tiny solver is experimental and will change.// WARNING WARNING WARNING//// A tiny least squares solver using Levenberg-Marquardt, intended for solving// small dense problems with low latency and low overhead. The implementation// takes care to do all allocation up front, so that no memory is allocated// during solving. This is especially useful when solving many similar problems;// for example, inverse pixel distortion for every pixel on a grid.//// Note: This code has no dependencies beyond Eigen, including on other parts of// Ceres, so it is possible to take this file alone and put it in another// project without the rest of Ceres.//// Algorithm based off of://// [1] K. Madsen, H. Nielsen, O. Tingleoff.//     Methods for Non-linear Least Squares Problems.//     http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf#ifndef CERES_PUBLIC_TINY_SOLVER_H_#define CERES_PUBLIC_TINY_SOLVER_H_#include <cassert>#include <cmath>#include "Eigen/Dense"namespace ceres {// To use tiny solver, create a class or struct that allows computing the cost// function (described below). This is similar to a ceres::CostFunction, but is// different to enable statically allocating all memory for the solver// (specifically, enum sizes). Key parts are the Scalar typedef, the enums to// describe problem sizes (needed to remove all heap allocations), and the// operator() overload to evaluate the cost and (optionally) jacobians.////   struct TinySolverCostFunctionTraits {//     typedef double Scalar;//     enum {//       NUM_RESIDUALS = <int> OR Eigen::Dynamic,//       NUM_PARAMETERS = <int> OR Eigen::Dynamic,//     };//     bool operator()(const double* parameters,//                     double* residuals,//                     double* jacobian) const;////     int NumResiduals() const;  -- Needed if NUM_RESIDUALS == Eigen::Dynamic.//     int NumParameters() const; -- Needed if NUM_PARAMETERS == Eigen::Dynamic.//   };//// For operator(), the size of the objects is:////   double* parameters -- NUM_PARAMETERS or NumParameters()//   double* residuals  -- NUM_RESIDUALS or NumResiduals()//   double* jacobian   -- NUM_RESIDUALS * NUM_PARAMETERS in column-major format//                         (Eigen's default); or NULL if no jacobian requested.//// An example (fully statically sized):////   struct MyCostFunctionExample {//     typedef double Scalar;//     enum {//       NUM_RESIDUALS = 2,//       NUM_PARAMETERS = 3,//     };//     bool operator()(const double* parameters,//                     double* residuals,//                     double* jacobian) const {//       residuals[0] = x + 2*y + 4*z;//       residuals[1] = y * z;//       if (jacobian) {//         jacobian[0 * 2 + 0] = 1;   // First column (x).//         jacobian[0 * 2 + 1] = 0;////         jacobian[1 * 2 + 0] = 2;   // Second column (y).//         jacobian[1 * 2 + 1] = z;////         jacobian[2 * 2 + 0] = 4;   // Third column (z).//         jacobian[2 * 2 + 1] = y;//       }//       return true;//     }//   };//// The solver supports either statically or dynamically sized cost// functions. If the number of residuals is dynamic then the Function// must define:////   int NumResiduals() const;//// If the number of parameters is dynamic then the Function must// define:////   int NumParameters() const;//template <typename Function,          typename LinearSolver =              Eigen::LDLT<Eigen::Matrix<typename Function::Scalar,                                        Function::NUM_PARAMETERS,                                        Function::NUM_PARAMETERS>>>class TinySolver { public:  // This class needs to have an Eigen aligned operator new as it contains  // fixed-size Eigen types.  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  enum {    NUM_RESIDUALS = Function::NUM_RESIDUALS,    NUM_PARAMETERS = Function::NUM_PARAMETERS  };  typedef typename Function::Scalar Scalar;  typedef typename Eigen::Matrix<Scalar, NUM_PARAMETERS, 1> Parameters;  enum Status {    GRADIENT_TOO_SMALL,            // eps > max(J'*f(x))    RELATIVE_STEP_SIZE_TOO_SMALL,  // eps > ||dx|| / (||x|| + eps)    COST_TOO_SMALL,                // eps > ||f(x)||^2 / 2    HIT_MAX_ITERATIONS,    // TODO(sameeragarwal): Deal with numerical failures.  };  struct Options {    Scalar gradient_tolerance = 1e-10;  // eps > max(J'*f(x))    Scalar parameter_tolerance = 1e-8;  // eps > ||dx|| / ||x||    Scalar cost_threshold =             // eps > ||f(x)||        std::numeric_limits<Scalar>::epsilon();    Scalar initial_trust_region_radius = 1e4;    int max_num_iterations = 50;  };  struct Summary {    Scalar initial_cost = -1;       // 1/2 ||f(x)||^2    Scalar final_cost = -1;         // 1/2 ||f(x)||^2    Scalar gradient_max_norm = -1;  // max(J'f(x))    int iterations = -1;    Status status = HIT_MAX_ITERATIONS;  };  bool Update(const Function& function, const Parameters& x) {    if (!function(x.data(), error_.data(), jacobian_.data())) {      return false;    }    error_ = -error_;    // On the first iteration, compute a diagonal (Jacobi) scaling    // matrix, which we store as a vector.    if (summary.iterations == 0) {      // jacobi_scaling = 1 / (1 + diagonal(J'J))      //      // 1 is added to the denominator to regularize small diagonal      // entries.      jacobi_scaling_ = 1.0 / (1.0 + jacobian_.colwise().norm().array());    }    // This explicitly computes the normal equations, which is numerically    // unstable. Nevertheless, it is often good enough and is fast.    //    // TODO(sameeragarwal): Refactor this to allow for DenseQR    // factorization.    jacobian_ = jacobian_ * jacobi_scaling_.asDiagonal();    jtj_ = jacobian_.transpose() * jacobian_;    g_ = jacobian_.transpose() * error_;    summary.gradient_max_norm = g_.array().abs().maxCoeff();    cost_ = error_.squaredNorm() / 2;    return true;  }  const Summary& Solve(const Function& function, Parameters* x_and_min) {    Initialize<NUM_RESIDUALS, NUM_PARAMETERS>(function);    assert(x_and_min);    Parameters& x = *x_and_min;    summary = Summary();    summary.iterations = 0;    // TODO(sameeragarwal): Deal with failure here.    Update(function, x);    summary.initial_cost = cost_;    summary.final_cost = cost_;    if (summary.gradient_max_norm < options.gradient_tolerance) {      summary.status = GRADIENT_TOO_SMALL;      return summary;    }    if (cost_ < options.cost_threshold) {      summary.status = COST_TOO_SMALL;      return summary;    }    Scalar u = 1.0 / options.initial_trust_region_radius;    Scalar v = 2;    for (summary.iterations = 1;         summary.iterations < options.max_num_iterations;         summary.iterations++) {      jtj_regularized_ = jtj_;      const Scalar min_diagonal = 1e-6;      const Scalar max_diagonal = 1e32;      for (int i = 0; i < lm_diagonal_.rows(); ++i) {        lm_diagonal_[i] = std::sqrt(            u * std::min(std::max(jtj_(i, i), min_diagonal), max_diagonal));        jtj_regularized_(i, i) += lm_diagonal_[i] * lm_diagonal_[i];      }      // TODO(sameeragarwal): Check for failure and deal with it.      linear_solver_.compute(jtj_regularized_);      lm_step_ = linear_solver_.solve(g_);      dx_ = jacobi_scaling_.asDiagonal() * lm_step_;      // Adding parameter_tolerance to x.norm() ensures that this      // works if x is near zero.      const Scalar parameter_tolerance =          options.parameter_tolerance *          (x.norm() + options.parameter_tolerance);      if (dx_.norm() < parameter_tolerance) {        summary.status = RELATIVE_STEP_SIZE_TOO_SMALL;        break;      }      x_new_ = x + dx_;      // TODO(keir): Add proper handling of errors from user eval of cost      // functions.      function(&x_new_[0], &f_x_new_[0], NULL);      const Scalar cost_change = (2 * cost_ - f_x_new_.squaredNorm());      // TODO(sameeragarwal): Better more numerically stable evaluation.      const Scalar model_cost_change = lm_step_.dot(2 * g_ - jtj_ * lm_step_);      // rho is the ratio of the actual reduction in error to the reduction      // in error that would be obtained if the problem was linear. See [1]      // for details.      Scalar rho(cost_change / model_cost_change);      if (rho > 0) {        // Accept the Levenberg-Marquardt step because the linear        // model fits well.        x = x_new_;        // TODO(sameeragarwal): Deal with failure.        Update(function, x);        if (summary.gradient_max_norm < options.gradient_tolerance) {          summary.status = GRADIENT_TOO_SMALL;          break;        }        if (cost_ < options.cost_threshold) {          summary.status = COST_TOO_SMALL;          break;        }        Scalar tmp = Scalar(2 * rho - 1);        u = u * std::max(1 / 3., 1 - tmp * tmp * tmp);        v = 2;        continue;      }      // Reject the update because either the normal equations failed to solve      // or the local linear model was not good (rho < 0). Instead, increase u      // to move closer to gradient descent.      u *= v;      v *= 2;    }    summary.final_cost = cost_;    return summary;  }  Options options;  Summary summary; private:  // Preallocate everything, including temporary storage needed for solving the  // linear system. This allows reusing the intermediate storage across solves.  LinearSolver linear_solver_;  Scalar cost_;  Parameters dx_, x_new_, g_, jacobi_scaling_, lm_diagonal_, lm_step_;  Eigen::Matrix<Scalar, NUM_RESIDUALS, 1> error_, f_x_new_;  Eigen::Matrix<Scalar, NUM_RESIDUALS, NUM_PARAMETERS> jacobian_;  Eigen::Matrix<Scalar, NUM_PARAMETERS, NUM_PARAMETERS> jtj_, jtj_regularized_;  // The following definitions are needed for template metaprogramming.  template <bool Condition, typename T>  struct enable_if;  template <typename T>  struct enable_if<true, T> {    typedef T type;  };  // The number of parameters and residuals are dynamically sized.  template <int R, int P>  typename enable_if<(R == Eigen::Dynamic && P == Eigen::Dynamic), void>::type  Initialize(const Function& function) {    Initialize(function.NumResiduals(), function.NumParameters());  }  // The number of parameters is dynamically sized and the number of  // residuals is statically sized.  template <int R, int P>  typename enable_if<(R == Eigen::Dynamic && P != Eigen::Dynamic), void>::type  Initialize(const Function& function) {    Initialize(function.NumResiduals(), P);  }  // The number of parameters is statically sized and the number of  // residuals is dynamically sized.  template <int R, int P>  typename enable_if<(R != Eigen::Dynamic && P == Eigen::Dynamic), void>::type  Initialize(const Function& function) {    Initialize(R, function.NumParameters());  }  // The number of parameters and residuals are statically sized.  template <int R, int P>  typename enable_if<(R != Eigen::Dynamic && P != Eigen::Dynamic), void>::type  Initialize(const Function& /* function */) {}  void Initialize(int num_residuals, int num_parameters) {    dx_.resize(num_parameters);    x_new_.resize(num_parameters);    g_.resize(num_parameters);    jacobi_scaling_.resize(num_parameters);    lm_diagonal_.resize(num_parameters);    lm_step_.resize(num_parameters);    error_.resize(num_residuals);    f_x_new_.resize(num_residuals);    jacobian_.resize(num_residuals, num_parameters);    jtj_.resize(num_parameters, num_parameters);    jtj_regularized_.resize(num_parameters, num_parameters);  }};}  // namespace ceres#endif  // CERES_PUBLIC_TINY_SOLVER_H_
 |