You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
376 lines
13 KiB
376 lines
13 KiB
// 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 depedencies 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 solve
|
|
// (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(); -- Needed if NUM_RESIDUALS == Eigen::Dynamic.
|
|
// int NumParameters(); -- 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 {
|
|
Options()
|
|
: gradient_tolerance(1e-10),
|
|
parameter_tolerance(1e-8),
|
|
cost_threshold(std::numeric_limits<Scalar>::epsilon()),
|
|
initial_trust_region_radius(1e4),
|
|
max_num_iterations(50) {}
|
|
Scalar gradient_tolerance; // eps > max(J'*f(x))
|
|
Scalar parameter_tolerance; // eps > ||dx|| / ||x||
|
|
Scalar cost_threshold; // eps > ||f(x)||
|
|
Scalar initial_trust_region_radius;
|
|
int max_num_iterations;
|
|
};
|
|
|
|
struct Summary {
|
|
Summary()
|
|
: initial_cost(-1),
|
|
final_cost(-1),
|
|
gradient_max_norm(-1),
|
|
iterations(0),
|
|
status(HIT_MAX_ITERATIONS) {}
|
|
|
|
Scalar initial_cost; // 1/2 ||f(x)||^2
|
|
Scalar final_cost; // 1/2 ||f(x)||^2
|
|
Scalar gradient_max_norm; // max(J'f(x))
|
|
int iterations;
|
|
Status status;
|
|
};
|
|
|
|
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();
|
|
|
|
// 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_
|