From 310a252fb63b259c8a5730c1dfe4d387997a53e1 Mon Sep 17 00:00:00 2001
From: Sameer Agarwal <sameeragarwal@google.com>
Date: Fri, 4 Aug 2023 18:30:52 -0700
Subject: [PATCH] Deal with infinite initial cost correctly.

Previously it could be the case that a residual block could return
a residual whose squared norm overflows and generates an infinity
which we did not detect. This would then lead to the trust region
minimizer incorrectly terminating indicating convergence while
generating a cost delta of NaN.

This change adds a check for that and also does two minor cosmetic
changes.

1. Reduce the level of nesting in program_evaluator.h by adding
   an early return.
2. The error message when IterationZero fails now says that the
   Initial residual and Jacobian failed, to indicate that the
   optimizer had no chance to do any work.

Fixes https://github.com/ceres-solver/ceres-solver/issues/988

Thanks to @Ashray-g for reporting this.

Change-Id: I52ae7627a66f637135209dbb2e42935b52c8bc77
---
 internal/ceres/program_evaluator.h       | 65 ++++++++++++++----------
 internal/ceres/solver_test.cc            | 24 +++++++++
 internal/ceres/trust_region_minimizer.cc |  2 +
 3 files changed, 64 insertions(+), 27 deletions(-)

diff --git a/internal/ceres/program_evaluator.h b/internal/ceres/program_evaluator.h
index 372b2602b..6e9ce616d 100644
--- a/internal/ceres/program_evaluator.h
+++ b/internal/ceres/program_evaluator.h
@@ -256,37 +256,48 @@ class ProgramEvaluator final : public Evaluator {
           }
         });
 
-    if (!abort) {
-      // Sum the cost and gradient (if requested) from each thread.
-      (*cost) = 0.0;
+    if (abort) {
+      return false;
+    }
+
+    // Sum the cost and gradient (if requested) from each thread.
+    (*cost) = 0.0;
+    if (gradient != nullptr) {
+      auto gradient_vector = VectorRef(gradient, num_parameters_);
+      ParallelSetZero(options_.context, options_.num_threads, gradient_vector);
+    }
+
+    for (int i = 0; i < options_.num_threads; ++i) {
+      (*cost) += evaluate_scratch_[i].cost;
       if (gradient != nullptr) {
         auto gradient_vector = VectorRef(gradient, num_parameters_);
-        ParallelSetZero(
-            options_.context, options_.num_threads, gradient_vector);
-      }
-      for (int i = 0; i < options_.num_threads; ++i) {
-        (*cost) += evaluate_scratch_[i].cost;
-        if (gradient != nullptr) {
-          auto gradient_vector = VectorRef(gradient, num_parameters_);
-          ParallelAssign(
-              options_.context,
-              options_.num_threads,
-              gradient_vector,
-              gradient_vector + VectorRef(evaluate_scratch_[i].gradient.get(),
-                                          num_parameters_));
-        }
-      }
-
-      // Finalize the Jacobian if it is available.
-      // `num_parameters` is passed to the finalizer so that additional
-      // storage can be reserved for additional diagonal elements if
-      // necessary.
-      if (jacobian != nullptr) {
-        JacobianFinalizer f;
-        f(jacobian, num_parameters_);
+        ParallelAssign(
+            options_.context,
+            options_.num_threads,
+            gradient_vector,
+            gradient_vector + VectorRef(evaluate_scratch_[i].gradient.get(),
+                                        num_parameters_));
       }
     }
-    return !abort;
+
+    // It is possible that after accumulation that the cost has become infinite
+    // or a nan.
+    if (!std::isfinite(*cost)) {
+      LOG(ERROR) << "Accumulated cost = " << *cost
+                 << " is not a finite number. Evaluation failed.";
+      return false;
+    }
+
+    // Finalize the Jacobian if it is available.
+    // `num_parameters` is passed to the finalizer so that additional
+    // storage can be reserved for additional diagonal elements if
+    // necessary.
+    if (jacobian != nullptr) {
+      JacobianFinalizer f;
+      f(jacobian, num_parameters_);
+    }
+
+    return true;
   }
 
   bool Plus(const double* state,
diff --git a/internal/ceres/solver_test.cc b/internal/ceres/solver_test.cc
index a8df3406d..4f3a8bd2e 100644
--- a/internal/ceres/solver_test.cc
+++ b/internal/ceres/solver_test.cc
@@ -1202,4 +1202,28 @@ TEST(Solver, IterativeSchurOptionsAccelerateSparse) {
   EXPECT_FALSE(options.IsValid(&message));
 }
 
+class LargeCostCostFunction : public SizedCostFunction<1, 1> {
+ public:
+  bool Evaluate(double const* const* parameters,
+                double* residuals,
+                double** jacobians) const override {
+    residuals[0] = 1e300;
+    if (jacobians && jacobians[0]) {
+      jacobians[0][0] = 1.0;
+    }
+    return true;
+  }
+};
+
+TEST(Solver, LargeCostProblem) {
+  double x = 1;
+  Problem problem;
+  problem.AddResidualBlock(new LargeCostCostFunction, nullptr, &x);
+  Solver::Options options;
+  Solver::Summary summary;
+  Solve(options, &problem, &summary);
+  LOG(INFO) << summary.FullReport();
+  EXPECT_EQ(summary.termination_type, FAILURE);
+}
+
 }  // namespace ceres::internal
diff --git a/internal/ceres/trust_region_minimizer.cc b/internal/ceres/trust_region_minimizer.cc
index 47f7fd503..85d85fdfd 100644
--- a/internal/ceres/trust_region_minimizer.cc
+++ b/internal/ceres/trust_region_minimizer.cc
@@ -218,6 +218,8 @@ bool TrustRegionMinimizer::IterationZero() {
   }
 
   if (!EvaluateGradientAndJacobian(/*new_evaluation_point=*/true)) {
+    solver_summary_->message =
+        "Initial residual and Jacobian evaluation failed.";
     return false;
   }