ruiz.hpp

Go to the documentation of this file.

//
// Copyright (c) 2022 INRIA
//
#ifndef PROXSUITE_PROXQP_SPARSE_PRECOND_RUIZ_HPP
#define PROXSUITE_PROXQP_SPARSE_PRECOND_RUIZ_HPP
 
#include "proxsuite/proxqp/sparse/fwd.hpp"
 
namespace proxsuite {
namespace proxqp {
namespace sparse {
 
namespace preconditioner {
enum struct Symmetry
{
  LOWER,
  UPPER,
};
 
namespace detail {
template<typename T, typename I>
void
rowwise_infty_norm(T* row_norm, proxsuite::linalg::sparse::MatRef<T, I> m)
{
  using namespace proxsuite::linalg::sparse::util;
 
  I const* mi = m.row_indices();
  T const* mx = m.values();
 
  for (usize j = 0; j < usize(m.ncols()); ++j) {
    auto col_start = m.col_start(j);
    auto col_end = m.col_end(j);
 
    for (usize p = col_start; p < col_end; ++p) {
      usize i = zero_extend(mi[p]);
      T mij = fabs(mx[p]);
      row_norm[i] = std::max(row_norm[i], mij);
    }
  }
}
 
template<typename T, typename I>
void
colwise_infty_norm_symhi(T* col_norm, proxsuite::linalg::sparse::MatRef<T, I> h)
{
  using namespace proxsuite::linalg::sparse::util;
 
  I const* hi = h.row_indices();
  T const* hx = h.values();
 
  for (usize j = 0; j < usize(h.ncols()); ++j) {
    auto col_start = h.col_start(j);
    auto col_end = h.col_end(j);
 
    T norm_j = 0;
 
    for (usize p = col_start; p < col_end; ++p) {
      usize i = zero_extend(hi[p]);
      if (i > j) {
        break;
      }
 
      T hij = fabs(hx[p]);
      norm_j = std::max(norm_j, hij);
      col_norm[i] = std::max(col_norm[i], hij);
    }
 
    col_norm[j] = norm_j;
  }
}
 
template<typename T, typename I>
void
colwise_infty_norm_symlo(T* col_norm, proxsuite::linalg::sparse::MatRef<T, I> h)
{
  using namespace proxsuite::linalg::sparse::util;
 
  I const* hi = h.row_indices();
  T const* hx = h.values();
 
  for (usize j = 0; j < usize(h.ncols()); ++j) {
    auto col_start = h.col_start(j);
    auto col_end = h.col_end(j);
 
    T norm_j = 0;
 
    if (col_end > col_start) {
      usize p = col_end;
      while (true) {
        --p;
        usize i = zero_extend(hi[p]);
        if (i < j) {
          break;
        }
 
        T hij = fabs(hx[p]);
        norm_j = std::max(norm_j, hij);
        col_norm[i] = std::max(col_norm[i], hij);
 
        if (p <= col_start) {
          break;
        }
      }
    }
    col_norm[j] = std::max(col_norm[j], norm_j);
  }
}
 
template<typename T, typename I>
auto
ruiz_scale_qp_in_place( //
  VectorViewMut<T> delta_,
  QpViewMut<T, I> qp,
  T epsilon,
  isize max_iter,
  bool preconditioning_for_infeasible_problems,
  Symmetry sym,
  proxsuite::linalg::veg::dynstack::DynStackMut stack) -> T
{
 
  T c = 1;
  auto S = delta_.to_eigen();
 
  isize n = qp.H.nrows();
  isize n_eq = qp.AT.ncols();
  isize n_in = qp.CT.ncols();
 
  T gamma = 1;
  i64 iter = 1;
 
  LDLT_TEMP_VEC(T, delta, n + n_eq + n_in, stack);
 
  I* Hi = qp.H.row_indices_mut();
  T* Hx = qp.H.values_mut();
 
  I* ATi = qp.AT.row_indices_mut();
  T* ATx = qp.AT.values_mut();
 
  I* CTi = qp.CT.row_indices_mut();
  T* CTx = qp.CT.values_mut();
 
  T const machine_eps = std::numeric_limits<T>::epsilon();
 
  while (infty_norm((1 - delta.array()).matrix()) > epsilon) {
    if (iter == max_iter) {
      break;
    } else {
      ++iter;
    }
 
    // norm_infty of each column of A (resp. C), i.e.,
    // each row of AT (resp. CT)
    {
      auto _a_infty_norm = stack.make_new(proxsuite::linalg::veg::Tag<T>{}, n);
      auto _c_infty_norm = stack.make_new(proxsuite::linalg::veg::Tag<T>{}, n);
      auto _h_infty_norm = stack.make_new(proxsuite::linalg::veg::Tag<T>{}, n);
      T* a_infty_norm = _a_infty_norm.ptr_mut();
      T* c_infty_norm = _c_infty_norm.ptr_mut();
      T* h_infty_norm = _h_infty_norm.ptr_mut();
 
      detail::rowwise_infty_norm(a_infty_norm, qp.AT.as_const());
      detail::rowwise_infty_norm(c_infty_norm, qp.CT.as_const());
      switch (sym) {
        case Symmetry::LOWER: {
          detail::colwise_infty_norm_symlo(h_infty_norm, qp.H.as_const());
          break;
        }
        case Symmetry::UPPER: {
          detail::colwise_infty_norm_symhi(h_infty_norm, qp.H.as_const());
          break;
        }
      }
 
      for (isize j = 0; j < n; ++j) {
        delta(j) = T(1) / (machine_eps + sqrt(std::max({
                                           h_infty_norm[j],
                                           a_infty_norm[j],
                                           c_infty_norm[j],
                                         })));
      }
    }
    using namespace proxsuite::linalg::sparse::util;
    if (preconditioning_for_infeasible_problems) {
      delta.tail(n_eq + n_in).setOnes();
    } else {
      for (usize j = 0; j < usize(n_eq); ++j) {
        T a_row_norm = 0;
        qp.AT.to_eigen();
        usize col_start = qp.AT.col_start(j);
        usize col_end = qp.AT.col_end(j);
 
        for (usize p = col_start; p < col_end; ++p) {
          T aji = fabs(ATx[p]);
          a_row_norm = std::max(a_row_norm, aji);
        }
        delta(n + isize(j)) = T(1) / (machine_eps + sqrt(a_row_norm));
      }
 
      for (usize j = 0; j < usize(n_in); ++j) {
        T c_row_norm = 0;
        usize col_start = qp.CT.col_start(j);
        usize col_end = qp.CT.col_end(j);
 
        for (usize p = col_start; p < col_end; ++p) {
          T cji = fabs(CTx[p]);
          c_row_norm = std::max(c_row_norm, cji);
        }
        delta(n + n_eq + isize(j)) = T(1) / (machine_eps + sqrt(c_row_norm));
      }
    }
 
    // removed as non deterministic when using avx
    // https://gitlab.com/libeigen/eigen/-/issues/1728
    // if (preconditioning_for_infeasible_problems) {
    //   T mean = delta.segment(n, n_eq_in).mean();
    //   delta.segment(n,n_eq_in).setConstant(mean);
    // }
 
    // normalize A
    for (usize j = 0; j < usize(n_eq); ++j) {
      usize col_start = qp.AT.col_start(j);
      usize col_end = qp.AT.col_end(j);
 
      T delta_j = delta(n + isize(j));
 
      for (usize p = col_start; p < col_end; ++p) {
        usize i = zero_extend(ATi[p]);
        T& aji = ATx[p];
        T delta_i = delta(isize(i));
        aji = delta_i * (aji * delta_j);
      }
    }
 
    // normalize C
    for (usize j = 0; j < usize(n_in); ++j) {
      usize col_start = qp.CT.col_start(j);
      usize col_end = qp.CT.col_end(j);
 
      T delta_j = delta(n + n_eq + isize(j));
 
      for (usize p = col_start; p < col_end; ++p) {
        usize i = zero_extend(CTi[p]);
        T& cji = CTx[p];
        T delta_i = delta(isize(i));
        cji = delta_i * (cji * delta_j);
      }
    }
 
    // normalize H
    switch (sym) {
      case Symmetry::LOWER: {
        for (usize j = 0; j < usize(n); ++j) {
          usize col_start = qp.H.col_start(j);
          usize col_end = qp.H.col_end(j);
          T delta_j = delta(isize(j));
 
          if (col_end > col_start) {
            usize p = col_end;
            while (true) {
              --p;
              usize i = zero_extend(Hi[p]);
              if (i < j) {
                break;
              }
              Hx[p] = delta_j * Hx[p] * delta(isize(i));
 
              if (p <= col_start) {
                break;
              }
            }
          }
        }
        break;
      }
      case Symmetry::UPPER: {
        for (usize j = 0; j < usize(n); ++j) {
          usize col_start = qp.H.col_start(j);
          usize col_end = qp.H.col_end(j);
          T delta_j = delta(isize(j));
 
          for (usize p = col_start; p < col_end; ++p) {
            usize i = zero_extend(Hi[p]);
            if (i > j) {
              break;
            }
            Hx[p] = delta_j * Hx[p] * delta(isize(i));
          }
        }
        break;
      }
    }
 
    // normalize vectors
    qp.g.to_eigen().array() *= delta.head(n).array();
    qp.b.to_eigen().array() *= delta.segment(n, n_eq).array();
    qp.l.to_eigen().array() *= delta.tail(n_in).array();
    qp.u.to_eigen().array() *= delta.tail(n_in).array();
 
    // additional normalization
    auto _h_infty_norm = stack.make_new(proxsuite::linalg::veg::Tag<T>{}, n);
    T* h_infty_norm = _h_infty_norm.ptr_mut();
 
    switch (sym) {
      case Symmetry::LOWER: {
        detail::colwise_infty_norm_symlo(h_infty_norm, qp.H.as_const());
        break;
      }
      case Symmetry::UPPER: {
        detail::colwise_infty_norm_symhi(h_infty_norm, qp.H.as_const());
        break;
      }
    }
 
    T avg = 0;
    for (isize i = 0; i < n; ++i) {
      avg += h_infty_norm[i];
    }
    avg /= T(n);
 
    gamma = 1 / std::max(avg, T(1));
 
    qp.g.to_eigen() *= gamma;
    qp.H.to_eigen() *= gamma;
 
    S.array() *= delta.array();
    c *= gamma;
  }
  return c;
}
} // namespace detail
 
template<typename T, typename I>
struct RuizEquilibration
{
  Vec<T> delta;
  isize n;
  T c;
  T epsilon;
  i64 max_iter;
  Symmetry sym;
 
  std::ostream* logger_ptr = nullptr;
 
  RuizEquilibration(isize n_,
                    isize n_eq_in,
                    T epsilon_ = T(1e-3),
                    i64 max_iter_ = 10,
                    Symmetry sym_ = Symmetry::UPPER,
                    std::ostream* logger = nullptr)
    : delta(Vec<T>::Ones(n_ + n_eq_in))
    , n(n_)
    , c(1)
    , epsilon(epsilon_)
    , max_iter(max_iter_)
    , sym(sym_)
    , logger_ptr(logger)
  {
    delta.setOnes();
  }
 
  static auto scale_qp_in_place_req(proxsuite::linalg::veg::Tag<T> tag,
                                    isize n,
                                    isize n_eq,
                                    isize n_in)
    -> proxsuite::linalg::veg::dynstack::StackReq
  {
    return proxsuite::linalg::dense::temp_vec_req(tag, n + n_eq + n_in) &
           proxsuite::linalg::veg::dynstack::StackReq::with_len(tag, 3 * n);
  }
 
  void scale_qp_in_place(QpViewMut<T, I> qp,
                         bool execute_preconditioner,
                         bool preconditioning_for_infeasible_problems,
                         const isize max_iter,
                         const T epsilon,
                         proxsuite::linalg::veg::dynstack::DynStackMut stack)
  {
    if (execute_preconditioner) {
      delta.setOnes();
      c = detail::ruiz_scale_qp_in_place( //
        { proxqp::from_eigen, delta },
        qp,
        epsilon,
        max_iter,
        preconditioning_for_infeasible_problems,
        sym,
        stack);
    } else {
      using proxsuite::linalg::sparse::util::zero_extend;
      isize n = qp.H.nrows();
      isize n_eq = qp.AT.ncols();
      isize n_in = qp.CT.ncols();
 
      I* Hi = qp.H.row_indices_mut();
      T* Hx = qp.H.values_mut();
 
      I* ATi = qp.AT.row_indices_mut();
      T* ATx = qp.AT.values_mut();
 
      I* CTi = qp.CT.row_indices_mut();
      T* CTx = qp.CT.values_mut();
 
      // normalize A
      for (usize j = 0; j < usize(n_eq); ++j) {
        usize col_start = qp.AT.col_start(j);
        usize col_end = qp.AT.col_end(j);
 
        T delta_j = delta(n + isize(j));
 
        for (usize p = col_start; p < col_end; ++p) {
          usize i = zero_extend(ATi[p]);
          T& aji = ATx[p];
          T delta_i = delta(isize(i));
          aji = delta_i * (aji * delta_j);
        }
      }
 
      // normalize C
      for (usize j = 0; j < usize(n_in); ++j) {
        usize col_start = qp.CT.col_start(j);
        usize col_end = qp.CT.col_end(j);
 
        T delta_j = delta(n + n_eq + isize(j));
 
        for (usize p = col_start; p < col_end; ++p) {
          usize i = zero_extend(CTi[p]);
          T& cji = CTx[p];
          T delta_i = delta(isize(i));
          cji = delta_i * (cji * delta_j);
        }
      }
 
      // normalize H
      switch (sym) {
        case Symmetry::LOWER: {
          for (usize j = 0; j < usize(n); ++j) {
            usize col_start = qp.H.col_start(j);
            usize col_end = qp.H.col_end(j);
            T delta_j = delta(isize(j));
 
            if (col_end > col_start) {
              usize p = col_end;
              while (true) {
                --p;
                usize i = zero_extend(Hi[p]);
                if (i < j) {
                  break;
                }
                Hx[p] = delta_j * Hx[p] * delta(isize(i));
 
                if (p <= col_start) {
                  break;
                }
              }
            }
          }
          break;
        }
        case Symmetry::UPPER: {
          for (usize j = 0; j < usize(n); ++j) {
            usize col_start = qp.H.col_start(j);
            usize col_end = qp.H.col_end(j);
            T delta_j = delta(isize(j));
 
            for (usize p = col_start; p < col_end; ++p) {
              usize i = zero_extend(Hi[p]);
              if (i > j) {
                break;
              }
              Hx[p] = delta_j * Hx[p] * delta(isize(i));
            }
          }
          break;
        }
      }
 
      // normalize vectors
      qp.g.to_eigen().array() *= delta.head(n).array();
      qp.b.to_eigen().array() *= delta.segment(n, n_eq).array();
      qp.l.to_eigen().array() *= delta.tail(n_in).array();
      qp.u.to_eigen().array() *= delta.tail(n_in).array();
 
      qp.g.to_eigen() *= c;
      qp.H.to_eigen() *= c;
    }
  }
 
  // modifies variables in place
  void scale_primal_in_place(VectorViewMut<T> primal) const
  {
    primal.to_eigen().array() /= delta.array().head(n);
  }
  void scale_dual_in_place(VectorViewMut<T> dual)
  {
    dual.to_eigen().array() = dual.as_const().to_eigen().array() /
                              delta.tail(delta.size() - n).array() * c;
  }
 
  void scale_dual_in_place_eq(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() =
      dual.as_const().to_eigen().array() /
      delta.middleRows(n, dual.to_eigen().size()).array() * c;
  }
  void scale_dual_in_place_in(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() = dual.as_const().to_eigen().array() /
                              delta.tail(dual.to_eigen().size()).array() * c;
  }
 
  void unscale_primal_in_place(VectorViewMut<T> primal) const
  {
    primal.to_eigen().array() *= delta.array().head(n);
  }
  void unscale_dual_in_place(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() = dual.as_const().to_eigen().array() *
                              delta.tail(delta.size() - n).array() / c;
  }
 
  void unscale_dual_in_place_eq(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() =
      dual.as_const().to_eigen().array() *
      delta.middleRows(n, dual.to_eigen().size()).array() / c;
  }
 
  void unscale_dual_in_place_in(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() = dual.as_const().to_eigen().array() *
                              delta.tail(dual.to_eigen().size()).array() / c;
  }
  // modifies residuals in place
  void scale_primal_residual_in_place(VectorViewMut<T> primal) const
  {
    primal.to_eigen().array() *= delta.tail(delta.size() - n).array();
  }
 
  void scale_primal_residual_in_place_eq(VectorViewMut<T> primal_eq) const
  {
    primal_eq.to_eigen().array() *=
      delta.middleRows(n, primal_eq.to_eigen().size()).array();
  }
  void scale_primal_residual_in_place_in(VectorViewMut<T> primal_in) const
  {
    primal_in.to_eigen().array() *=
      delta.tail(primal_in.to_eigen().size()).array();
  }
  void scale_dual_residual_in_place(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() *= delta.head(n).array() * c;
  }
  void unscale_primal_residual_in_place(VectorViewMut<T> primal) const
  {
    primal.to_eigen().array() /= delta.tail(delta.size() - n).array();
  }
  void unscale_primal_residual_in_place_eq(VectorViewMut<T> primal_eq) const
  {
    primal_eq.to_eigen().array() /=
      delta.middleRows(n, primal_eq.to_eigen().size()).array();
  }
  void unscale_primal_residual_in_place_in(VectorViewMut<T> primal_in) const
  {
    primal_in.to_eigen().array() /=
      delta.tail(primal_in.to_eigen().size()).array();
  }
  void unscale_dual_residual_in_place(VectorViewMut<T> dual) const
  {
    dual.to_eigen().array() /= delta.head(n).array() * c;
  }
};
 
} // namespace preconditioner
 
} // namespace sparse
} // namespace proxqp
} // namespace proxsuite
 
#endif /* end of include guard PROXSUITE_PROXQP_SPARSE_PRECOND_RUIZ_HPP */