saberbud/ceres-solver
1
0
Fork 0
mirror of https://github.com/ceres-solver/ceres-solver.git synced 2026-08-29 16:40:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

Two changes to TinySolver

Browse Source 
1. Add function_tolerance based termination, updating the
   termination logic to be in line with Ceres.
2. Replace the use of "error" with "residuals" in the code.

Change-Id: I3fd543f3a8226fe7f07eeec358559cba934679b2
This commit is contained in:
Sameer Agarwal
2021-09-14 09:54:50 -07:00
parent 4dff3ea2c4
commit 8841119134
2 changed files with 63 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/ceres/tiny_solver.h
+62 -30
View File
@@ -1,5 +1,5 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2019 Google Inc. All rights reserved.
// Copyright 2021 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
@@ -84,7 +84,8 @@ namespace ceres {
// 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.
// (Eigen's default); or nullptr if no jacobian
// requested.
//
// An example (fully statically sized):
//
@@ -126,8 +127,8 @@ namespace ceres {
//
template <typename Function,
typename LinearSolver =
Eigen::LDLT<Eigen::Matrix<typename Function::Scalar,
Function::NUM_PARAMETERS,
Eigen::LDLT<Eigen::Matrix<typename Function::Scalar, //
Function::NUM_PARAMETERS, //
Function::NUM_PARAMETERS>>>
class TinySolver {
public:
@@ -143,37 +144,55 @@ class TinySolver {
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
// max_norm |J'(x) * f(x)| < gradient_tolerance
GRADIENT_TOO_SMALL,
// ||dx|| <= parameter_tolerance * (||x|| + parameter_tolerance)
RELATIVE_STEP_SIZE_TOO_SMALL,
// cost_threshold > ||f(x)||^2 / 2
COST_TOO_SMALL,
// num_iterations >= max_num_iterations
HIT_MAX_ITERATIONS,
// (new_cost - old_cost) < function_tolerance * old_cost
COST_CHANGE_TOO_SMALL,
// 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;
// max_norm |J'(x) * f(x)| < gradient_tolerance
Scalar gradient_tolerance = 1e-10;
// ||dx|| <= parameter_tolerance * (||x|| + parameter_tolerance)
Scalar parameter_tolerance = 1e-8;
// (new_cost - old_cost) < function_tolerance * old_cost
Scalar function_tolerance = 1e-6;
// cost_threshold > ||f(x)||^2 / 2
Scalar cost_threshold = std::numeric_limits<Scalar>::epsilon();
Scalar initial_trust_region_radius = 1e4;
};
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))
// 1/2 ||f(x_0)||^2
Scalar initial_cost = -1;
// 1/2 ||f(x)||^2
Scalar final_cost = -1;
// max_norm(J'f(x))
Scalar gradient_max_norm = -1;
int iterations = -1;
Status status = HIT_MAX_ITERATIONS;
};
bool Update(const Function& function, const Parameters& x) {
if (!function(x.data(), error_.data(), jacobian_.data())) {
if (!function(x.data(), residuals_.data(), jacobian_.data())) {
return false;
}
error_ = -error_;
residuals_ = -residuals_;
// On the first iteration, compute a diagonal (Jacobi) scaling
// matrix, which we store as a vector.
@@ -192,9 +211,9 @@ class TinySolver {
// factorization.
jacobian_ = jacobian_ * jacobi_scaling_.asDiagonal();
jtj_ = jacobian_.transpose() * jacobian_;
g_ = jacobian_.transpose() * error_;
g_ = jacobian_.transpose() * residuals_;
summary.gradient_max_norm = g_.array().abs().maxCoeff();
cost_ = error_.squaredNorm() / 2;
cost_ = residuals_.squaredNorm() / 2;
return true;
}
@@ -253,10 +272,9 @@ class TinySolver {
// TODO(keir): Add proper handling of errors from user eval of cost
// functions.
function(&x_new_[0], &f_x_new_[0], NULL);
function(&x_new_[0], &f_x_new_[0], nullptr);
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_);
@@ -269,6 +287,12 @@ class TinySolver {
// model fits well.
x = x_new_;
if (std::abs(cost_change) < options.function_tolerance) {
cost_ = f_x_new_.squaredNorm() / 2;
summary.status = COST_CHANGE_TOO_SMALL;
break;
}
// TODO(sameeragarwal): Deal with failure.
Update(function, x);
if (summary.gradient_max_norm < options.gradient_tolerance) {
@@ -284,14 +308,22 @@ class TinySolver {
Scalar tmp = Scalar(2 * rho - 1);
u = u * std::max(Scalar(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;
} else {
// Reject the update because either the normal equations failed to solve
// or the local linear model was not good (rho < 0).
// Additionally if the cost change is too small, then terminate.
if (std::abs(cost_change) < options.function_tolerance) {
// Terminate
summary.status = COST_CHANGE_TOO_SMALL;
break;
}
// Reduce the size of the trust region.
u *= v;
v *= 2;
}
}
summary.final_cost = cost_;
@@ -307,7 +339,7 @@ class TinySolver {
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, 1> residuals_, f_x_new_;
Eigen::Matrix<Scalar, NUM_RESIDUALS, NUM_PARAMETERS> jacobian_;
Eigen::Matrix<Scalar, NUM_PARAMETERS, NUM_PARAMETERS> jtj_, jtj_regularized_;
@@ -355,7 +387,7 @@ class TinySolver {
jacobi_scaling_.resize(num_parameters);
lm_diagonal_.resize(num_parameters);
lm_step_.resize(num_parameters);
error_.resize(num_residuals);
residuals_.resize(num_residuals);
f_x_new_.resize(num_residuals);
jacobian_.resize(num_residuals, num_parameters);
jtj_.resize(num_parameters, num_parameters);