Fix a bug in TrustRegionMinimizer.

LocalParameterization::Plus is allowed to return false when for some
reason the operation cannot be performed. Uptil now, this would
cause the TrustRegionMinimizer to terminate with a numerical failure.

This is not correct behaviour. Just like CostFunction::Evaluate,
returning false from LocalParameterization::Plus should result
in the current step to be rejected rather than the entire optimization
coming to a halt.

LineSearchMinimizer has also been updated to use the same pattern.

Thanks to Michael Vitus for reporting this.

Change-Id: I1351966bc6db3bf6cd46387b78d4e52a9df51696
diff --git a/internal/ceres/line_search_minimizer.cc b/internal/ceres/line_search_minimizer.cc
index 2cc89fa..5533e20 100644
--- a/internal/ceres/line_search_minimizer.cc
+++ b/internal/ceres/line_search_minimizer.cc
@@ -69,6 +69,9 @@
 // use.
 const double kEpsilon = 1e-12;
 
+// TODO(sameeragarwal): I think there is a small bug here, in that if
+// the evaluation fails, then the state can contain garbage. Look at
+// this more carefully.
 bool Evaluate(Evaluator* evaluator,
               const Vector& x,
               LineSearchMinimizer::State* state) {
@@ -310,9 +313,10 @@
         WallTimeInSeconds() - iteration_start_time;
 
     // TODO(sameeragarwal): Collect stats.
-    if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data()) ||
-        !Evaluate(evaluator, x_plus_delta, &current_state)) {
-      LOG(WARNING) << "Evaluation failed.";
+    if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
+      LOG(WARNING) << "x_plus_delta = Plus(x, delta) failed. ";
+    } else if (!Evaluate(evaluator, x_plus_delta, &current_state)) {
+      LOG(WARNING) << "Step failed to evaluate. ";
     } else {
       x = x_plus_delta;
     }
diff --git a/internal/ceres/trust_region_minimizer.cc b/internal/ceres/trust_region_minimizer.cc
index 03d6c8e..8e2bc67 100644
--- a/internal/ceres/trust_region_minimizer.cc
+++ b/internal/ceres/trust_region_minimizer.cc
@@ -287,24 +287,18 @@
 
       // Undo the Jacobian column scaling.
       delta = (trust_region_step.array() * scale.array()).matrix();
-      if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
-        summary->termination_type = NUMERICAL_FAILURE;
-        summary->error =
-            "Terminating. Failed to compute Plus(x, delta, x_plus_delta).";
 
-        LOG(WARNING) << summary->error;
-        return;
-      }
-
-      // Try this step.
       double new_cost = numeric_limits<double>::max();
-      if (!evaluator->Evaluate(x_plus_delta.data(),
-                               &new_cost,
-                               NULL, NULL, NULL)) {
-        // If the evaluation of the new cost fails, treat it as a step
-        // with high cost.
+      if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
+        LOG(WARNING) << "x_plus_delta = Plus(x, delta) failed. "
+                     << "Treating it as a step with infinite cost";
+      } else if (!evaluator->Evaluate(x_plus_delta.data(),
+                                      &new_cost,
+                                      NULL,
+                                      NULL,
+                                      NULL)) {
         LOG(WARNING) << "Step failed to evaluate. "
-                     << "Treating it as step with infinite cost";
+                     << "Treating it as a step with infinite cost";
         new_cost = numeric_limits<double>::max();
       } else {
         // Check if performing an inner iteration will make it better.