Improve infeasibility detection for constant and
variable parameter blocks.

1. Check if constant parameter blocks have infeasible values.
2. Check if variable parameter blocks have impossible bounds.

Change-Id: I0a58bc3eba42f0655fe90b58ea6ee21fae8c8f61

internal/ceres/solver_impl.cc

diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc
index 070ed80..937ab78 100644
--- a/internal/ceres/solver_impl.cc
+++ b/internal/ceres/solver_impl.cc
@@ -345,39 +345,57 @@
   return false;
 }
 
-// Returns true, if the problem has any constant parameter blocks
-// which are not feasible. This means that Ceres cannot produce a
-// feasible solution to the problem.
-bool HasInfeasibleConstantParameterBlock(const ProblemImpl* problem,
-                                         string* message) {
+// Returns false, if the problem has any constant parameter blocks
+// which are not feasible, or any variable parameter blocks which have
+// a lower bound greater than or equal to the upper bound.
+bool ParameterBlocksAreFeasible(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) {
-    if (!parameter_blocks[i]->IsConstant()) {
-      continue;
-    }
-
     const double* array = parameter_blocks[i]->user_state();
     const double* lower_bounds = parameter_blocks[i]->lower_bounds();
     const double* upper_bounds = parameter_blocks[i]->upper_bounds();
     const int size = parameter_blocks[i]->Size();
-    for (int j = 0; j < size; ++j) {
-      if (array[j] < lower_bounds[j] || array[j] > upper_bounds[j]) {
-        *message = StringPrintf(
-            "ParameterBlock: %p with size %d has at least one infeasible value."
-            "\nFirst infeasible value is at index: %d."
-            "\nLower bound: %e, value: %e, upper bound: %e"
-            "\nParameter block values: ",
-            array, size, j, lower_bounds[j], array[j], upper_bounds[j]);
-        AppendArrayToString(size, array, message);
-        return true;
+    if (parameter_blocks[i]->IsConstant()) {
+      // Constant parameter blocks must start in the feasible region
+      // to ultimately produce a feasible solution, since Ceres cannot
+      // change them.
+      for (int j = 0; j < size; ++j) {
+        if (array[j] < lower_bounds[j] || array[j] > upper_bounds[j]) {
+          *message = StringPrintf(
+              "ParameterBlock: %p with size %d has at least one infeasible value."
+              "\nFirst infeasible value is at index: %d."
+              "\nLower bound: %e, value: %e, upper bound: %e"
+              "\nParameter block values: ",
+              array, size, j, lower_bounds[j], array[j], upper_bounds[j]);
+          AppendArrayToString(size, array, message);
+          return false;
+        }
+      }
+    } else {
+      // Variable parameter blocks must have non-empty feasible
+      // regions, otherwise there is no way to produce a feasible
+      // solution.
+      for (int j = 0; j < size; ++j) {
+        if (lower_bounds[j] >= upper_bounds[j]) {
+          *message = StringPrintf(
+              "ParameterBlock: %p with size %d has at least one infeasible bound."
+              "\nFirst infeasible bound is at index: %d."
+              "\nLower bound: %e, upper bound: %e"
+              "\nParameter block values: ",
+              array, size, j, lower_bounds[j], upper_bounds[j]);
+          AppendArrayToString(size, array, message);
+          return false;
+        }
       }
     }
   }
-  return false;
+
+  return true;
 }
 
+
 }  // namespace
 
 void SolverImpl::TrustRegionMinimize(
@@ -594,7 +612,7 @@
     return;
   }
 
-  if (HasInfeasibleConstantParameterBlock(problem_impl, &summary->message)) {
+  if (!ParameterBlocksAreFeasible(problem_impl, &summary->message)) {
     LOG(ERROR) << "Terminating: " << summary->message;
     return;
   }

internal/ceres/trust_region_minimizer.cc

diff --git a/internal/ceres/trust_region_minimizer.cc b/internal/ceres/trust_region_minimizer.cc
index 72e89b9..f8ecac2 100644
--- a/internal/ceres/trust_region_minimizer.cc
+++ b/internal/ceres/trust_region_minimizer.cc
@@ -130,9 +130,9 @@
 
   // If the problem is bounds constrained, then enable the use of a
   // line search after the trust region step has been computed. This
-  // line search will automatically use a projected the test point
-  // onto the feasible set, there by guaranteeing the feasibility of
-  // the final output.
+  // line search will automatically use a projected test point onto
+  // the feasible set, there by guaranteeing the feasibility of the
+  // final output.
   //
   // TODO(sameeragarwal): Make line search available more generally.
   const bool use_line_search = options.is_constrained;