Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/include/ceres/tiny_solver.h b/include/ceres/tiny_solver.h
new file mode 100644
index 0000000..b6484fe
--- /dev/null
+++ b/include/ceres/tiny_solver.h
@@ -0,0 +1,364 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2017 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:
+ 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_