56 #ifndef OPTIMIZATION__LINE_SEARCH__MORE_THUENTE_LINE_SEARCH_HPP_
57 #define OPTIMIZATION__LINE_SEARCH__MORE_THUENTE_LINE_SEARCH_HPP_
71 namespace comp = helper_functions::comparisons;
72 namespace optimization
119 const StepT max_step,
120 const StepT min_step,
122 const StepT mu = 1.e-4F,
123 const StepT eta = 0.1F,
124 const std::int32_t max_iterations = 10)
126 m_step_min{min_step},
127 m_optimization_direction{optimization_direction},
130 m_max_iterations{max_iterations}
132 if (min_step < 0.0F) {
throw std::domain_error(
"Min step cannot be negative.");}
133 if (max_step < min_step) {
throw std::domain_error(
"Max step cannot be smaller than min step.");}
134 if (mu < 0.0F || mu > 1.0F) {
throw std::domain_error(
"mu must be in (0, 1).");}
135 if (eta < 0.0F || eta > 1.0F) {
throw std::domain_error(
"eta must be in (0, 1).");}
136 if (max_iterations < 1) {
throw std::domain_error(
"Less than 1 iteration is not allowed.");}
137 m_compute_mode.set_score().set_jacobian();
157 template<
typename DomainValueT,
typename OptimizationProblemT>
158 DomainValueT compute_next_step_(
159 const DomainValueT & x0,
160 const DomainValueT & initial_step,
161 OptimizationProblemT & optimization_problem);
174 template<
typename OptimizationProblemT>
175 class ObjectiveFunction;
181 template<
typename ObjectiveFunctionT>
182 class AuxiliaryFunction;
185 template<
typename FunctionValueT>
186 StepT find_next_step_length(
187 const FunctionValueT & f_t,
const FunctionValueT & f_l,
const FunctionValueT & f_u);
191 template<
typename FunctionValueT>
192 Interval update_interval(
193 const FunctionValueT & f_t,
const FunctionValueT & f_l,
const FunctionValueT & f_u);
196 OptimizationDirection m_optimization_direction;
197 ComputeMode m_compute_mode{};
200 std::int32_t m_max_iterations{};
203 template<
typename DomainValueT,
typename OptimizationProblemT>
205 const DomainValueT & x0,
const DomainValueT & initial_step,
206 OptimizationProblemT & optimization_problem)
209 if (a_t < m_step_min) {
215 using FunctionPhi = ObjectiveFunction<OptimizationProblemT>;
217 using FunctionPsi = AuxiliaryFunction<FunctionPhi>;
218 FunctionPhi phi{x0, initial_step, optimization_problem, m_optimization_direction};
219 FunctionPsi psi{phi, m_mu};
223 const auto phi_0 = phi(0.0F);
224 auto phi_t = phi(a_t);
225 auto psi_t = psi(a_t);
226 auto f_l = psi(interval.a_l);
227 auto f_u = psi(interval.a_u);
229 using ValueT =
typename OptimizationProblemT::Value;
231 bool use_auxiliary_function =
true;
233 for (
auto step_iterations = 0; step_iterations < m_max_iterations; ++step_iterations) {
234 constexpr decltype(psi_t.value) ZERO{};
235 if ((psi_t.value <= ZERO) &&
236 (std::abs(phi_t.derivative) <=
static_cast<ValueT
>(m_eta) * std::abs(phi_0.derivative)))
246 if (use_auxiliary_function) {
247 a_t = find_next_step_length(psi_t, f_l, f_u);
249 a_t = find_next_step_length(phi_t, f_l, f_u);
251 if (a_t < m_step_min || std::isnan(a_t)) {
263 if (use_auxiliary_function && (psi_t.value <= 0.0 && psi_t.derivative > 0.0)) {
264 use_auxiliary_function =
false;
266 f_l = phi(interval.a_l);
267 f_u = phi(interval.a_u);
270 if (use_auxiliary_function) {
273 interval = update_interval(psi_t, f_l, f_u);
274 f_l = psi(interval.a_l);
275 f_u = psi(interval.a_u);
279 interval = update_interval(phi_t, f_l, f_u);
280 f_l = phi(interval.a_l);
281 f_u = phi(interval.a_u);
283 constexpr
auto EPS = std::numeric_limits<StepT>::epsilon();
290 return a_t * phi.get_step_direction();
293 template<
typename OptimizationProblemT>
294 class MoreThuenteLineSearch::ObjectiveFunction
296 using ValueT =
typename OptimizationProblemT::Value;
297 using JacobianT =
typename OptimizationProblemT::Jacobian;
298 using DomainValueT =
typename OptimizationProblemT::DomainValue;
310 const DomainValueT & starting_state,
311 const DomainValueT & initial_step,
312 OptimizationProblemT & underlying_function,
314 : m_starting_state{starting_state},
315 m_step_direction{initial_step.normalized()},
316 m_underlying_function{underlying_function}
318 m_compute_mode.set_score().set_jacobian();
319 m_underlying_function.evaluate(m_starting_state, m_compute_mode);
320 m_underlying_function.jacobian(m_starting_state, m_underlying_function_jacobian);
321 const auto derivative = m_underlying_function_jacobian.dot(m_step_direction);
324 if (derivative > ValueT{0.0}) {
325 m_step_direction *= -1.0;
329 if (derivative < ValueT{0.0}) {
330 m_step_direction *= -1.0;
335 m_multiplier = ValueT{-1.0};
341 FunctionValue operator()(
const StepT & step_size)
343 if (step_size <
StepT{0.0}) {
throw std::runtime_error(
"Step cannot be negative");}
344 const auto current_state = m_starting_state + step_size * m_step_direction;
345 m_underlying_function.evaluate(current_state, m_compute_mode);
346 m_underlying_function.jacobian(current_state, m_underlying_function_jacobian);
349 m_multiplier * m_underlying_function(current_state),
350 m_multiplier * m_underlying_function_jacobian.dot(m_step_direction)};
354 const DomainValueT & get_step_direction() const noexcept {
return m_step_direction;}
357 DomainValueT m_starting_state;
358 DomainValueT m_step_direction;
359 OptimizationProblemT & m_underlying_function;
360 ComputeMode m_compute_mode{};
361 JacobianT m_underlying_function_jacobian;
362 ValueT m_multiplier{1.0};
366 template<
typename ObjectiveFunctionT>
367 class MoreThuenteLineSearch::AuxiliaryFunction
369 using FunctionValue =
typename ObjectiveFunctionT::FunctionValue;
370 using ValueT = decltype(FunctionValue::value);
374 AuxiliaryFunction(ObjectiveFunctionT & objective_function,
const StepT & mu)
375 : m_objective_function{objective_function},
377 m_initial_objective_function_value{objective_function(0.0F)} {}
380 FunctionValue operator()(
const StepT & step_size)
382 const auto & objective_function_value = m_objective_function(step_size);
384 objective_function_value.value -
385 m_initial_objective_function_value.value -
386 static_cast<ValueT
>(m_mu) *
static_cast<ValueT
>(step_size) *
387 objective_function_value.derivative;
388 const auto derivative =
389 objective_function_value.derivative -
static_cast<ValueT
>(m_mu) *
390 m_initial_objective_function_value.derivative;
391 return {step_size, value, derivative};
395 ObjectiveFunctionT & m_objective_function;
397 FunctionValue m_initial_objective_function_value{};
398 FunctionValue m_value{};
401 template<
typename FunctionValueT>
402 MoreThuenteLineSearch::StepT MoreThuenteLineSearch::find_next_step_length(
403 const FunctionValueT & f_t,
const FunctionValueT & f_l,
const FunctionValueT & f_u)
405 using ValueT = decltype(FunctionValueT::value);
407 if (std::isnan(f_t.argument) || std::isnan(f_l.argument) || std::isnan(f_u.argument)) {
408 throw std::runtime_error(
"Got nan values in the step computation function.");
410 constexpr
auto kValueEps = 0.00001;
411 constexpr
auto kStepEps = 0.00001F;
414 const auto find_cubic_minimizer = [kStepEps](
const auto & f_a,
const auto & f_b) -> StepT {
418 const auto z =
static_cast<ValueT
>(3.0) * (f_a.value - f_b.value) /
419 (
static_cast<ValueT
>(f_b.argument) -
static_cast<ValueT
>(f_a.argument)) + f_a.derivative +
421 const auto w = std::sqrt(z * z - f_a.derivative * f_b.derivative);
423 return static_cast<StepT
>(
424 static_cast<ValueT
>(f_b.argument) -
425 (
static_cast<ValueT
>(f_b.argument) -
static_cast<ValueT
>(f_a.argument)) *
426 (f_b.derivative + w - z) /
427 (f_b.derivative - f_a.derivative +
static_cast<ValueT
>(2.0) * w));
431 const auto find_a_q = [kStepEps](
432 const FunctionValueT & f_a,
const FunctionValueT & f_b) -> StepT {
438 static_cast<ValueT
>(f_a.argument) +
static_cast<ValueT
>(0.5) *
439 (
static_cast<ValueT
>(f_b.argument) -
static_cast<ValueT
>(f_a.argument)) *
440 (
static_cast<ValueT
>(f_b.argument) -
static_cast<ValueT
>(f_a.argument)) *
442 (f_a.value - f_b.value +
443 (
static_cast<ValueT
>(f_b.argument) -
static_cast<ValueT
>(f_a.argument)) *
448 const auto find_a_s = [kStepEps](
449 const FunctionValueT & f_a,
const FunctionValueT & f_b) -> StepT {
453 return static_cast<StepT
>(
454 static_cast<ValueT
>(f_a.argument) +
455 (
static_cast<ValueT
>(f_b.argument) -
static_cast<ValueT
>(f_a.argument)) * f_a.derivative /
456 (f_a.derivative - f_b.derivative));
460 if (f_t.value > f_l.value) {
461 const auto a_c = find_cubic_minimizer(f_l, f_t);
462 const auto a_q = find_a_q(f_l, f_t);
463 if (std::fabs(a_c - f_l.argument) < std::fabs(a_q - f_l.argument)) {
466 return 0.5F * (a_q + a_c);
468 }
else if (f_t.derivative * f_l.derivative < 0) {
469 const auto a_c = find_cubic_minimizer(f_l, f_t);
470 const auto a_s = find_a_s(f_l, f_t);
471 if (std::fabs(a_c - f_t.argument) >= std::fabs(a_s - f_t.argument)) {
476 }
else if (
comp::abs_lte(std::abs(f_t.derivative), std::abs(f_l.derivative), kValueEps)) {
478 const auto a_c = find_cubic_minimizer(f_l, f_t);
479 const auto a_s = find_a_s(f_l, f_t);
480 if (std::fabs(a_c - f_t.argument) < std::fabs(a_s - f_t.argument)) {
483 static_cast<StepT
>(a_c));
487 static_cast<StepT
>(a_s));
490 return find_cubic_minimizer(f_t, f_u);
494 template<
typename FunctionValueT>
495 MoreThuenteLineSearch::Interval MoreThuenteLineSearch::update_interval(
496 const FunctionValueT & f_t,
const FunctionValueT & f_l,
const FunctionValueT & f_u)
498 using ValueT = decltype(FunctionValueT::value);
502 if (f_t.value > f_l.value) {
503 return {f_l.argument, f_t.argument};
504 }
else if (f_t.derivative *
static_cast<ValueT
>(f_t.argument - f_l.argument) < 0) {
505 return {f_t.argument, f_u.argument};
506 }
else if (f_t.derivative *
static_cast<ValueT
>(f_t.argument - f_l.argument) > 0) {
507 return {f_t.argument, f_l.argument};
510 return {f_t.argument, f_t.argument};
518 #endif // OPTIMIZATION__LINE_SEARCH__MORE_THUENTE_LINE_SEARCH_HPP_