Two changes to TinySolver
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
diff --git a/include/ceres/tiny_solver.h b/include/ceres/tiny_solver.h
index 0957f78..a8e699b 100644
--- a/include/ceres/tiny_solver.h
+++ b/include/ceres/tiny_solver.h
@@ -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 @@
// 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 @@
//
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 @@
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 @@
// 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 @@
// 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 @@
// 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 @@
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 @@
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 @@
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);
