1// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2012 Google Inc. All rights reserved.
3// http://code.google.com/p/ceres-solver/
4//
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are met:
7//
8// * Redistributions of source code must retain the above copyright notice,
9//   this list of conditions and the following disclaimer.
10// * Redistributions in binary form must reproduce the above copyright notice,
11//   this list of conditions and the following disclaimer in the documentation
12//   and/or other materials provided with the distribution.
13// * Neither the name of Google Inc. nor the names of its contributors may be
14//   used to endorse or promote products derived from this software without
15//   specific prior written permission.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27// POSSIBILITY OF SUCH DAMAGE.
28//
29// Author: sameeragarwal@google.com (Sameer Agarwal)
30//
31// Generic loop for line search based optimization algorithms.
32//
33// This is primarily inpsired by the minFunc packaged written by Mark
34// Schmidt.
35//
36// http://www.di.ens.fr/~mschmidt/Software/minFunc.html
37//
38// For details on the theory and implementation see "Numerical
39// Optimization" by Nocedal & Wright.
40
41#include "ceres/line_search_minimizer.h"
42
43#include <algorithm>
44#include <cstdlib>
45#include <cmath>
46#include <string>
47#include <vector>
48
49#include "Eigen/Dense"
50#include "ceres/array_utils.h"
51#include "ceres/evaluator.h"
52#include "ceres/internal/eigen.h"
53#include "ceres/internal/port.h"
54#include "ceres/internal/scoped_ptr.h"
55#include "ceres/line_search.h"
56#include "ceres/line_search_direction.h"
57#include "ceres/stringprintf.h"
58#include "ceres/types.h"
59#include "ceres/wall_time.h"
60#include "glog/logging.h"
61
62namespace ceres {
63namespace internal {
64namespace {
65
66// TODO(sameeragarwal): I think there is a small bug here, in that if
67// the evaluation fails, then the state can contain garbage. Look at
68// this more carefully.
69bool Evaluate(Evaluator* evaluator,
70              const Vector& x,
71              LineSearchMinimizer::State* state,
72              string* message) {
73  if (!evaluator->Evaluate(x.data(),
74                           &(state->cost),
75                           NULL,
76                           state->gradient.data(),
77                           NULL)) {
78    *message = "Gradient evaluation failed.";
79    return false;
80  }
81
82  Vector negative_gradient = -state->gradient;
83  Vector projected_gradient_step(x.size());
84  if (!evaluator->Plus(x.data(),
85                       negative_gradient.data(),
86                       projected_gradient_step.data())) {
87    *message = "projected_gradient_step = Plus(x, -gradient) failed.";
88    return false;
89  }
90
91  state->gradient_squared_norm = (x - projected_gradient_step).squaredNorm();
92  state->gradient_max_norm =
93      (x - projected_gradient_step).lpNorm<Eigen::Infinity>();
94  return true;
95}
96
97}  // namespace
98
99void LineSearchMinimizer::Minimize(const Minimizer::Options& options,
100                                   double* parameters,
101                                   Solver::Summary* summary) {
102  const bool is_not_silent = !options.is_silent;
103  double start_time = WallTimeInSeconds();
104  double iteration_start_time =  start_time;
105
106  Evaluator* evaluator = CHECK_NOTNULL(options.evaluator);
107  const int num_parameters = evaluator->NumParameters();
108  const int num_effective_parameters = evaluator->NumEffectiveParameters();
109
110  summary->termination_type = NO_CONVERGENCE;
111  summary->num_successful_steps = 0;
112  summary->num_unsuccessful_steps = 0;
113
114  VectorRef x(parameters, num_parameters);
115
116  State current_state(num_parameters, num_effective_parameters);
117  State previous_state(num_parameters, num_effective_parameters);
118
119  Vector delta(num_effective_parameters);
120  Vector x_plus_delta(num_parameters);
121
122  IterationSummary iteration_summary;
123  iteration_summary.iteration = 0;
124  iteration_summary.step_is_valid = false;
125  iteration_summary.step_is_successful = false;
126  iteration_summary.cost_change = 0.0;
127  iteration_summary.gradient_max_norm = 0.0;
128  iteration_summary.gradient_norm = 0.0;
129  iteration_summary.step_norm = 0.0;
130  iteration_summary.linear_solver_iterations = 0;
131  iteration_summary.step_solver_time_in_seconds = 0;
132
133  // Do initial cost and Jacobian evaluation.
134  if (!Evaluate(evaluator, x, &current_state, &summary->message)) {
135    summary->termination_type = FAILURE;
136    summary->message = "Initial cost and jacobian evaluation failed. "
137        "More details: " + summary->message;
138    LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
139    return;
140  }
141
142  summary->initial_cost = current_state.cost + summary->fixed_cost;
143  iteration_summary.cost = current_state.cost + summary->fixed_cost;
144
145  iteration_summary.gradient_max_norm = current_state.gradient_max_norm;
146  iteration_summary.gradient_norm = sqrt(current_state.gradient_squared_norm);
147
148  if (iteration_summary.gradient_max_norm <= options.gradient_tolerance) {
149    summary->message = StringPrintf("Gradient tolerance reached. "
150                                    "Gradient max norm: %e <= %e",
151                                    iteration_summary.gradient_max_norm,
152                                    options.gradient_tolerance);
153    summary->termination_type = CONVERGENCE;
154    VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
155    return;
156  }
157
158  iteration_summary.iteration_time_in_seconds =
159      WallTimeInSeconds() - iteration_start_time;
160  iteration_summary.cumulative_time_in_seconds =
161      WallTimeInSeconds() - start_time
162      + summary->preprocessor_time_in_seconds;
163  summary->iterations.push_back(iteration_summary);
164
165  LineSearchDirection::Options line_search_direction_options;
166  line_search_direction_options.num_parameters = num_effective_parameters;
167  line_search_direction_options.type = options.line_search_direction_type;
168  line_search_direction_options.nonlinear_conjugate_gradient_type =
169      options.nonlinear_conjugate_gradient_type;
170  line_search_direction_options.max_lbfgs_rank = options.max_lbfgs_rank;
171  line_search_direction_options.use_approximate_eigenvalue_bfgs_scaling =
172      options.use_approximate_eigenvalue_bfgs_scaling;
173  scoped_ptr<LineSearchDirection> line_search_direction(
174      LineSearchDirection::Create(line_search_direction_options));
175
176  LineSearchFunction line_search_function(evaluator);
177
178  LineSearch::Options line_search_options;
179  line_search_options.interpolation_type =
180      options.line_search_interpolation_type;
181  line_search_options.min_step_size = options.min_line_search_step_size;
182  line_search_options.sufficient_decrease =
183      options.line_search_sufficient_function_decrease;
184  line_search_options.max_step_contraction =
185      options.max_line_search_step_contraction;
186  line_search_options.min_step_contraction =
187      options.min_line_search_step_contraction;
188  line_search_options.max_num_iterations =
189      options.max_num_line_search_step_size_iterations;
190  line_search_options.sufficient_curvature_decrease =
191      options.line_search_sufficient_curvature_decrease;
192  line_search_options.max_step_expansion =
193      options.max_line_search_step_expansion;
194  line_search_options.function = &line_search_function;
195
196  scoped_ptr<LineSearch>
197      line_search(LineSearch::Create(options.line_search_type,
198                                     line_search_options,
199                                     &summary->message));
200  if (line_search.get() == NULL) {
201    summary->termination_type = FAILURE;
202    LOG_IF(ERROR, is_not_silent) << "Terminating: " << summary->message;
203    return;
204  }
205
206  LineSearch::Summary line_search_summary;
207  int num_line_search_direction_restarts = 0;
208
209  while (true) {
210    if (!RunCallbacks(options, iteration_summary, summary)) {
211      break;
212    }
213
214    iteration_start_time = WallTimeInSeconds();
215    if (iteration_summary.iteration >= options.max_num_iterations) {
216      summary->message = "Maximum number of iterations reached.";
217      summary->termination_type = NO_CONVERGENCE;
218      VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
219      break;
220    }
221
222    const double total_solver_time = iteration_start_time - start_time +
223        summary->preprocessor_time_in_seconds;
224    if (total_solver_time >= options.max_solver_time_in_seconds) {
225      summary->message = "Maximum solver time reached.";
226      summary->termination_type = NO_CONVERGENCE;
227      VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
228      break;
229    }
230
231    iteration_summary = IterationSummary();
232    iteration_summary.iteration = summary->iterations.back().iteration + 1;
233    iteration_summary.step_is_valid = false;
234    iteration_summary.step_is_successful = false;
235
236    bool line_search_status = true;
237    if (iteration_summary.iteration == 1) {
238      current_state.search_direction = -current_state.gradient;
239    } else {
240      line_search_status = line_search_direction->NextDirection(
241          previous_state,
242          current_state,
243          &current_state.search_direction);
244    }
245
246    if (!line_search_status &&
247        num_line_search_direction_restarts >=
248        options.max_num_line_search_direction_restarts) {
249      // Line search direction failed to generate a new direction, and we
250      // have already reached our specified maximum number of restarts,
251      // terminate optimization.
252      summary->message =
253          StringPrintf("Line search direction failure: specified "
254                       "max_num_line_search_direction_restarts: %d reached.",
255                       options.max_num_line_search_direction_restarts);
256      summary->termination_type = FAILURE;
257      LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
258      break;
259    } else if (!line_search_status) {
260      // Restart line search direction with gradient descent on first iteration
261      // as we have not yet reached our maximum number of restarts.
262      CHECK_LT(num_line_search_direction_restarts,
263               options.max_num_line_search_direction_restarts);
264
265      ++num_line_search_direction_restarts;
266      LOG_IF(WARNING, is_not_silent)
267          << "Line search direction algorithm: "
268          << LineSearchDirectionTypeToString(
269              options.line_search_direction_type)
270          << ", failed to produce a valid new direction at "
271          << "iteration: " << iteration_summary.iteration
272          << ". Restarting, number of restarts: "
273          << num_line_search_direction_restarts << " / "
274          << options.max_num_line_search_direction_restarts
275          << " [max].";
276      line_search_direction.reset(
277          LineSearchDirection::Create(line_search_direction_options));
278      current_state.search_direction = -current_state.gradient;
279    }
280
281    line_search_function.Init(x, current_state.search_direction);
282    current_state.directional_derivative =
283        current_state.gradient.dot(current_state.search_direction);
284
285    // TODO(sameeragarwal): Refactor this into its own object and add
286    // explanations for the various choices.
287    //
288    // Note that we use !line_search_status to ensure that we treat cases when
289    // we restarted the line search direction equivalently to the first
290    // iteration.
291    const double initial_step_size =
292        (iteration_summary.iteration == 1 || !line_search_status)
293        ? min(1.0, 1.0 / current_state.gradient_max_norm)
294        : min(1.0, 2.0 * (current_state.cost - previous_state.cost) /
295              current_state.directional_derivative);
296    // By definition, we should only ever go forwards along the specified search
297    // direction in a line search, most likely cause for this being violated
298    // would be a numerical failure in the line search direction calculation.
299    if (initial_step_size < 0.0) {
300      summary->message =
301          StringPrintf("Numerical failure in line search, initial_step_size is "
302                       "negative: %.5e, directional_derivative: %.5e, "
303                       "(current_cost - previous_cost): %.5e",
304                       initial_step_size, current_state.directional_derivative,
305                       (current_state.cost - previous_state.cost));
306      summary->termination_type = FAILURE;
307      LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
308      break;
309    }
310
311    line_search->Search(initial_step_size,
312                        current_state.cost,
313                        current_state.directional_derivative,
314                        &line_search_summary);
315    if (!line_search_summary.success) {
316      summary->message =
317          StringPrintf("Numerical failure in line search, failed to find "
318                       "a valid step size, (did not run out of iterations) "
319                       "using initial_step_size: %.5e, initial_cost: %.5e, "
320                       "initial_gradient: %.5e.",
321                       initial_step_size, current_state.cost,
322                       current_state.directional_derivative);
323      LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
324      summary->termination_type = FAILURE;
325      break;
326    }
327
328    current_state.step_size = line_search_summary.optimal_step_size;
329    delta = current_state.step_size * current_state.search_direction;
330
331    previous_state = current_state;
332    iteration_summary.step_solver_time_in_seconds =
333        WallTimeInSeconds() - iteration_start_time;
334
335    if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data())) {
336      summary->termination_type = FAILURE;
337      summary->message =
338          "x_plus_delta = Plus(x, delta) failed. This should not happen "
339          "as the step was valid when it was selected by the line search.";
340      LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
341      break;
342    } else if (!Evaluate(evaluator,
343                         x_plus_delta,
344                         &current_state,
345                         &summary->message)) {
346      summary->termination_type = FAILURE;
347      summary->message =
348          "Step failed to evaluate. This should not happen as the step was "
349          "valid when it was selected by the line search. More details: " +
350          summary->message;
351      LOG_IF(WARNING, is_not_silent) << "Terminating: " << summary->message;
352      break;
353    } else {
354      x = x_plus_delta;
355    }
356
357    iteration_summary.gradient_max_norm = current_state.gradient_max_norm;
358    iteration_summary.gradient_norm = sqrt(current_state.gradient_squared_norm);
359    iteration_summary.cost_change = previous_state.cost - current_state.cost;
360    iteration_summary.cost = current_state.cost + summary->fixed_cost;
361    iteration_summary.step_norm = delta.norm();
362    iteration_summary.step_is_valid = true;
363    iteration_summary.step_is_successful = true;
364    iteration_summary.step_norm = delta.norm();
365    iteration_summary.step_size =  current_state.step_size;
366    iteration_summary.line_search_function_evaluations =
367        line_search_summary.num_function_evaluations;
368    iteration_summary.line_search_gradient_evaluations =
369        line_search_summary.num_gradient_evaluations;
370    iteration_summary.line_search_iterations =
371        line_search_summary.num_iterations;
372    iteration_summary.iteration_time_in_seconds =
373        WallTimeInSeconds() - iteration_start_time;
374    iteration_summary.cumulative_time_in_seconds =
375        WallTimeInSeconds() - start_time
376        + summary->preprocessor_time_in_seconds;
377
378    summary->iterations.push_back(iteration_summary);
379    ++summary->num_successful_steps;
380
381    if (iteration_summary.gradient_max_norm <= options.gradient_tolerance) {
382      summary->message = StringPrintf("Gradient tolerance reached. "
383                                      "Gradient max norm: %e <= %e",
384                                      iteration_summary.gradient_max_norm,
385                                      options.gradient_tolerance);
386      summary->termination_type = CONVERGENCE;
387      VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
388      break;
389    }
390
391    const double absolute_function_tolerance =
392        options.function_tolerance * previous_state.cost;
393    if (fabs(iteration_summary.cost_change) < absolute_function_tolerance) {
394      summary->message =
395          StringPrintf("Function tolerance reached. "
396                       "|cost_change|/cost: %e <= %e",
397                       fabs(iteration_summary.cost_change) /
398                       previous_state.cost,
399                       options.function_tolerance);
400      summary->termination_type = CONVERGENCE;
401      VLOG_IF(1, is_not_silent) << "Terminating: " << summary->message;
402      break;
403    }
404  }
405}
406
407}  // namespace internal
408}  // namespace ceres
409