Add line search to the trust region minimizer.

If enabled, the TrustRegionMinimizer can now further improve the
quality of the step with a Armijo line search.

This is the first step towards adding support for bounds on
variables.

Change-Id: I453b42853cfa6ca4f75812900c13d286a473c2df

internal/ceres/minimizer.h

@@ -114,6 +114,7 @@ class Minimizer {
       callbacks = options.callbacks;
       inner_iteration_minimizer = NULL;
       inner_iteration_tolerance = options.inner_iteration_tolerance;
+      is_constrained = false;
     }
 
     int max_num_iterations;
@@ -180,6 +181,9 @@ class Minimizer {
 
     Minimizer* inner_iteration_minimizer;
     double inner_iteration_tolerance;
+
+    // Use a bounds constrained optimization algorithm.
+    bool is_constrained;
   };
 
   static bool RunCallbacks(const vector<IterationCallback*> callbacks,

internal/ceres/trust_region_minimizer.cc

@@ -44,6 +44,7 @@
 #include "ceres/file.h"
 #include "ceres/internal/eigen.h"
 #include "ceres/internal/scoped_ptr.h"
+#include "ceres/line_search.h"
 #include "ceres/linear_least_squares_problems.h"
 #include "ceres/sparse_matrix.h"
 #include "ceres/stringprintf.h"
@@ -57,6 +58,45 @@ namespace internal {
 namespace {
 // Small constant for various floating point issues.
 const double kEpsilon = 1e-12;
+
+LineSearch::Summary DoLineSearch(const Minimizer::Options& options,
+                                 const Vector& x,
+                                 const Vector& gradient,
+                                 const double cost,
+                                 const Vector& delta,
+                                 Evaluator* evaluator) {
+  LineSearchFunction line_search_function(evaluator);
+  line_search_function.Init(x, delta);
+
+  LineSearch::Options line_search_options;
+  line_search_options.interpolation_type =
+      options.line_search_interpolation_type;
+  line_search_options.min_step_size = options.min_line_search_step_size;
+  line_search_options.sufficient_decrease =
+      options.line_search_sufficient_function_decrease;
+  line_search_options.max_step_contraction =
+      options.max_line_search_step_contraction;
+  line_search_options.min_step_contraction =
+      options.min_line_search_step_contraction;
+  line_search_options.max_num_iterations =
+      options.max_num_line_search_step_size_iterations;
+  line_search_options.sufficient_curvature_decrease =
+      options.line_search_sufficient_curvature_decrease;
+  line_search_options.max_step_expansion =
+      options.max_line_search_step_expansion;
+  line_search_options.function = &line_search_function;
+
+  string message;
+  scoped_ptr<LineSearch>
+      line_search(CHECK_NOTNULL(
+                      LineSearch::Create(ceres::ARMIJO,
+                                         line_search_options,
+                                         &message)));
+  LineSearch::Summary summary;
+  line_search->Search(1.0, cost, gradient.dot(delta), &summary);
+  return summary;
+}
+
 }  // namespace
 
 // Compute a scaling vector that is used to improve the conditioning
@@ -81,24 +121,30 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options,
   double start_time = WallTimeInSeconds();
   double iteration_start_time =  start_time;
   Init(options);
+
+  Evaluator* evaluator = CHECK_NOTNULL(options_.evaluator);
+  SparseMatrix* jacobian = CHECK_NOTNULL(options_.jacobian);
+  TrustRegionStrategy* strategy = CHECK_NOTNULL(options_.trust_region_strategy);
+
   const bool is_not_silent = !options.is_silent;
 
+  // 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.
+  //
+  // TODO(sameeragarwal): Make line search available more generally.
+  const bool use_line_search = options.is_constrained;
+
   summary->termination_type = NO_CONVERGENCE;
   summary->num_successful_steps = 0;
   summary->num_unsuccessful_steps = 0;
 
-  Evaluator* evaluator = CHECK_NOTNULL(options_.evaluator);
-  SparseMatrix* jacobian = CHECK_NOTNULL(options_.jacobian);
-  TrustRegionStrategy* strategy = CHECK_NOTNULL(options_.trust_region_strategy);
-
   const int num_parameters = evaluator->NumParameters();
   const int num_effective_parameters = evaluator->NumEffectiveParameters();
   const int num_residuals = evaluator->NumResiduals();
 
-  VectorRef x_min(parameters, num_parameters);
-  Vector x = x_min;
-  double x_norm = x.norm();
-
   Vector residuals(num_residuals);
   Vector trust_region_step(num_effective_parameters);
   Vector delta(num_effective_parameters);
@@ -121,6 +167,23 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options,
   iteration_summary.linear_solver_iterations = 0;
   iteration_summary.step_solver_time_in_seconds = 0;
 
+  VectorRef x_min(parameters, num_parameters);
+  Vector x = x_min;
+  // Project onto the feasible set.
+  if (options.is_constrained) {
+    delta.setZero();
+    if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
+      summary->message = "Unable to project initial point onto the feasible set.";
+      summary->termination_type = FAILURE;
+      LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
+      return;
+    }
+    x_min = x_plus_delta;
+    x = x_plus_delta;
+  }
+
+  double x_norm = x.norm();
+
   // Do initial cost and Jacobian evaluation.
   double cost = 0.0;
   if (!evaluator->Evaluate(x.data(),
@@ -128,9 +191,9 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options,
                            residuals.data(),
                            gradient.data(),
                            jacobian)) {
-    summary->message = "Terminating: Residual and Jacobian evaluation failed.";
+    summary->message = "Residual and Jacobian evaluation failed.";
     summary->termination_type = FAILURE;
-    LOG_IF(WARNING, is_not_silent) << summary->message;
+    LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
     return;
   }
 
@@ -305,19 +368,36 @@ void TrustRegionMinimizer::Minimize(const Minimizer::Options& options,
       // Undo the Jacobian column scaling.
       delta = (trust_region_step.array() * scale.array()).matrix();
 
-      double new_cost = numeric_limits<double>::max();
-      if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
+      // Try improving the step further by using an ARMIJO line
+      // search.
+      //
+      // TODO(sameeragarwal): What happens to trust region sizing as
+      // it interacts with the line search ?
+      if (use_line_search) {
+        const LineSearch::Summary line_search_summary =
+            DoLineSearch(options, x, gradient, cost, delta, evaluator);
+        if (line_search_summary.success) {
+          delta *= line_search_summary.optimal_step_size;
+        }
+      }
+
+      double new_cost = std::numeric_limits<double>::max();
+      if (evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
+        if (!evaluator->Evaluate(x_plus_delta.data(),
+                                &new_cost,
+                                NULL,
+                                NULL,
+                                NULL)) {
+          LOG(WARNING) << "Step failed to evaluate. "
+                       << "Treating it as a step with infinite cost";
+          new_cost = numeric_limits<double>::max();
+        }
+      } else {
         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 a step with infinite cost";
-        new_cost = numeric_limits<double>::max();
-      } else {
+      }
+
+      if (new_cost < std::numeric_limits<double>::max()) {
         // Check if performing an inner iteration will make it better.
         if (inner_iterations_are_enabled) {
           ++summary->num_inner_iteration_steps;