1 files changed, 67 insertions, 35 deletions

diff --git a/extern/ceres/include/ceres/tiny_solver.h b/extern/ceres/include/ceres/tiny_solver.h
index 47db5824dc5..86a655dc07d 100644
--- a/extern/ceres/include/ceres/tiny_solver.h
+++ b/extern/ceres/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 @@ 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:
@@ -139,41 +140,59 @@ class TinySolver {
     NUM_RESIDUALS = Function::NUM_RESIDUALS,
     NUM_PARAMETERS = Function::NUM_PARAMETERS
   };
-  typedef typename Function::Scalar Scalar;
-  typedef typename Eigen::Matrix<Scalar, NUM_PARAMETERS, 1> Parameters;
+  using Scalar = typename Function::Scalar;
+  using Parameters = typename Eigen::Matrix<Scalar, NUM_PARAMETERS, 1>;
 
   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;
   }
 
@@ -231,7 +250,7 @@ class TinySolver {
       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));
+            u * (std::min)((std::max)(jtj_(i, i), min_diagonal), max_diagonal));
         jtj_regularized_(i, i) += lm_diagonal_[i] * lm_diagonal_[i];
       }
 
@@ -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) {
@@ -282,16 +306,24 @@ class TinySolver {
         }
 
         Scalar tmp = Scalar(2 * rho - 1);
-        u = u * std::max(1 / 3., 1 - tmp * tmp * tmp);
+        u = u * (std::max)(Scalar(1 / 3.), Scalar(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_;
 
@@ -317,7 +349,7 @@ class TinySolver {
 
   template <typename T>
   struct enable_if<true, T> {
-    typedef T type;
+    using type = T;
   };
 
   // The number of parameters and residuals are dynamically sized.
@@ -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);