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/examples/nist.cc b/examples/nist.cc index 70897ac..1456612 100644 --- a/examples/nist.cc +++ b/examples/nist.cc
@@ -622,6 +622,7 @@ std::numeric_limits<double>::epsilon(); solver.options.parameter_tolerance = std::numeric_limits<double>::epsilon(); + solver.options.function_tolerance = 0.0; Eigen::Matrix<double, num_parameters, 1> x; x = initial_parameters.transpose();
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);