diff --git a/include/ceres/iteration_callback.h b/include/ceres/iteration_callback.h index 57cf0a644..0dc4c96b4 100644 --- a/include/ceres/iteration_callback.h +++ b/include/ceres/iteration_callback.h @@ -52,6 +52,8 @@ struct IterationSummary { gradient_max_norm(0.0), step_norm(0.0), eta(0.0), + step_size(0.0), + line_search_function_evaluations(0), linear_solver_iterations(0), iteration_time_in_seconds(0.0), step_solver_time_in_seconds(0.0), @@ -116,6 +118,12 @@ struct IterationSummary { // ignore it. double eta; + // Step sized computed by the line search algorithm. + double step_size; + + // Number of function evaluations used by the line search algorithm. + int line_search_function_evaluations; + // Number of iterations taken by the linear solver to solve for the // Newton step. int linear_solver_iterations; diff --git a/include/ceres/solver.h b/include/ceres/solver.h index 62fb08760..8aa91ee8f 100644 --- a/include/ceres/solver.h +++ b/include/ceres/solver.h @@ -58,6 +58,10 @@ class Solver { struct Options { // Default constructor that sets up a generic sparse problem. Options() { + minimizer_type = TRUST_REGION; + line_search_direction_type = STEEPEST_DESCENT; + line_search_type = ARMIJO; + nonlinear_conjugate_gradient_type = FLETCHER_REEVES; trust_region_strategy_type = LEVENBERG_MARQUARDT; dogleg_type = TRADITIONAL_DOGLEG; use_nonmonotonic_steps = false; @@ -122,6 +126,35 @@ class Solver { ~Options(); // Minimizer options ---------------------------------------- + // Ceres supports the two major families of optimization strategies - + // Trust Region and Line Search. + // + // 1. The line search approach first finds a descent direction + // along which the objective function will be reduced and then + // computes a step size that decides how far should move along + // that direction. The descent direction can be computed by + // various methods, such as gradient descent, Newton's method and + // Quasi-Newton method. The step size can be determined either + // exactly or inexactly. + // + // 2. The trust region approach approximates the objective + // function using using a model function (often a quadratic) over + // a subset of the search space known as the trust region. If the + // model function succeeds in minimizing the true objective + // function the trust region is expanded; conversely, otherwise it + // is contracted and the model optimization problem is solved + // again. + // + // Trust region methods are in some sense dual to line search methods: + // trust region methods first choose a step size (the size of the + // trust region) and then a step direction while line search methods + // first choose a step direction and then a step size. + MinimizerType minimizer_type; + + LineSearchDirectionType line_search_direction_type; + LineSearchType line_search_type; + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; + TrustRegionStrategyType trust_region_strategy_type; // Type of dogleg strategy to use. diff --git a/include/ceres/types.h b/include/ceres/types.h index 888e9b085..60e076a12 100644 --- a/include/ceres/types.h +++ b/include/ceres/types.h @@ -152,6 +152,41 @@ enum LoggingType { PER_MINIMIZER_ITERATION }; +enum MinimizerType { + LINE_SEARCH, + TRUST_REGION +}; + +enum LineSearchDirectionType { + // Negative of the gradient. + STEEPEST_DESCENT, + + // A generalization of the Conjugate Gradient method to non-linear + // functions. The generalization can be performed in a number of + // different ways, resulting in a variety of search directions. The + // precise choice of the non-linear conjugate gradient algorithm + // used is determined by NonlineConjuateGradientType. + NONLINEAR_CONJUGATE_GRADIENT, +}; + +// Nonliner conjugate gradient methods are a generalization of the +// method of Conjugate Gradients for linear systems. The +// generalization can be carried out in a number of different ways +// leading to number of different rules for computing the search +// direction. Ceres provides a number of different variants. For more +// details see Numerical Optimization by Nocedal & Wright. +enum NonlinearConjugateGradientType { + FLETCHER_REEVES, + POLAK_RIBIRERE, + HESTENES_STIEFEL +}; + +enum LineSearchType { + // Backtracking line search with polynomial interpolation or + // bisection. + ARMIJO +}; + // Ceres supports different strategies for computing the trust region // step. enum TrustRegionStrategyType { diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index f48189f99..9f5767c5e 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt @@ -59,6 +59,7 @@ SET(CERES_INTERNAL_SRC iterative_schur_complement_solver.cc levenberg_marquardt_strategy.cc line_search.cc + line_search_minimizer.cc linear_least_squares_problems.cc linear_operator.cc linear_solver.cc diff --git a/internal/ceres/line_search.cc b/internal/ceres/line_search.cc index fbb75a067..0ab9a8299 100644 --- a/internal/ceres/line_search.cc +++ b/internal/ceres/line_search.cc @@ -80,32 +80,36 @@ void LineSearchFunction::Init(const Vector& position, bool LineSearchFunction::Evaluate(const double x, double* f, double* g) { scaled_direction_ = x * direction_; - if (evaluator_->Plus(position_.data(), - scaled_direction_.data(), - evaluation_point_.data()) && - evaluator_->Evaluate(evaluation_point_.data(), - f, - NULL, - gradient_.data(), NULL)) { - *g = direction_.dot(gradient_); - return IsFinite(*f) && IsFinite(*g); + if (!evaluator_->Plus(position_.data(), + scaled_direction_.data(), + evaluation_point_.data())) { + return false; } - return false; + + if (g == NULL) { + return (evaluator_->Evaluate(evaluation_point_.data(), + f, NULL, NULL, NULL) && + IsFinite(*f)); + } + + if (!evaluator_->Evaluate(evaluation_point_.data(), + f, + NULL, + gradient_.data(), NULL)) { + return false; + } + + *g = direction_.dot(gradient_); + return IsFinite(*f) && IsFinite(*g); } void ArmijoLineSearch::Search(const LineSearch::Options& options, - double initial_step_size, + const double initial_step_size, + const double initial_cost, + const double initial_gradient, Summary* summary) { *CHECK_NOTNULL(summary) = LineSearch::Summary(); Function* function = options.function; - double initial_cost = 0.0; - double initial_gradient = 0.0; - summary->num_evaluations = 1; - if (!function->Evaluate(0.0, &initial_cost, &initial_gradient)) { - LOG(WARNING) << "Line search failed. " - << "Evaluation at the initial point failed."; - return; - } double previous_step_size = 0.0; double previous_cost = 0.0; @@ -118,7 +122,10 @@ void ArmijoLineSearch::Search(const LineSearch::Options& options, bool step_size_is_valid = false; ++summary->num_evaluations; - step_size_is_valid = function->Evaluate(step_size, &cost, &gradient); + step_size_is_valid = + function->Evaluate(step_size, + &cost, + options.interpolation_degree < 2 ? NULL : &gradient); while (!step_size_is_valid || cost > (initial_cost + options.sufficient_decrease * initial_gradient @@ -148,7 +155,7 @@ void ArmijoLineSearch::Search(const LineSearch::Options& options, samples.push_back(ValueSample(step_size, cost)); if (options.use_higher_degree_interpolation_when_possible && - summary->num_evaluations > 2 && + summary->num_evaluations > 1 && previous_step_size_is_valid) { // Three point interpolation, using function values and the // initial gradient. @@ -161,7 +168,7 @@ void ArmijoLineSearch::Search(const LineSearch::Options& options, gradient)); if (options.use_higher_degree_interpolation_when_possible && - summary->num_evaluations > 2 && + summary->num_evaluations > 1 && previous_step_size_is_valid) { // Three point interpolation using the function values and // the gradients. @@ -190,7 +197,10 @@ void ArmijoLineSearch::Search(const LineSearch::Options& options, } ++summary->num_evaluations; - step_size_is_valid = function->Evaluate(step_size, &cost, &gradient); + step_size_is_valid = + function->Evaluate(step_size, + &cost, + options.interpolation_degree < 2 ? NULL : &gradient); } summary->optimal_step_size = step_size; diff --git a/internal/ceres/line_search.h b/internal/ceres/line_search.h index bc4332915..fccf63b59 100644 --- a/internal/ceres/line_search.h +++ b/internal/ceres/line_search.h @@ -136,7 +136,7 @@ class LineSearch { // // g is the gradient f'(x) at x. // - // Both f and g must not be NULL; + // f must not be null. The gradient is computed only if g is not null. virtual bool Evaluate(double x, double* f, double* g) = 0; }; @@ -156,12 +156,18 @@ class LineSearch { // Perform the line search. // - // initial_step_size must be a positive number. summary must not be - // null and will contain the result of the line search. + // initial_step_size must be a positive number. + // + // initial_cost and initial_gradient are the values and gradient of + // the function at zero. + // summary must not be null and will contain the result of the line + // search. // // Summary::success is true if a non-zero step size is found. virtual void Search(const LineSearch::Options& options, double initial_step_size, + double initial_cost, + double initial_gradient, Summary* summary) = 0; }; @@ -195,6 +201,8 @@ class ArmijoLineSearch : public LineSearch { virtual ~ArmijoLineSearch() {} virtual void Search(const LineSearch::Options& options, double initial_step_size, + double initial_cost, + double initial_gradient, Summary* summary); }; diff --git a/internal/ceres/line_search_minimizer.cc b/internal/ceres/line_search_minimizer.cc new file mode 100644 index 000000000..5a36edbfc --- /dev/null +++ b/internal/ceres/line_search_minimizer.cc @@ -0,0 +1,366 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Generic loop for line search based optimization algorithms. +// +// This is primarily inpsired by the minFunc packaged written by Mark +// Schmidt. +// +// http://www.di.ens.fr/~mschmidt/Software/minFunc.html +// +// For details on the theory and implementation see "Numerical +// Optimization" by Nocedal & Wright. + +#include "ceres/line_search_minimizer.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Eigen/Dense" +#include "ceres/array_utils.h" +#include "ceres/evaluator.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/line_search.h" +#include "ceres/stringprintf.h" +#include "ceres/types.h" +#include "ceres/wall_time.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { +namespace { +// Small constant for various floating point issues. +const double kEpsilon = 1e-12; +} // namespace + +// Execute the list of IterationCallbacks sequentially. If any one of +// the callbacks does not return SOLVER_CONTINUE, then stop and return +// its status. +CallbackReturnType LineSearchMinimizer::RunCallbacks( + const IterationSummary& iteration_summary) { + for (int i = 0; i < options_.callbacks.size(); ++i) { + const CallbackReturnType status = + (*options_.callbacks[i])(iteration_summary); + if (status != SOLVER_CONTINUE) { + return status; + } + } + return SOLVER_CONTINUE; +} + +void LineSearchMinimizer::Init(const Minimizer::Options& options) { + options_ = options; +} + +void LineSearchMinimizer::Minimize(const Minimizer::Options& options, + double* parameters, + Solver::Summary* summary) { + double start_time = WallTimeInSeconds(); + double iteration_start_time = start_time; + Init(options); + + Evaluator* evaluator = CHECK_NOTNULL(options_.evaluator); + const int num_parameters = evaluator->NumParameters(); + const int num_effective_parameters = evaluator->NumEffectiveParameters(); + + summary->termination_type = NO_CONVERGENCE; + summary->num_successful_steps = 0; + summary->num_unsuccessful_steps = 0; + + VectorRef x(parameters, num_parameters); + + Vector gradient(num_effective_parameters); + double gradient_squared_norm; + Vector previous_gradient(num_effective_parameters); + Vector gradient_change(num_effective_parameters); + double previous_gradient_squared_norm = 0.0; + + Vector search_direction(num_effective_parameters); + Vector previous_search_direction(num_effective_parameters); + + Vector delta(num_effective_parameters); + Vector x_plus_delta(num_parameters); + + double directional_derivative = 0.0; + double previous_directional_derivative = 0.0; + + IterationSummary iteration_summary; + iteration_summary.iteration = 0; + iteration_summary.step_is_valid = false; + iteration_summary.step_is_successful = false; + iteration_summary.cost_change = 0.0; + iteration_summary.gradient_max_norm = 0.0; + iteration_summary.step_norm = 0.0; + iteration_summary.linear_solver_iterations = 0; + iteration_summary.step_solver_time_in_seconds = 0; + + // Do initial cost and Jacobian evaluation. + double cost = 0.0; + double previous_cost = 0.0; + if (!evaluator->Evaluate(x.data(), &cost, NULL, gradient.data(), NULL)) { + LOG(WARNING) << "Terminating: Cost and gradient evaluation failed."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + gradient_squared_norm = gradient.squaredNorm(); + iteration_summary.cost = cost + summary->fixed_cost; + iteration_summary.gradient_max_norm = gradient.lpNorm(); + + // The initial gradient max_norm is bounded from below so that we do + // not divide by zero. + const double gradient_max_norm_0 = + max(iteration_summary.gradient_max_norm, kEpsilon); + const double absolute_gradient_tolerance = + options_.gradient_tolerance * gradient_max_norm_0; + + if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) { + summary->termination_type = GRADIENT_TOLERANCE; + VLOG(1) << "Terminating: Gradient tolerance reached." + << "Relative gradient max norm: " + << iteration_summary.gradient_max_norm / gradient_max_norm_0 + << " <= " << options_.gradient_tolerance; + return; + } + + iteration_summary.iteration_time_in_seconds = + WallTimeInSeconds() - iteration_start_time; + iteration_summary.cumulative_time_in_seconds = + WallTimeInSeconds() - start_time + + summary->preprocessor_time_in_seconds; + summary->iterations.push_back(iteration_summary); + + // Call the various callbacks. TODO(sameeragarwal): Here and in + // trust_region_minimizer make this into a function that can be + // shared. + switch (RunCallbacks(iteration_summary)) { + case SOLVER_TERMINATE_SUCCESSFULLY: + summary->termination_type = USER_SUCCESS; + VLOG(1) << "Terminating: User callback returned USER_SUCCESS."; + return; + case SOLVER_ABORT: + summary->termination_type = USER_ABORT; + VLOG(1) << "Terminating: User callback returned USER_ABORT."; + return; + case SOLVER_CONTINUE: + break; + default: + LOG(FATAL) << "Unknown type of user callback status"; + } + + LineSearchFunction line_search_function(evaluator); + LineSearch::Options line_search_options; + line_search_options.function = &line_search_function; + + // TODO(sameeragarwal): Make this parameterizable over different + // line searches. + ArmijoLineSearch line_search; + LineSearch::Summary line_search_summary; + + while (true) { + iteration_start_time = WallTimeInSeconds(); + if (iteration_summary.iteration >= options_.max_num_iterations) { + summary->termination_type = NO_CONVERGENCE; + VLOG(1) << "Terminating: Maximum number of iterations reached."; + break; + } + + const double total_solver_time = iteration_start_time - start_time + + summary->preprocessor_time_in_seconds; + if (total_solver_time >= options_.max_solver_time_in_seconds) { + summary->termination_type = NO_CONVERGENCE; + VLOG(1) << "Terminating: Maximum solver time reached."; + break; + } + + previous_search_direction = search_direction; + + iteration_summary = IterationSummary(); + iteration_summary.iteration = summary->iterations.back().iteration + 1; + iteration_summary.step_is_valid = false; + iteration_summary.step_is_successful = false; + + if (iteration_summary.iteration == 1) { + search_direction = -gradient; + directional_derivative = -gradient_squared_norm; + } else { + // TODO(sameeragarwal): This should probably be refactored into + // a set of functions. But we will do that once things settle + // down in this solver. + switch (options_.line_search_direction_type) { + case STEEPEST_DESCENT: + search_direction = -gradient; + directional_derivative = -gradient_squared_norm; + break; + + case NONLINEAR_CONJUGATE_GRADIENT: + { + double beta = 0.0; + + switch (options_.nonlinear_conjugate_gradient_type) { + case FLETCHER_REEVES: + beta = gradient.squaredNorm() / + previous_gradient_squared_norm; + break; + + case POLAK_RIBIRERE: + gradient_change = gradient - previous_gradient; + beta = gradient.dot(gradient_change) / + previous_gradient_squared_norm; + break; + + case HESTENES_STIEFEL: + gradient_change = gradient - previous_gradient; + beta = gradient.dot(gradient_change) / + previous_search_direction.dot(gradient_change); + break; + + default: + LOG(FATAL) << "Unknown nonlinear conjugate gradient type: " + << options_.nonlinear_conjugate_gradient_type; + } + + search_direction = -gradient + beta * previous_search_direction; + } + + directional_derivative = gradient.dot(search_direction); + if (directional_derivative > -options.function_tolerance) { + LOG(WARNING) << "Restarting non-linear conjugate gradients: " + << directional_derivative; + search_direction = -gradient; + directional_derivative = -gradient_squared_norm; + } + break; + + default: + LOG(FATAL) << "Unknown line search direction type: " + << options_.line_search_direction_type; + } + } + + // TODO(sameeragarwal): Refactor this into its own object and add + // explanations for the various choices. + const double initial_step_size = (iteration_summary.iteration == 1) + ? min(1.0, 1.0 / gradient.lpNorm()) + : min(1.0, 2.0 * (cost - previous_cost) / directional_derivative); + + previous_cost = cost; + previous_gradient = gradient; + previous_gradient_squared_norm = gradient_squared_norm; + previous_directional_derivative = directional_derivative; + + line_search_function.Init(x, search_direction); + line_search.Search(line_search_options, + initial_step_size, + cost, + directional_derivative, + &line_search_summary); + + delta = line_search_summary.optimal_step_size * search_direction; + // TODO(sameeragarwal): Collect stats. + if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data()) || + !evaluator->Evaluate(x_plus_delta.data(), + &cost, + NULL, + gradient.data(), + NULL)) { + LOG(WARNING) << "Evaluation failed."; + cost = previous_cost; + gradient = previous_gradient; + } else { + x = x_plus_delta; + gradient_squared_norm = gradient.squaredNorm(); + } + + + iteration_summary.cost = cost + summary->fixed_cost; + iteration_summary.cost_change = previous_cost - cost; + iteration_summary.step_norm = delta.norm(); + iteration_summary.gradient_max_norm = gradient.lpNorm(); + iteration_summary.step_is_valid = true; + iteration_summary.step_is_successful = true; + iteration_summary.step_norm = delta.norm(); + iteration_summary.step_size = line_search_summary.optimal_step_size; + iteration_summary.line_search_function_evaluations = + line_search_summary.num_evaluations; + + if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) { + summary->termination_type = GRADIENT_TOLERANCE; + VLOG(1) << "Terminating: Gradient tolerance reached." + << "Relative gradient max norm: " + << iteration_summary.gradient_max_norm / gradient_max_norm_0 + << " <= " << options_.gradient_tolerance; + break; + } + + const double absolute_function_tolerance = + options_.function_tolerance * previous_cost; + if (fabs(iteration_summary.cost_change) < absolute_function_tolerance) { + VLOG(1) << "Terminating. Function tolerance reached. " + << "|cost_change|/cost: " + << fabs(iteration_summary.cost_change) / previous_cost + << " <= " << options_.function_tolerance; + summary->termination_type = FUNCTION_TOLERANCE; + return; + } + + iteration_summary.iteration_time_in_seconds = + WallTimeInSeconds() - iteration_start_time; + iteration_summary.cumulative_time_in_seconds = + WallTimeInSeconds() - start_time + + summary->preprocessor_time_in_seconds; + summary->iterations.push_back(iteration_summary); + + switch (RunCallbacks(iteration_summary)) { + case SOLVER_TERMINATE_SUCCESSFULLY: + summary->termination_type = USER_SUCCESS; + VLOG(1) << "Terminating: User callback returned USER_SUCCESS."; + return; + case SOLVER_ABORT: + summary->termination_type = USER_ABORT; + VLOG(1) << "Terminating: User callback returned USER_ABORT."; + return; + case SOLVER_CONTINUE: + break; + default: + LOG(FATAL) << "Unknown type of user callback status"; + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/internal/ceres/line_search_minimizer.h b/internal/ceres/line_search_minimizer.h new file mode 100644 index 000000000..a9556fd67 --- /dev/null +++ b/internal/ceres/line_search_minimizer.h @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_LINE_SEARCH_MINIMIZER_H_ +#define CERES_INTERNAL_LINE_SEARCH_MINIMIZER_H_ + +#include "ceres/minimizer.h" +#include "ceres/solver.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// Generic line search minimization algorithm. +// +// For example usage, see SolverImpl::Minimize. +class LineSearchMinimizer : public Minimizer { + public: + ~LineSearchMinimizer() {} + virtual void Minimize(const Minimizer::Options& options, + double* parameters, + Solver::Summary* summary); + + private: + void Init(const Minimizer::Options& options); + CallbackReturnType RunCallbacks(const IterationSummary& iteration_summary); + Minimizer::Options options_; +}; + +} // namespace internal +} // namespace ceres +#endif // CERES_INTERNAL_LINE_SEARCH_MINIMIZER_H_ diff --git a/internal/ceres/minimizer.h b/internal/ceres/minimizer.h index 22c10f011..3d2bdeec1 100644 --- a/internal/ceres/minimizer.h +++ b/internal/ceres/minimizer.h @@ -78,6 +78,9 @@ class Minimizer { max_num_consecutive_invalid_steps = options.max_num_consecutive_invalid_steps; min_trust_region_radius = options.min_trust_region_radius; + line_search_direction_type = options.line_search_direction_type; + line_search_type = options.line_search_type; + nonlinear_conjugate_gradient_type = options.nonlinear_conjugate_gradient_type; evaluator = NULL; trust_region_strategy = NULL; jacobian = NULL; @@ -87,13 +90,13 @@ class Minimizer { int max_num_iterations; double max_solver_time_in_seconds; + int num_threads; // Number of times the linear solver should be retried in case of // numerical failure. The retries are done by exponentially scaling up // mu at each retry. This leads to stronger and stronger // regularization making the linear least squares problem better // conditioned at each retry. - int num_threads; int max_step_solver_retries; double gradient_tolerance; double parameter_tolerance; @@ -108,6 +111,10 @@ class Minimizer { string lsqp_dump_directory; int max_num_consecutive_invalid_steps; int min_trust_region_radius; + LineSearchDirectionType line_search_direction_type; + LineSearchType line_search_type; + NonlinearConjugateGradientType nonlinear_conjugate_gradient_type; + // List of callbacks that are executed by the Minimizer at the end // of each iteration. diff --git a/internal/ceres/polynomial.cc b/internal/ceres/polynomial.cc index 3b88471f6..2134d6e06 100644 --- a/internal/ceres/polynomial.cc +++ b/internal/ceres/polynomial.cc @@ -267,7 +267,6 @@ Vector FindInterpolatingPolynomial(const vector& samples) { for (int i = 0; i < num_samples; ++i) { const FunctionSample& sample = samples[i]; if (sample.value_is_valid) { - LOG(INFO) << "value constraint"; for (int j = 0; j <= degree; ++j) { lhs(row, j) = pow(sample.x, degree - j); } @@ -277,7 +276,6 @@ Vector FindInterpolatingPolynomial(const vector& samples) { if (sample.gradient_is_valid) { for (int j = 0; j < degree; ++j) { - LOG(INFO) << "gradient constraint"; lhs(row, j) = (degree - j) * pow(sample.x, degree - j - 1); } rhs(row) = sample.gradient; diff --git a/internal/ceres/solver_impl.cc b/internal/ceres/solver_impl.cc index 64e0f8e40..4e0f1330b 100644 --- a/internal/ceres/solver_impl.cc +++ b/internal/ceres/solver_impl.cc @@ -39,6 +39,7 @@ #include "ceres/iteration_callback.h" #include "ceres/levenberg_marquardt_strategy.h" #include "ceres/linear_solver.h" +#include "ceres/line_search_minimizer.h" #include "ceres/map_util.h" #include "ceres/minimizer.h" #include "ceres/ordered_groups.h" @@ -78,12 +79,12 @@ class StateUpdatingCallback : public IterationCallback { // Callback for logging the state of the minimizer to STDERR or STDOUT // depending on the user's preferences and logging level. -class LoggingCallback : public IterationCallback { +class TrustRegionLoggingCallback : public IterationCallback { public: - explicit LoggingCallback(bool log_to_stdout) + explicit TrustRegionLoggingCallback(bool log_to_stdout) : log_to_stdout_(log_to_stdout) {} - ~LoggingCallback() {} + ~TrustRegionLoggingCallback() {} CallbackReturnType operator()(const IterationSummary& summary) { const char* kReportRowFormat = @@ -112,6 +113,42 @@ class LoggingCallback : public IterationCallback { const bool log_to_stdout_; }; +// Callback for logging the state of the minimizer to STDERR or STDOUT +// depending on the user's preferences and logging level. +class LineSearchLoggingCallback : public IterationCallback { + public: + explicit LineSearchLoggingCallback(bool log_to_stdout) + : log_to_stdout_(log_to_stdout) {} + + ~LineSearchLoggingCallback() {} + + CallbackReturnType operator()(const IterationSummary& summary) { + const char* kReportRowFormat = + "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " + "s:% 3.2e e:% 3d it:% 3.2e tt:% 3.2e"; + string output = StringPrintf(kReportRowFormat, + summary.iteration, + summary.cost, + summary.cost_change, + summary.gradient_max_norm, + summary.step_norm, + summary.step_size, + summary.line_search_function_evaluations, + summary.iteration_time_in_seconds, + summary.cumulative_time_in_seconds); + if (log_to_stdout_) { + cout << output << endl; + } else { + VLOG(1) << output; + } + return SOLVER_CONTINUE; + } + + private: + const bool log_to_stdout_; +}; + + // Basic callback to record the execution of the solver to a file for // offline analysis. class FileLoggingCallback : public IterationCallback { @@ -142,13 +179,14 @@ class FileLoggingCallback : public IterationCallback { } // namespace -void SolverImpl::Minimize(const Solver::Options& options, - Program* program, - CoordinateDescentMinimizer* inner_iteration_minimizer, - Evaluator* evaluator, - LinearSolver* linear_solver, - double* parameters, - Solver::Summary* summary) { +void SolverImpl::TrustRegionMinimize( + const Solver::Options& options, + Program* program, + CoordinateDescentMinimizer* inner_iteration_minimizer, + Evaluator* evaluator, + LinearSolver* linear_solver, + double* parameters, + Solver::Summary* summary) { Minimizer::Options minimizer_options(options); // TODO(sameeragarwal): Add support for logging the configuration @@ -160,7 +198,7 @@ void SolverImpl::Minimize(const Solver::Options& options, file_logging_callback.get()); } - LoggingCallback logging_callback(options.minimizer_progress_to_stdout); + TrustRegionLoggingCallback logging_callback(options.minimizer_progress_to_stdout); if (options.logging_type != SILENT) { minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), &logging_callback); @@ -200,9 +238,60 @@ void SolverImpl::Minimize(const Solver::Options& options, WallTimeInSeconds() - minimizer_start_time; } -void SolverImpl::Solve(const Solver::Options& original_options, - ProblemImpl* original_problem_impl, + +void SolverImpl::LineSearchMinimize( + const Solver::Options& options, + Program* program, + Evaluator* evaluator, + double* parameters, + Solver::Summary* summary) { + Minimizer::Options minimizer_options(options); + + // TODO(sameeragarwal): Add support for logging the configuration + // and more detailed stats. + scoped_ptr file_logging_callback; + if (!options.solver_log.empty()) { + file_logging_callback.reset(new FileLoggingCallback(options.solver_log)); + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + file_logging_callback.get()); + } + + LineSearchLoggingCallback logging_callback(options.minimizer_progress_to_stdout); + if (options.logging_type != SILENT) { + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + &logging_callback); + } + + StateUpdatingCallback updating_callback(program, parameters); + if (options.update_state_every_iteration) { + // This must get pushed to the front of the callbacks so that it is run + // before any of the user callbacks. + minimizer_options.callbacks.insert(minimizer_options.callbacks.begin(), + &updating_callback); + } + + minimizer_options.evaluator = evaluator; + + LineSearchMinimizer minimizer; + double minimizer_start_time = WallTimeInSeconds(); + minimizer.Minimize(minimizer_options, parameters, summary); + summary->minimizer_time_in_seconds = + WallTimeInSeconds() - minimizer_start_time; +} + +void SolverImpl::Solve(const Solver::Options& options, + ProblemImpl* problem_impl, Solver::Summary* summary) { + if (options.minimizer_type == TRUST_REGION) { + TrustRegionSolve(options, problem_impl, summary); + } else { + LineSearchSolve(options, problem_impl, summary); + } +} + +void SolverImpl::TrustRegionSolve(const Solver::Options& original_options, + ProblemImpl* original_problem_impl, + Solver::Summary* summary) { double solver_start_time = WallTimeInSeconds(); Program* original_program = original_problem_impl->mutable_program(); @@ -448,13 +537,13 @@ void SolverImpl::Solve(const Solver::Options& original_options, minimizer_start_time - solver_start_time; // Run the optimization. - Minimize(options, - reduced_program.get(), - inner_iteration_minimizer.get(), - evaluator.get(), - linear_solver.get(), - parameters.data(), - summary); + TrustRegionMinimize(options, + reduced_program.get(), + inner_iteration_minimizer.get(), + evaluator.get(), + linear_solver.get(), + parameters.data(), + summary); // If the user aborted mid-optimization or the optimization // terminated because of a numerical failure, then return without @@ -509,6 +598,257 @@ void SolverImpl::Solve(const Solver::Options& original_options, WallTimeInSeconds() - post_process_start_time; } +void SolverImpl::LineSearchSolve(const Solver::Options& original_options, + ProblemImpl* original_problem_impl, + Solver::Summary* summary) { + double solver_start_time = WallTimeInSeconds(); + + 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->num_parameter_blocks = problem_impl->NumParameterBlocks(); + summary->num_parameters = problem_impl->NumParameters(); + summary->num_residual_blocks = problem_impl->NumResidualBlocks(); + summary->num_residuals = problem_impl->NumResiduals(); + + // Empty programs are usually a user error. + if (summary->num_parameter_blocks == 0) { + summary->error = "Problem contains no parameter blocks."; + LOG(ERROR) << summary->error; + return; + } + + if (summary->num_residual_blocks == 0) { + summary->error = "Problem contains no residual blocks."; + LOG(ERROR) << summary->error; + return; + } + + Solver::Options options(original_options); + + // This ensures that we get a Block Jacobian Evaluator along with + // none of the Schur nonsense. This file will have to be extensively + // refactored to deal with the various bits of cleanups related to + // line search. + options.linear_solver_type = CGNR; + + options.linear_solver_ordering = NULL; + options.inner_iteration_ordering = NULL; + +#ifndef CERES_USE_OPENMP + if (options.num_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_threads=1 is supported. Switching " + << "to single threaded mode."; + options.num_threads = 1; + } +#endif + + summary->num_threads_given = original_options.num_threads; + summary->num_threads_used = options.num_threads; + + if (original_options.linear_solver_ordering != NULL) { + if (!IsOrderingValid(original_options, problem_impl, &summary->error)) { + LOG(ERROR) << summary->error; + return; + } + options.linear_solver_ordering = + new ParameterBlockOrdering(*original_options.linear_solver_ordering); + } else { + options.linear_solver_ordering = new ParameterBlockOrdering; + const ProblemImpl::ParameterMap& parameter_map = + problem_impl->parameter_map(); + for (ProblemImpl::ParameterMap::const_iterator it = parameter_map.begin(); + it != parameter_map.end(); + ++it) { + options.linear_solver_ordering->AddElementToGroup(it->first, 0); + } + } + + // Evaluate the initial cost, residual vector and the jacobian + // matrix if requested by the user. The initial cost needs to be + // computed on the original unpreprocessed problem, as it is used to + // determine the value of the "fixed" part of the objective function + // after the problem has undergone reduction. + if (!Evaluator::Evaluate(original_program, + options.num_threads, + &(summary->initial_cost), + options.return_initial_residuals + ? &summary->initial_residuals + : NULL, + options.return_initial_gradient + ? &summary->initial_gradient + : NULL, + options.return_initial_jacobian + ? &summary->initial_jacobian + : NULL)) { + summary->termination_type = NUMERICAL_FAILURE; + summary->error = "Unable to evaluate the initial cost."; + LOG(ERROR) << summary->error; + return; + } + + original_program->SetParameterBlockStatePtrsToUserStatePtrs(); + + // If the user requests gradient checking, construct a new + // ProblemImpl by wrapping the CostFunctions of problem_impl inside + // GradientCheckingCostFunction and replacing problem_impl with + // gradient_checking_problem_impl. + scoped_ptr gradient_checking_problem_impl; + if (options.check_gradients) { + VLOG(1) << "Checking Gradients"; + gradient_checking_problem_impl.reset( + CreateGradientCheckingProblemImpl( + problem_impl, + options.numeric_derivative_relative_step_size, + options.gradient_check_relative_precision)); + + // From here on, problem_impl will point to the gradient checking + // version. + problem_impl = gradient_checking_problem_impl.get(); + } + + // Create the three objects needed to minimize: the transformed program, the + // evaluator, and the linear solver. + scoped_ptr reduced_program(CreateReducedProgram(&options, + problem_impl, + &summary->fixed_cost, + &summary->error)); + if (reduced_program == NULL) { + return; + } + + summary->num_parameter_blocks_reduced = reduced_program->NumParameterBlocks(); + summary->num_parameters_reduced = reduced_program->NumParameters(); + summary->num_residual_blocks_reduced = reduced_program->NumResidualBlocks(); + summary->num_residuals_reduced = reduced_program->NumResiduals(); + + if (summary->num_parameter_blocks_reduced == 0) { + summary->preprocessor_time_in_seconds = + WallTimeInSeconds() - solver_start_time; + + LOG(INFO) << "Terminating: FUNCTION_TOLERANCE reached. " + << "No non-constant parameter blocks found."; + + // FUNCTION_TOLERANCE is the right convergence here, as we know + // that the objective function is constant and cannot be changed + // any further. + summary->termination_type = FUNCTION_TOLERANCE; + + double post_process_start_time = WallTimeInSeconds(); + // Evaluate the final cost, residual vector and the jacobian + // matrix if requested by the user. + if (!Evaluator::Evaluate(original_program, + options.num_threads, + &summary->final_cost, + options.return_final_residuals + ? &summary->final_residuals + : NULL, + options.return_final_gradient + ? &summary->final_gradient + : NULL, + options.return_final_jacobian + ? &summary->final_jacobian + : NULL)) { + summary->termination_type = NUMERICAL_FAILURE; + summary->error = "Unable to evaluate the final cost."; + LOG(ERROR) << summary->error; + return; + } + + // Ensure the program state is set to the user parameters on the way out. + original_program->SetParameterBlockStatePtrsToUserStatePtrs(); + + summary->postprocessor_time_in_seconds = + WallTimeInSeconds() - post_process_start_time; + return; + } + + scoped_ptr evaluator(CreateEvaluator(options, + problem_impl->parameter_map(), + reduced_program.get(), + &summary->error)); + if (evaluator == NULL) { + return; + } + + // The optimizer works on contiguous parameter vectors; allocate some. + Vector parameters(reduced_program->NumParameters()); + + // Collect the discontiguous parameters into a contiguous state vector. + reduced_program->ParameterBlocksToStateVector(parameters.data()); + + Vector original_parameters = parameters; + + double minimizer_start_time = WallTimeInSeconds(); + summary->preprocessor_time_in_seconds = + minimizer_start_time - solver_start_time; + + // Run the optimization. + LineSearchMinimize(options, + reduced_program.get(), + evaluator.get(), + parameters.data(), + summary); + + // If the user aborted mid-optimization or the optimization + // terminated because of a numerical failure, then return without + // updating user state. + if (summary->termination_type == USER_ABORT || + summary->termination_type == NUMERICAL_FAILURE) { + return; + } + + double post_process_start_time = WallTimeInSeconds(); + + // Push the contiguous optimized parameters back to the user's parameters. + reduced_program->StateVectorToParameterBlocks(parameters.data()); + reduced_program->CopyParameterBlockStateToUserState(); + + // Evaluate the final cost, residual vector and the jacobian + // matrix if requested by the user. + if (!Evaluator::Evaluate(original_program, + options.num_threads, + &summary->final_cost, + options.return_final_residuals + ? &summary->final_residuals + : NULL, + options.return_final_gradient + ? &summary->final_gradient + : NULL, + options.return_final_jacobian + ? &summary->final_jacobian + : NULL)) { + // This failure requires careful handling. + // + // At this point, we have modified the user's state, but the + // evaluation failed and we inform him of NUMERICAL_FAILURE. Ceres + // guarantees that user's state is not modified if the solver + // returns with NUMERICAL_FAILURE. Thus, we need to restore the + // user's state to their original values. + + reduced_program->StateVectorToParameterBlocks(original_parameters.data()); + reduced_program->CopyParameterBlockStateToUserState(); + + summary->termination_type = NUMERICAL_FAILURE; + summary->error = "Unable to evaluate the final cost."; + LOG(ERROR) << summary->error; + return; + } + + // Ensure the program state is set to the user parameters on the way out. + original_program->SetParameterBlockStatePtrsToUserStatePtrs(); + + // Stick a fork in it, we're done. + summary->postprocessor_time_in_seconds = + WallTimeInSeconds() - post_process_start_time; +} + + bool SolverImpl::IsOrderingValid(const Solver::Options& options, const ProblemImpl* problem_impl, string* error) { diff --git a/internal/ceres/solver_impl.h b/internal/ceres/solver_impl.h index 09141aedc..92c37fb10 100644 --- a/internal/ceres/solver_impl.h +++ b/internal/ceres/solver_impl.h @@ -54,6 +54,14 @@ class SolverImpl { ProblemImpl* problem_impl, Solver::Summary* summary); + static void TrustRegionSolve(const Solver::Options& options, + ProblemImpl* problem_impl, + Solver::Summary* summary); + + static void LineSearchSolve(const Solver::Options& options, + ProblemImpl* problem_impl, + Solver::Summary* summary); + // Create the transformed Program, which has all the fixed blocks // and residuals eliminated, and in the case of automatic schur // ordering, has the E blocks first in the resulting program, with @@ -99,14 +107,23 @@ class SolverImpl { Program* program, string* error); - // Run the minimization for the given evaluator and configuration. - static void Minimize(const Solver::Options &options, - Program* program, - CoordinateDescentMinimizer* inner_iteration_minimizer, - Evaluator* evaluator, - LinearSolver* linear_solver, - double* parameters, - Solver::Summary* summary); + // Run the TrustRegionMinimizer for the given evaluator and configuration. + static void TrustRegionMinimize( + const Solver::Options &options, + Program* program, + CoordinateDescentMinimizer* inner_iteration_minimizer, + Evaluator* evaluator, + LinearSolver* linear_solver, + double* parameters, + Solver::Summary* summary); + + // Run the LineSearchMinimizer for the given evaluator and configuration. + static void LineSearchMinimize( + const Solver::Options &options, + Program* program, + Evaluator* evaluator, + double* parameters, + Solver::Summary* summary); // Remove the fixed or unused parameter blocks and residuals // depending only on fixed parameters from the problem. Also updates diff --git a/jni/Android.mk b/jni/Android.mk index 0e1e6b977..349a9c117 100644 --- a/jni/Android.mk +++ b/jni/Android.mk @@ -123,6 +123,7 @@ LOCAL_SRC_FILES := $(CERES_SRC_PATH)/array_utils.cc \ $(CERES_SRC_PATH)/iterative_schur_complement_solver.cc \ $(CERES_SRC_PATH)/levenberg_marquardt_strategy.cc \ $(CERES_SRC_PATH)/line_search.cc \ + $(CERES_SRC_PATH)/line_search_minimizer.cc \ $(CERES_SRC_PATH)/linear_least_squares_problems.cc \ $(CERES_SRC_PATH)/linear_operator.cc \ $(CERES_SRC_PATH)/linear_solver.cc \