Validate ParameterBlocks before solve.

Ensure that all parameter blocks have finite values
before the minimizer is called.

Change-Id: I15fd9c487247989626f799496bb8f5ea8728d6f0

internal/ceres/array_utils.cc

@@ -32,7 +32,10 @@
 
 #include <cmath>
 #include <cstddef>
+#include <string>
+
 #include "ceres/fpclassify.h"
+#include "ceres/stringprintf.h"
 
 namespace ceres {
 namespace internal {
@@ -55,6 +58,20 @@ bool IsArrayValid(const int size, const double* x) {
   return true;
 }
 
+int FindInvalidValue(const int size, const double* x) {
+  if (x == NULL) {
+    return size;
+  }
+
+  for (int i = 0; i < size; ++i) {
+    if (!IsFinite(x[i]) || (x[i] == kImpossibleValue))  {
+      return i;
+    }
+  }
+
+  return size;
+};
+
 void InvalidateArray(const int size, double* x) {
   if (x != NULL) {
     for (int i = 0; i < size; ++i) {
@@ -63,5 +80,19 @@ void InvalidateArray(const int size, double* x) {
   }
 }
 
+void AppendArrayToString(const int size, const double* x, string* result) {
+  for (int i = 0; i < size; ++i) {
+    if (x == NULL) {
+      StringAppendF(result, "Not Computed  ");
+    } else {
+      if (x[i] == kImpossibleValue) {
+        StringAppendF(result, "Uninitialized ");
+      } else {
+        StringAppendF(result, "%12g ", x[i]);
+      }
+    }
+  }
+}
+
 }  // namespace internal
 }  // namespace ceres

internal/ceres/array_utils.h

@@ -57,6 +57,14 @@ void InvalidateArray(int size, double* x);
 // equal to the "impossible" value used by InvalidateArray.
 bool IsArrayValid(int size, const double* x);
 
+// If the array contains an invalid value, return the index for it,
+// otherwise return size.
+int FindInvalidValue(const int size, const double* x);
+
+// Utility routine to print an array of doubles to a string. If the
+// array pointer is NULL, it is treated as an array of zeros.
+void AppendArrayToString(const int size, const double* x, string* result);
+
 extern const double kImpossibleValue;
 
 }  // namespace internal

internal/ceres/array_utils_test.cc

@@ -54,5 +54,22 @@ TEST(ArrayUtils, IsArrayValid) {
   EXPECT_FALSE(IsArrayValid(3, x));
 }
 
+TEST(ArrayUtils, FindInvalidIndex) {
+  double x[3];
+  x[0] = 0.0;
+  x[1] = 1.0;
+  x[2] = 2.0;
+  EXPECT_EQ(FindInvalidValue(3, x), 3);
+  x[1] = std::numeric_limits<double>::infinity();
+  EXPECT_EQ(FindInvalidValue(3, x), 1);
+  x[1] = std::numeric_limits<double>::quiet_NaN();
+  EXPECT_EQ(FindInvalidValue(3, x), 1);
+  x[1] = std::numeric_limits<double>::signaling_NaN();
+  EXPECT_EQ(FindInvalidValue(3, x), 1);
+  EXPECT_EQ(FindInvalidValue(1, NULL), 1);
+  InvalidateArray(3, x);
+  EXPECT_EQ(FindInvalidValue(3, x), 0);
+}
+
 }  // namespace internal
 }  // namespace ceres

internal/ceres/residual_block_utils.cc

@@ -61,24 +61,6 @@ void InvalidateEvaluation(const ResidualBlock& block,
   }
 }
 
-// Utility routine to print an array of doubles to a string. If the
-// array pointer is NULL, it is treated as an array of zeros.
-namespace {
-void AppendArrayToString(const int size, const double* x, string* result) {
-  for (int i = 0; i < size; ++i) {
-    if (x == NULL) {
-      StringAppendF(result, "Not Computed  ");
-    } else {
-      if (x[i] == kImpossibleValue) {
-        StringAppendF(result, "Uninitialized ");
-      } else {
-        StringAppendF(result, "%12g ", x[i]);
-      }
-    }
-  }
-}
-}  // namespace
-
 string EvaluationToString(const ResidualBlock& block,
                           double const* const* parameters,
                           double* cost,

internal/ceres/solver_impl.cc

@@ -224,6 +224,28 @@ void SummarizeReducedProgram(const Program& program, Solver::Summary* summary) {
   summary->num_residuals_reduced = program.NumResiduals();
 }
 
+bool ParameterBlocksAreFinite(const ProblemImpl* problem,
+                              string* message) {
+  CHECK_NOTNULL(message);
+  const Program& program = problem->program();
+  const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks();
+  for (int i = 0; i < parameter_blocks.size(); ++i) {
+    const double* array = parameter_blocks[i]->user_state();
+    const int size = parameter_blocks[i]->Size();
+    const int invalid_index = FindInvalidValue(size, array);
+    if (invalid_index != size) {
+      *message = StringPrintf(
+          "ParameterBlock: %p with size %d has at least one invalid value.\n"
+          "First invalid value is at index: %d.\n"
+          "Parameter block values: ",
+          array, size, invalid_index);
+      AppendArrayToString(size, array, message);
+      return false;
+    }
+  }
+  return true;
+}
+
 bool LineSearchOptionsAreValid(const Solver::Options& options,
                                string* message) {
   // Validate values for configuration parameters supplied by user.
@@ -419,7 +441,7 @@ void SolverImpl::Solve(const Solver::Options& options,
           << " residual blocks, "
           << problem_impl->NumResiduals()
           << " residuals.";
-
+  *CHECK_NOTNULL(summary) = Solver::Summary();
   if (options.minimizer_type == TRUST_REGION) {
     TrustRegionSolve(options, problem_impl, summary);
   } else {
@@ -440,9 +462,6 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options,
   Program* original_program = original_problem_impl->mutable_program();
   ProblemImpl* problem_impl = original_problem_impl;
 
-  // Reset the summary object to its default values.
-  *CHECK_NOTNULL(summary) = Solver::Summary();
-
   summary->minimizer_type = TRUST_REGION;
 
   SummarizeGivenProgram(*original_program, summary);
@@ -484,6 +503,11 @@ void SolverImpl::TrustRegionSolve(const Solver::Options& original_options,
     return;
   }
 
+  if (!ParameterBlocksAreFinite(problem_impl, &summary->message)) {
+    LOG(ERROR) << "Terminating: " << summary->message;
+    return;
+  }
+
   event_logger.AddEvent("Init");
 
   original_program->SetParameterBlockStatePtrsToUserStatePtrs();
@@ -704,9 +728,6 @@ void SolverImpl::LineSearchSolve(const Solver::Options& original_options,
   Program* original_program = original_problem_impl->mutable_program();
   ProblemImpl* problem_impl = original_problem_impl;
 
-  // Reset the summary object to its default values.
-  *CHECK_NOTNULL(summary) = Solver::Summary();
-
   SummarizeGivenProgram(*original_program, summary);
   summary->minimizer_type = LINE_SEARCH;
   summary->line_search_direction_type =
@@ -746,6 +767,11 @@ void SolverImpl::LineSearchSolve(const Solver::Options& original_options,
   summary->num_threads_given = original_options.num_threads;
   summary->num_threads_used = options.num_threads;
 
+  if (!ParameterBlocksAreFinite(problem_impl, &summary->message)) {
+    LOG(ERROR) << "Terminating: " << summary->message;
+    return;
+  }
+
   if (original_options.linear_solver_ordering != NULL) {
     if (!IsOrderingValid(original_options, problem_impl, &summary->message)) {
       LOG(ERROR) << summary->message;

internal/ceres/solver_impl_test.cc

@@ -1085,5 +1085,18 @@ TEST(CompactifyArray, NonContiguousRepeatingEntries) {
   EXPECT_EQ(array, expected);
 }
 
+TEST(SolverImpl, ProblemHasNanParameterBlocks) {
+  Problem problem;
+  double x[2];
+  x[0] = 1.0;
+  x[1] = std::numeric_limits<double>::quiet_NaN();
+  problem.AddResidualBlock(new MockCostFunctionBase<1, 2, 0, 0>(), NULL, x);
+  Solver::Options options;
+  Solver::Summary summary;
+  Solve(options, &problem, &summary);
+  EXPECT_EQ(summary.termination_type, FAILURE);
+  EXPECT_NE(summary.message.find("has at least one invalid value"), string::npos);
+}
+
 }  // namespace internal
 }  // namespace ceres