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

@@ -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,

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

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;
   }