line_search_minimizer.cc revision 1d2624a10e2c559f8ba9ef89eaa30832c0a83a96
11d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Ceres Solver - A fast non-linear least squares minimizer 21d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Copyright 2012 Google Inc. All rights reserved. 31d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// http://code.google.com/p/ceres-solver/ 41d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 51d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Redistribution and use in source and binary forms, with or without 61d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// modification, are permitted provided that the following conditions are met: 71d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 81d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions of source code must retain the above copyright notice, 91d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer. 101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Redistributions in binary form must reproduce the above copyright notice, 111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// this list of conditions and the following disclaimer in the documentation 121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// and/or other materials provided with the distribution. 131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// * Neither the name of Google Inc. nor the names of its contributors may be 141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// used to endorse or promote products derived from this software without 151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// specific prior written permission. 161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// POSSIBILITY OF SUCH DAMAGE. 281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Author: sameeragarwal@google.com (Sameer Agarwal) 301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Generic loop for line search based optimization algorithms. 321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// This is primarily inpsired by the minFunc packaged written by Mark 341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Schmidt. 351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// http://www.di.ens.fr/~mschmidt/Software/minFunc.html 371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// For details on the theory and implementation see "Numerical 391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Optimization" by Nocedal & Wright. 401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#ifndef CERES_NO_LINE_SEARCH_MINIMIZER 421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/line_search_minimizer.h" 441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <algorithm> 461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <cstdlib> 471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <cmath> 481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <string> 491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include <vector> 501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "Eigen/Dense" 521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/array_utils.h" 531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/evaluator.h" 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/eigen.h" 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/port.h" 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/internal/scoped_ptr.h" 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/line_search.h" 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/line_search_direction.h" 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/stringprintf.h" 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/types.h" 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "ceres/wall_time.h" 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#include "glog/logging.h" 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace ceres { 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace internal { 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingnamespace { 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Small constant for various floating point issues. 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// TODO(sameeragarwal): Change to a better name if this has only one 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// use. 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingconst double kEpsilon = 1e-12; 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingbool Evaluate(Evaluator* evaluator, 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const Vector& x, 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LineSearchMinimizer::State* state) { 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const bool status = evaluator->Evaluate(x.data(), 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &(state->cost), 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling NULL, 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling state->gradient.data(), 791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling NULL); 801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (status) { 811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling state->gradient_squared_norm = state->gradient.squaredNorm(); 821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling state->gradient_max_norm = state->gradient.lpNorm<Eigen::Infinity>(); 831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return status; 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace 891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid LineSearchMinimizer::Minimize(const Minimizer::Options& options, 911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double* parameters, 921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Solver::Summary* summary) { 931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double start_time = WallTimeInSeconds(); 941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling double iteration_start_time = start_time; 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Evaluator* evaluator = CHECK_NOTNULL(options.evaluator); 971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int num_parameters = evaluator->NumParameters(); 981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int num_effective_parameters = evaluator->NumEffectiveParameters(); 991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->termination_type = NO_CONVERGENCE; 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->num_successful_steps = 0; 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->num_unsuccessful_steps = 0; 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling VectorRef x(parameters, num_parameters); 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling State current_state(num_parameters, num_effective_parameters); 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling State previous_state(num_parameters, num_effective_parameters); 1081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Vector delta(num_effective_parameters); 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling Vector x_plus_delta(num_parameters); 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling IterationSummary iteration_summary; 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.iteration = 0; 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.step_is_valid = false; 1151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.step_is_successful = false; 1161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.cost_change = 0.0; 1171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.gradient_max_norm = 0.0; 1181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.step_norm = 0.0; 1191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.linear_solver_iterations = 0; 1201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.step_solver_time_in_seconds = 0; 1211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Do initial cost and Jacobian evaluation. 1231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!Evaluate(evaluator, x, ¤t_state)) { 1241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(WARNING) << "Terminating: Cost and gradient evaluation failed."; 1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->termination_type = NUMERICAL_FAILURE; 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return; 1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->initial_cost = current_state.cost + summary->fixed_cost; 1301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.cost = current_state.cost + summary->fixed_cost; 1311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.gradient_max_norm = current_state.gradient_max_norm; 1331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // The initial gradient max_norm is bounded from below so that we do 1351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // not divide by zero. 1361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double initial_gradient_max_norm = 1371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling max(iteration_summary.gradient_max_norm, kEpsilon); 1381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double absolute_gradient_tolerance = 1391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.gradient_tolerance * initial_gradient_max_norm; 1401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) { 1421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->termination_type = GRADIENT_TOLERANCE; 1431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling VLOG(1) << "Terminating: Gradient tolerance reached." 1441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "Relative gradient max norm: " 1451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << iteration_summary.gradient_max_norm / initial_gradient_max_norm 1461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << " <= " << options.gradient_tolerance; 1471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return; 1481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.iteration_time_in_seconds = 1511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling WallTimeInSeconds() - iteration_start_time; 1521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.cumulative_time_in_seconds = 1531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling WallTimeInSeconds() - start_time 1541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + summary->preprocessor_time_in_seconds; 1551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->iterations.push_back(iteration_summary); 1561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LineSearchDirection::Options line_search_direction_options; 1581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_direction_options.num_parameters = num_effective_parameters; 1591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_direction_options.type = options.line_search_direction_type; 1601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_direction_options.nonlinear_conjugate_gradient_type = 1611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.nonlinear_conjugate_gradient_type; 1621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_direction_options.max_lbfgs_rank = options.max_lbfgs_rank; 1631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_direction_options.use_approximate_eigenvalue_bfgs_scaling = 1641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.use_approximate_eigenvalue_bfgs_scaling; 1651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_ptr<LineSearchDirection> line_search_direction( 1661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LineSearchDirection::Create(line_search_direction_options)); 1671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LineSearchFunction line_search_function(evaluator); 1691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LineSearch::Options line_search_options; 1711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_options.interpolation_type = 1721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.line_search_interpolation_type; 1731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_options.min_step_size = options.min_line_search_step_size; 1741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_options.sufficient_decrease = 1751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.line_search_sufficient_function_decrease; 1761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_options.max_step_contraction = 1771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.max_line_search_step_contraction; 1781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_options.min_step_contraction = 1791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.min_line_search_step_contraction; 1801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_options.max_num_iterations = 1811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.max_num_line_search_step_size_iterations; 1821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_options.sufficient_curvature_decrease = 1831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.line_search_sufficient_curvature_decrease; 1841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_options.max_step_expansion = 1851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.max_line_search_step_expansion; 1861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_options.function = &line_search_function; 1871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling scoped_ptr<LineSearch> 1891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search(LineSearch::Create(options.line_search_type, 1901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_options, 1911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &summary->error)); 1921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (line_search.get() == NULL) { 1931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(ERROR) << "Ceres bug: Unable to create a LineSearch object, please " 1941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "contact the developers!, error: " << summary->error; 1951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->termination_type = DID_NOT_RUN; 1961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return; 1971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 1981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LineSearch::Summary line_search_summary; 2001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling int num_line_search_direction_restarts = 0; 2011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling while (true) { 2031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!RunCallbacks(options.callbacks, iteration_summary, summary)) { 2041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return; 2051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_start_time = WallTimeInSeconds(); 2081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (iteration_summary.iteration >= options.max_num_iterations) { 2091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->termination_type = NO_CONVERGENCE; 2101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling VLOG(1) << "Terminating: Maximum number of iterations reached."; 2111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling break; 2121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double total_solver_time = iteration_start_time - start_time + 2151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->preprocessor_time_in_seconds; 2161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (total_solver_time >= options.max_solver_time_in_seconds) { 2171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->termination_type = NO_CONVERGENCE; 2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling VLOG(1) << "Terminating: Maximum solver time reached."; 2191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling break; 2201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary = IterationSummary(); 2231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.iteration = summary->iterations.back().iteration + 1; 2241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.step_is_valid = false; 2251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.step_is_successful = false; 2261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling bool line_search_status = true; 2281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (iteration_summary.iteration == 1) { 2291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling current_state.search_direction = -current_state.gradient; 2301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else { 2311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_status = line_search_direction->NextDirection( 2321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling previous_state, 2331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling current_state, 2341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ¤t_state.search_direction); 2351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!line_search_status && 2381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling num_line_search_direction_restarts >= 2391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.max_num_line_search_direction_restarts) { 2401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Line search direction failed to generate a new direction, and we 2411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // have already reached our specified maximum number of restarts, 2421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // terminate optimization. 2431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->error = 2441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling StringPrintf("Line search direction failure: specified " 2451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling "max_num_line_search_direction_restarts: %d reached.", 2461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.max_num_line_search_direction_restarts); 2471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(WARNING) << summary->error << " terminating optimization."; 2481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->termination_type = NUMERICAL_FAILURE; 2491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling break; 2501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else if (!line_search_status) { 2521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Restart line search direction with gradient descent on first iteration 2531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // as we have not yet reached our maximum number of restarts. 2541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_LT(num_line_search_direction_restarts, 2551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.max_num_line_search_direction_restarts); 2561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++num_line_search_direction_restarts; 2581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(WARNING) 2591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "Line search direction algorithm: " 2601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << LineSearchDirectionTypeToString(options.line_search_direction_type) 2611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << ", failed to produce a valid new direction at iteration: " 2621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << iteration_summary.iteration << ". Restarting, number of " 2631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "restarts: " << num_line_search_direction_restarts << " / " 2641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << options.max_num_line_search_direction_restarts << " [max]."; 2651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_direction.reset( 2661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LineSearchDirection::Create(line_search_direction_options)); 2671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling current_state.search_direction = -current_state.gradient; 2681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_function.Init(x, current_state.search_direction); 2711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling current_state.directional_derivative = 2721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling current_state.gradient.dot(current_state.search_direction); 2731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // TODO(sameeragarwal): Refactor this into its own object and add 2751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // explanations for the various choices. 2761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // 2771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // Note that we use !line_search_status to ensure that we treat cases when 2781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // we restarted the line search direction equivalently to the first 2791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // iteration. 2801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double initial_step_size = 2811d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (iteration_summary.iteration == 1 || !line_search_status) 2821d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ? min(1.0, 1.0 / current_state.gradient_max_norm) 2831d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling : min(1.0, 2.0 * (current_state.cost - previous_state.cost) / 2841d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling current_state.directional_derivative); 2851d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // By definition, we should only ever go forwards along the specified search 2861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // direction in a line search, most likely cause for this being violated 2871d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // would be a numerical failure in the line search direction calculation. 2881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (initial_step_size < 0.0) { 2891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->error = 2901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling StringPrintf("Numerical failure in line search, initial_step_size is " 2911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling "negative: %.5e, directional_derivative: %.5e, " 2921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling "(current_cost - previous_cost): %.5e", 2931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling initial_step_size, current_state.directional_derivative, 2941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling (current_state.cost - previous_state.cost)); 2951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(WARNING) << summary->error; 2961d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->termination_type = NUMERICAL_FAILURE; 2971d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling break; 2981d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search->Search(initial_step_size, 3011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling current_state.cost, 3021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling current_state.directional_derivative, 3031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling &line_search_summary); 3041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling current_state.step_size = line_search_summary.optimal_step_size; 3061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling delta = current_state.step_size * current_state.search_direction; 3071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling previous_state = current_state; 3091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.step_solver_time_in_seconds = 3101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling WallTimeInSeconds() - iteration_start_time; 3111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling // TODO(sameeragarwal): Collect stats. 3131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (!evaluator->Plus(x.data(), delta.data(), x_plus_delta.data()) || 3141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling !Evaluate(evaluator, x_plus_delta, ¤t_state)) { 3151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling LOG(WARNING) << "Evaluation failed."; 3161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } else { 3171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling x = x_plus_delta; 3181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.gradient_max_norm = current_state.gradient_max_norm; 3211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (iteration_summary.gradient_max_norm <= absolute_gradient_tolerance) { 3221d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->termination_type = GRADIENT_TOLERANCE; 3231d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling VLOG(1) << "Terminating: Gradient tolerance reached." 3241d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "Relative gradient max norm: " 3251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << iteration_summary.gradient_max_norm / initial_gradient_max_norm 3261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << " <= " << options.gradient_tolerance; 3271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling break; 3281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3301d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.cost_change = previous_state.cost - current_state.cost; 3311d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const double absolute_function_tolerance = 3321d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling options.function_tolerance * previous_state.cost; 3331d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling if (fabs(iteration_summary.cost_change) < absolute_function_tolerance) { 3341d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling VLOG(1) << "Terminating. Function tolerance reached. " 3351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << "|cost_change|/cost: " 3361d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << fabs(iteration_summary.cost_change) / previous_state.cost 3371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling << " <= " << options.function_tolerance; 3381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->termination_type = FUNCTION_TOLERANCE; 3391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return; 3401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.cost = current_state.cost + summary->fixed_cost; 3431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.step_norm = delta.norm(); 3441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.step_is_valid = true; 3451d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.step_is_successful = true; 3461d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.step_norm = delta.norm(); 3471d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.step_size = current_state.step_size; 3481d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.line_search_function_evaluations = 3491d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_summary.num_function_evaluations; 3501d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.line_search_gradient_evaluations = 3511d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_summary.num_gradient_evaluations; 3521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.line_search_iterations = 3531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling line_search_summary.num_iterations; 3541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.iteration_time_in_seconds = 3551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling WallTimeInSeconds() - iteration_start_time; 3561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling iteration_summary.cumulative_time_in_seconds = 3571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling WallTimeInSeconds() - start_time 3581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling + summary->preprocessor_time_in_seconds; 3591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling summary->iterations.push_back(iteration_summary); 3611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling ++summary->num_successful_steps; 3621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 3631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace internal 3661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} // namespace ceres 3671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling#endif // CERES_NO_LINE_SEARCH_MINIMIZER 369