vpQuadProg.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Quadratic Programming
 *
 * Authors:
 * Olivier Kermorgant
 *
 *****************************************************************************/

#include <algorithm>
#include <visp3/core/vpMatrixException.h>
#include <visp3/core/vpQuadProg.h>

#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)

void vpQuadProg::fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r,
                                   const double &tol)
{
#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)
  unsigned int n = H.getCols();
  if (H.getRows() != n || c.getRows() != n) {
    throw(vpException(vpMatrixException::dimensionError,
                      "vpQuadProg::fromCanonicalCost: H is not square or not the same dimension as c"));
  }

  vpColVector d(n);
  vpMatrix V(n, n);
  // Compute the eigenvalues and eigenvectors
  H.eigenValues(d, V);
  // find first non-nullptr eigen value
  unsigned int k = 0;
  for (unsigned int i = 0; i < n; ++i) {
    if (d[i] > tol)
      d[i] = sqrt(d[i]);
    else if (d[i] < tol)
      throw(vpException(vpMatrixException::matrixError, "vpQuadProg::fromCanonicalCost: H is not positive"));
    else
      k = i + 1;
  }
  // build (Q,r) such that H = Q.^T.Q and c = -Q^T.r
  vpMatrix D(n - k, n - k);
  vpMatrix P(n - k, n);
  D.diag(d.extract(k, n - k));
  for (unsigned int i = 0; i < n - k; ++i)
    P[i][k + i] = 1;

  Q = D * P * V.transpose();
  r = -Q.t().pseudoInverse() * c;
#else
  throw(vpException(vpException::functionNotImplementedError, "Symmetric matrix decomposition is not implemented. You "
                                                              "should install Lapack, Eigen3 or OpenCV 3rd party"));
#endif
}

bool vpQuadProg::setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol)
{
  x1 = b;
  Z = A;
  if (A.getRows() == b.getRows() && vpLinProg::colReduction(Z, x1, false, tol))
    return true;

  std::cout << "vpQuadProg::setEqualityConstraint: equality constraint infeasible" << std::endl;
  return false;
}

bool vpQuadProg::solveByProjection(const vpMatrix &Q, const vpColVector &r, vpMatrix &A, vpColVector &b, vpColVector &x,
                                   const double &tol)
{
  if (A.getRows()) {
    if (!vpLinProg::colReduction(A, b, false, tol))
      return false;

    if (A.getCols() && (Q * A).infinityNorm() > tol)
      x = b + A * (Q * A).solveBySVD(r - Q * b);
    else
      x = b;
  } else
    x = Q.solveBySVD(r);
  return true;
}

bool vpQuadProg::solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol) const
{
  unsigned int n = Q.getCols();
  if (Q.getRows() != r.getRows() || Z.getRows() != n || x1.getRows() != n) {
    std::cout << "vpQuadProg::solveQPe: wrong dimension\n"
              << "Q: " << Q.getRows() << "x" << Q.getCols() << " - r: " << r.getRows() << std::endl;
    std::cout << "Z: " << Z.getRows() << "x" << Z.getCols() << " - x1: " << x1.getRows() << std::endl;
    throw vpMatrixException::dimensionError;
  }
  if (Z.getCols()) {
    if ((Q * Z).infinityNorm() > tol)
      x = x1 + Z * (Q * Z).solveBySVD(r - Q * x1);
    else
      x = x1;
  } else
    x = Q.solveBySVD(r);
  return true;
}

bool vpQuadProg::solveQPe(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, vpColVector &x,
                          const double &tol)
{
  checkDimensions(Q, r, &A, &b, nullptr, nullptr, "solveQPe");

  if (!solveByProjection(Q, r, A, b, x, tol)) {
    std::cout << "vpQuadProg::solveQPe: equality constraint infeasible" << std::endl;
    return false;
  }
  return true;
}

bool vpQuadProg::solveQP(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, const vpMatrix &C,
                         const vpColVector &d, vpColVector &x, const double &tol)
{
  if (A.getRows() == 0)
    return solveQPi(Q, r, C, d, x, false, tol);

  checkDimensions(Q, r, &A, &b, &C, &d, "solveQP");

  if (!vpLinProg::colReduction(A, b, false, tol)) {
    std::cout << "vpQuadProg::solveQP: equality constraint infeasible" << std::endl;
    return false;
  }

  if (A.getCols() && solveQPi(Q * A, r - Q * b, C * A, d - C * b, x, false, tol)) {
    x = b + A * x;
    return true;
  } else if (vpLinProg::allLesser(C, b, d, tol)) // Ax = b has only 1 solution
  {
    x = b;
    return true;
  }
  std::cout << "vpQuadProg::solveQP: inequality constraint infeasible" << std::endl;
  return false;
}

bool vpQuadProg::solveQPi(const vpMatrix &Q, const vpColVector &r, const vpMatrix &C, const vpColVector &d,
                          vpColVector &x, bool use_equality, const double &tol)
{
  unsigned int n = checkDimensions(Q, r, nullptr, nullptr, &C, &d, "solveQPi");

  if (use_equality) {
    if (Z.getRows() == n) {
      if (Z.getCols() && solveQPi(Q * Z, r - Q * x1, C * Z, d - C * x1, x, false, tol)) {
        // back to initial solution
        x = x1 + Z * x;
        return true;
      } else if (vpLinProg::allLesser(C, x1, d, tol)) {
        x = x1;
        return true;
      }
      std::cout << "vpQuadProg::solveQPi: inequality constraint infeasible" << std::endl;
      return false;
    } else
      std::cout << "vpQuadProg::solveQPi: use_equality before setEqualityConstraint" << std::endl;
  }

  const unsigned int p = C.getRows();

  // look for trivial solution
  // r = 0 and d > 0 -> x = 0
  if (vpLinProg::allZero(r, tol) && (d.getRows() == 0 || vpLinProg::allGreater(d, -tol))) {
    x.resize(n);
    return true;
  }

  // go for solver
  // build feasible point
  vpMatrix A;
  vpColVector b;
  // check active set - all values should be < rows of C
  for (auto v : active) {
    if (v >= p) {
      active.clear();
      std::cout << "vpQuadProg::solveQPi: some constraints have been removed since last call\n";
      break;
    }
  }

  // warm start from previous active set
  A.resize((unsigned int)active.size(), n);
  b.resize((unsigned int)active.size());
  for (unsigned int i = 0; i < active.size(); ++i) {
    for (unsigned int j = 0; j < n; ++j)
      A[i][j] = C[active[i]][j];
    b[i] = d[active[i]];
  }
  if (!solveByProjection(Q, r, A, b, x, tol))
    x.resize(n);

  // or from simplex if we really have no clue
  if (!vpLinProg::allLesser(C, x, d, tol)) {
    // feasible point with simplex:
    //  min r
    //   st C.(x + z1 - z2) + y - r = d
    //   st z1, z2, y, r >= 0
    // dim r = violated constraints
    // feasible if r can be minimized to 0

    // count how many violated constraints
    vpColVector e = d - C * x;
    unsigned int k = 0;
    for (unsigned int i = 0; i < p; ++i) {
      if (e[i] < -tol)
        k++;
    }
    // cost vector
    vpColVector c(2 * n + p + k);
    for (unsigned int i = 0; i < k; ++i)
      c[2 * n + p + i] = 1;

    vpColVector xc(2 * n + p + k);

    vpMatrix A_lp(p, 2 * n + p + k);
    unsigned int l = 0;
    for (unsigned int i = 0; i < p; ++i) {
      // copy [C -C] part
      for (unsigned int j = 0; j < n; ++j) {
        A_lp[i][j] = C[i][j];
        A_lp[i][n + j] = -C[i][j];
      }
      // y-part
      A_lp[i][2 * n + i] = 1;
      if (e[i] < -tol) {
        // r-part
        A_lp[i][2 * n + p + l] = -1;
        xc[2 * n + p + l] = -e[i];
        l++;
      } else
        xc[2 * n + i] = e[i];
    }
    vpLinProg::simplex(c, A_lp, e, xc);

    // r-part should be 0
    if (!vpLinProg::allLesser(xc.extract(2 * n + p, k), tol)) {
      std::cout << "vpQuadProg::solveQPi: inequality constraints not feasible" << std::endl;
      return false;
    }

    // update x to feasible point
    x += xc.extract(0, n) - xc.extract(n, n);
    // init active/inactive sets from y-part of x
    active.clear();
    active.reserve(p);
    inactive.clear();
    for (unsigned int i = 0; i < p; ++i) {
      if (C.getRow(i) * x - d[i] < -tol)
        inactive.push_back(i);
      else
        active.push_back(i);
    }
  } else // warm start feasible
  {
    // using previous active set, check that inactive is sync
    if (active.size() + inactive.size() != p) {
      inactive.clear();
      for (unsigned int i = 0; i < p; ++i) {
        if (std::find(active.begin(), active.end(), i) == active.end())
          inactive.push_back(i);
      }
    }
  }

  vpMatrix Ap;
  bool update_Ap = true;
  unsigned int last_active = C.getRows();

  vpColVector u, g = r - Q * x, mu;

  // solve at one iteration
  while (true) {
    A.resize((unsigned int)active.size(), n);
    b.resize((unsigned int)active.size());
    for (unsigned int i = 0; i < active.size(); ++i) {
      for (unsigned int j = 0; j < n; ++j)
        A[i][j] = C[active[i]][j];
    }

    if (update_Ap && active.size())
      Ap = A.pseudoInverse(); // to get Lagrange multipliers if needed

    if (!solveByProjection(Q, g, A, b, u, tol)) {
      std::cout << "vpQuadProg::solveQPi: QP seems infeasible, too many constraints activated\n";
      return false;
    }

    // 0-update = optimal or useless activated constraints
    if (vpLinProg::allZero(u, tol)) {
      // compute multipliers if any
      unsigned int ineqInd = (unsigned int)active.size();
      if (active.size()) {
        mu = -Ap.transpose() * Q.transpose() * (Q * u - g);
        // find most negative one if any - except last activated in case of degeneracy
        double ineqMax = -tol;
        for (unsigned int i = 0; i < mu.getRows(); ++i) {
          if (mu[i] < ineqMax && active[i] != last_active) {
            ineqInd = i;
            ineqMax = mu[i];
          }
        }
      }

      if (ineqInd == active.size()) // KKT condition no useless constraint
        return true;

      // useless inequality, deactivate
      inactive.push_back(active[ineqInd]);
      if (active.size() == 1)
        active.clear();
      else
        active.erase(active.begin() + ineqInd);
      update_Ap = true;
    } else // u != 0, can improve xc
    {
      unsigned int ineqInd = 0;
      // step length to next constraint
      double alpha = 1;
      for (unsigned int i = 0; i < inactive.size(); ++i) {
        const double Cu = C.getRow(inactive[i]) * u;
        if (Cu > tol) {
          const double a = (d[inactive[i]] - C.getRow(inactive[i]) * x) / Cu;
          if (a < alpha) {
            alpha = a;
            ineqInd = i;
          }
        }
      }
      if (alpha < 1) {
        last_active = inactive[ineqInd];
        if (active.size()) {
          auto it = active.begin();
          while (it != active.end() && *it < inactive[ineqInd])
            it++;
          active.insert(it, inactive[ineqInd]);
        } else
          active.push_back(inactive[ineqInd]);
        inactive.erase(inactive.begin() + ineqInd);
        update_Ap = true;
      } else
        update_Ap = false;
      // update x for next iteration
      x += alpha * u;
      g -= alpha * Q * u;
    }
  }
}

vpColVector vpQuadProg::solveSVDorQR(const vpMatrix &A, const vpColVector &b)
{
  // assume A is full rank
  if (A.getRows() > A.getCols())
    return A.solveBySVD(b);
  return A.solveByQR(b);
}
#else
void dummy_vpQuadProg(){};
#endif