diff options
Diffstat (limited to 'extern/ceres/include/ceres/tiny_solver.h')
| -rw-r--r-- | extern/ceres/include/ceres/tiny_solver.h | 368 |
1 files changed, 368 insertions, 0 deletions
diff --git a/extern/ceres/include/ceres/tiny_solver.h b/extern/ceres/include/ceres/tiny_solver.h new file mode 100644 index 00000000000..47db5824dc5 --- /dev/null +++ b/extern/ceres/include/ceres/tiny_solver.h @@ -0,0 +1,368 @@ +// 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_ |