panoc-helpers.hpp

Go to the documentation of this file.

#pragma once
 
#include <panoc-alm/inner/decl/panoc-stop-crit.hpp>
#include <panoc-alm/util/atomic_stop_signal.hpp>
#include <panoc-alm/util/problem.hpp>
#include <panoc-alm/util/solverstatus.hpp>
 
#include <stdexcept>
 
namespace pa::detail {
 
inline real_t calc_ψ_ŷ(const Problem &p, 
                       crvec x,          
                       crvec y, 
                       crvec Σ, 
                       rvec ŷ   
) {
    if (p.m == 0) /* [[unlikely]] */
        return p.f(x);
 
    // g(x)
    p.g(x, ŷ);
    // ζ = g(x) + Σ⁻¹y
    ŷ += Σ.asDiagonal().inverse() * y;
    // d = ζ - Π(ζ, D)
    ŷ = projecting_difference(ŷ, p.D);
    // dᵀŷ, ŷ = Σ d
    real_t dᵀŷ = 0;
    for (unsigned i = 0; i < p.m; ++i) {
        dᵀŷ += ŷ(i) * Σ(i) * ŷ(i); // TODO: vectorize
        ŷ(i) = Σ(i) * ŷ(i);
    }
    // ψ(x) = f(x) + ½ dᵀŷ
    real_t ψ = p.f(x) + 0.5 * dᵀŷ;
 
    return ψ;
}
 
inline void calc_grad_ψ_from_ŷ(const Problem &p, 
                               crvec x, 
                               crvec ŷ, 
                               rvec grad_ψ, 
                               rvec work_n  
) {
    // ∇ψ = ∇f(x) + ∇g(x) ŷ
    p.grad_f(x, grad_ψ);
    if (p.m != 0) /* [[likely]] */ {
        p.grad_g_prod(x, ŷ, work_n);
        grad_ψ += work_n;
    }
}
 
inline real_t calc_ψ_grad_ψ(const Problem &p, 
                            crvec x, 
                            crvec y, 
                            crvec Σ, 
                            rvec grad_ψ, 
                            rvec work_n, 
                            rvec work_m  
) {
    // ψ(x) = f(x) + ½ dᵀŷ
    real_t ψ = calc_ψ_ŷ(p, x, y, Σ, work_m);
    // ∇ψ = ∇f(x) + ∇g(x) ŷ
    calc_grad_ψ_from_ŷ(p, x, work_m, grad_ψ, work_n);
    return ψ;
}
 
inline void calc_grad_ψ(const Problem &p, 
                        crvec x,          
                        crvec y,     
                        crvec Σ,     
                        rvec grad_ψ, 
                        rvec work_n, 
                        rvec work_m  
) {
    if (p.m == 0) /* [[unlikely]] */
        return p.grad_f(x, grad_ψ);
 
    // g(x)
    p.g(x, work_m);
    // ζ = g(x) + Σ⁻¹y
    work_m += (y.array() / Σ.array()).matrix();
    // d = ζ - Π(ζ, D)
    work_m = projecting_difference(work_m, p.D);
    // ŷ = Σ d
    work_m = Σ.asDiagonal() * work_m;
 
    // ∇ψ = ∇f(x) + ∇g(x) ŷ
    p.grad_f(x, grad_ψ);
    p.grad_g_prod(x, work_m, work_n);
    grad_ψ += work_n;
}
 
inline void calc_err_z(const Problem &p, 
                       crvec x̂,   
                       crvec y,   
                       crvec Σ,   
                       rvec err_z 
) {
    // g(x̂)
    p.g(x̂, err_z);
    // ζ = g(x̂) + Σ⁻¹y
    // ẑ = Π(ζ, D)
    // g(x) - ẑ
    err_z = err_z - project(err_z + Σ.asDiagonal().inverse() * y, p.D);
    // TODO: catastrophic cancellation?
}
 
inline auto
projected_gradient_step(const Box &C, 
                        real_t γ,     
                        crvec x,      
                        crvec grad_ψ  
) {
    using binary_real_f = real_t (*)(real_t, real_t);
    return (-γ * grad_ψ)
        .binaryExpr(C.lowerbound - x, binary_real_f(std::fmax))
        .binaryExpr(C.upperbound - x, binary_real_f(std::fmin));
}
 
inline void calc_x̂(const Problem &prob, 
                   real_t γ,            
                   crvec x,             
                   crvec grad_ψ,        
                   rvec x̂, 
                   rvec p  
) {
    p = projected_gradient_step(prob.C, γ, x, grad_ψ);
    x̂ = x + p;
}
 
inline bool stop_crit_requires_grad_̂ψₖ(PANOCStopCrit crit) {
    switch (crit) {
        case PANOCStopCrit::ApproxKKT: [[fallthrough]];
        case PANOCStopCrit::ApproxKKT2: return true;
        case PANOCStopCrit::ProjGradNorm: [[fallthrough]];
        case PANOCStopCrit::ProjGradNorm2: [[fallthrough]];
        case PANOCStopCrit::ProjGradUnitNorm: [[fallthrough]];
        case PANOCStopCrit::ProjGradUnitNorm2: [[fallthrough]];
        case PANOCStopCrit::FPRNorm: [[fallthrough]];
        case PANOCStopCrit::FPRNorm2: return false;
        case PANOCStopCrit::Ipopt: return true;
    }
    throw std::out_of_range("Invalid PANOCStopCrit");
}
 
inline real_t calc_error_stop_crit(
    const Box &C,       
    PANOCStopCrit crit, 
    crvec pₖ,      
    real_t γ,      
    crvec xₖ,      
    crvec x̂ₖ,      
    crvec ŷₖ,      
    crvec grad_ψₖ, 
    crvec grad_̂ψₖ  
) {
    switch (crit) {
        case PANOCStopCrit::ApproxKKT: {
            auto err = (1 / γ) * pₖ + (grad_ψₖ - grad_̂ψₖ);
            // These parentheses     ^^^               ^^^     are important to
            // prevent catastrophic cancellation when the step is small
            return vec_util::norm_inf(err);
        }
        case PANOCStopCrit::ApproxKKT2: {
            auto err = (1 / γ) * pₖ + (grad_ψₖ - grad_̂ψₖ);
            // These parentheses     ^^^               ^^^     are important to
            // prevent catastrophic cancellation when the step is small
            return err.norm();
        }
        case PANOCStopCrit::ProjGradNorm: {
            return vec_util::norm_inf(pₖ);
        }
        case PANOCStopCrit::ProjGradNorm2: {
            return pₖ.norm();
        }
        case PANOCStopCrit::ProjGradUnitNorm: {
            return vec_util::norm_inf(
                projected_gradient_step(C, 1, xₖ, grad_ψₖ));
        }
        case PANOCStopCrit::ProjGradUnitNorm2: {
            return projected_gradient_step(C, 1, xₖ, grad_ψₖ).norm();
        }
        case PANOCStopCrit::FPRNorm: {
            return vec_util::norm_inf(pₖ) / γ;
        }
        case PANOCStopCrit::FPRNorm2: {
            return pₖ.norm() / γ;
        }
        case PANOCStopCrit::Ipopt: {
            auto err =
                vec_util::norm_inf(projected_gradient_step(C, 1, x̂ₖ, grad_̂ψₖ));
            auto n = 2 * (ŷₖ.size() + x̂ₖ.size());
            if (n == 0)
                return err;
            auto C_lagr_mult =
                vec_util::norm_1(projecting_difference(x̂ₖ - grad_̂ψₖ, C));
            auto D_lagr_mult   = vec_util::norm_1(ŷₖ);
            const real_t s_max = 100;
            real_t s_d =
                std::max(s_max, (C_lagr_mult + D_lagr_mult) / n) / s_max;
            return err / s_d;
        }
    }
    throw std::out_of_range("Invalid PANOCStopCrit");
}
 
inline real_t descent_lemma(
    const Problem &problem,
    real_t rounding_tolerance,
    real_t L_max,
    crvec xₖ,
    real_t ψₖ,
    crvec grad_ψₖ,
    crvec y,
    crvec Σ,
    rvec x̂ₖ,
    rvec pₖ,
    rvec ŷx̂ₖ,
    real_t &ψx̂ₖ,
    real_t &norm_sq_pₖ,
    real_t &grad_ψₖᵀpₖ,
    real_t &Lₖ,
    real_t &γₖ) {
 
    real_t old_γₖ = γₖ;
    real_t margin = (1 + std::abs(ψₖ)) * rounding_tolerance;
    while (ψx̂ₖ - ψₖ > grad_ψₖᵀpₖ + 0.5 * Lₖ * norm_sq_pₖ + margin) {
        if (not(Lₖ * 2 <= L_max))
            break;
 
        Lₖ *= 2;
        γₖ /= 2;
 
        // Calculate x̂ₖ and pₖ (with new step size)
        calc_x̂(problem, γₖ, xₖ, grad_ψₖ, /* in ⟹ out */ x̂ₖ, pₖ);
        // Calculate ∇ψ(xₖ)ᵀpₖ and ‖pₖ‖²
        grad_ψₖᵀpₖ = grad_ψₖ.dot(pₖ);
        norm_sq_pₖ = pₖ.squaredNorm();
 
        // Calculate ψ(x̂ₖ) and ŷ(x̂ₖ)
        ψx̂ₖ = calc_ψ_ŷ(problem, x̂ₖ, y, Σ, /* in ⟹ out */ ŷx̂ₖ);
    }
    return old_γₖ;
}
 
template <class ParamsT, class DurationT>
inline SolverStatus check_all_stop_conditions(
    const ParamsT &params,
    DurationT time_elapsed,
    unsigned iteration,
    const AtomicStopSignal &stop_signal,
    real_t ε,
    real_t εₖ,
    unsigned no_progress) {
 
    bool out_of_time     = time_elapsed > params.max_time;
    bool out_of_iter     = iteration == params.max_iter;
    bool interrupted     = stop_signal.stop_requested();
    bool not_finite      = not std::isfinite(εₖ);
    bool conv            = εₖ <= ε;
    bool max_no_progress = no_progress > params.max_no_progress;
    return conv              ? SolverStatus::Converged
           : out_of_time     ? SolverStatus::MaxTime
           : out_of_iter     ? SolverStatus::MaxIter
           : not_finite      ? SolverStatus::NotFinite
           : max_no_progress ? SolverStatus::NoProgress
           : interrupted     ? SolverStatus::Interrupted
                             : SolverStatus::Unknown;
}
 
inline void calc_augmented_lagrangian_hessian(
    const Problem &problem,
    crvec xₖ,
    crvec ŷxₖ,
    crvec y,
    crvec Σ,
    rvec g,
    mat &H,
    rvec work_n) {
 
    // Compute the Hessian of the Lagrangian
    problem.hess_L(xₖ, ŷxₖ, H);
    // Compute the Hessian of the augmented Lagrangian
    problem.g(xₖ, g);
    for (vec::Index i = 0; i < problem.m; ++i) {
        real_t ζ = g(i) + y(i) / Σ(i);
        bool inactive =
            problem.D.lowerbound(i) < ζ && ζ < problem.D.upperbound(i);
        if (not inactive) {
            problem.grad_gi(xₖ, i, work_n);
            H += work_n * Σ(i) * work_n.transpose();
        }
    }
}
 
inline void calc_augmented_lagrangian_hessian_prod_fd(
    const Problem &problem,
    crvec xₖ,
    crvec y,
    crvec Σ,
    crvec grad_ψ,
    crvec v,
    rvec Hv,
    rvec work_n1,
    rvec work_n2,
    rvec work_m) {
 
    real_t cbrt_ε = std::cbrt(std::numeric_limits<real_t>::epsilon());
    real_t h      = cbrt_ε * (1 + xₖ.norm());
    rvec xₖh      = work_n1;
    xₖh           = xₖ + h * v;
    calc_grad_ψ(problem, xₖh, y, Σ, Hv, work_n2, work_m);
    Hv -= grad_ψ;
    Hv /= h;
}
 
inline real_t initial_lipschitz_estimate(
    const Problem &problem,
    crvec xₖ,
    crvec y,
    crvec Σ,
    real_t ε,
    real_t δ,
    real_t L_min,
    real_t L_max,
    real_t &ψ,
    rvec grad_ψ,
    rvec work_n1,
    rvec work_n2,
    rvec work_n3,
    rvec work_m) {
 
    auto h        = (xₖ * ε).cwiseAbs().cwiseMax(δ);
    work_n1       = xₖ + h;
    real_t norm_h = h.norm();
    // Calculate ∇ψ(x₀ + h)
    calc_grad_ψ(problem, work_n1, y, Σ, /* in ⟹ out */ work_n2, work_n3,
                work_m);
    // Calculate ψ(xₖ), ∇ψ(x₀)
    ψ = calc_ψ_grad_ψ(problem, xₖ, y, Σ, /* in ⟹ out */ grad_ψ, work_n1,
                      work_m);
 
    // Estimate Lipschitz constant using finite differences
    real_t L = (work_n2 - grad_ψ).norm() / norm_h;
    return std::clamp(L, L_min, L_max);
}
 
inline real_t initial_lipschitz_estimate(
    const Problem &problem,
    crvec xₖ,
    crvec y,
    crvec Σ,
    real_t ε,
    real_t δ,
    real_t L_min,
    real_t L_max,
    rvec grad_ψ,
    rvec work_n1,
    rvec work_n2,
    rvec work_n3,
    rvec work_m) {
 
    auto h        = (xₖ * ε).cwiseAbs().cwiseMax(δ);
    work_n1       = xₖ + h;
    real_t norm_h = h.norm();
    // Calculate ∇ψ(x₀ + h)
    calc_grad_ψ(problem, work_n1, y, Σ, /* in ⟹ out */ work_n2, work_n3,
                work_m);
    // Calculate ∇ψ(x₀)
    calc_grad_ψ(problem, xₖ, y, Σ, /* in ⟹ out */ grad_ψ, work_n1, work_m);
 
    // Estimate Lipschitz constant using finite differences
    real_t L = (work_n2 - grad_ψ).norm() / norm_h;
    return std::clamp(L, L_min, L_max);
}
 
} // namespace pa::detail