commit 5e8321c39a152df296187f0737f5d15f04a1f65e
author Sameer Agarwal <sameeragarwal@google.com> Wed Jan 22 17:23:27 2014 -0800
committer Sameer Agarwal <sameeragarwal@google.com> Wed Feb 05 11:12:16 2014 -0800
tree b30ea907586e219deb8b5540b46b4819cb1b5383
parent 63cff485b6e7f8283e32065b58f1eeef69017ebc

Validate ParameterBlocks before solve.

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

Change-Id: I15fd9c487247989626f799496bb8f5ea8728d6f0

internal/ceres/solver_impl.cc

diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc
index 1001a55..0d55c05 100644
--- a/internal/ceres/solver_impl.cc
+++ b/internal/ceres/solver_impl.cc
@@ -224,6 +224,28 @@
   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 @@
           << " 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 @@
   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 @@
     return;
   }
 
+  if (!ParameterBlocksAreFinite(problem_impl, &summary->message)) {
+    LOG(ERROR) << "Terminating: " << summary->message;
+    return;
+  }
+
   event_logger.AddEvent("Init");
 
   original_program->SetParameterBlockStatePtrsToUserStatePtrs();
@@ -704,9 +728,6 @@
   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 @@
   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;