vpLinProg.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:
 * Linear Programming
 *
 * Authors:
 * Olivier Kermorgant
 *
 *****************************************************************************/

#include <visp3/core/vpLinProg.h>

bool vpLinProg::colReduction(vpMatrix &A, vpColVector &b, bool full_rank, const double &tol)
{
  unsigned int m = A.getRows();
  unsigned int n = A.getCols();

  // degeneracy if A is actually null
  if(A.infinityNorm() < tol)
  {
    if(b.infinityNorm() < tol)
    {
      b.resize(n);
      A.eye(n);
      return true;
    }
    else
      return false;
  }

  // try with standard QR
  vpMatrix Q, R;
  unsigned int r;

  if(full_rank) // caller thinks rank is n or m, can use basic QR
  {
    r = A.transpose().qr(Q, R, false, true);
    // degenerate but easy case - rank is number of columns
    if(r == n)
    {
      // full rank a priori not feasible (A is high thin matrix)
      const vpColVector x = Q * R.inverseTriangular().t() * b.extract(0, r);
      if(allClose(A, x, b, tol))
      {
        b = x;
        A.resize(n,0);
        return true;
      }
      return false;
    }
    else if (r == m)  // most common use case - rank is number of rows
    {
      b = Q * R.inverseTriangular().t() * b;
      // build projection to kernel of Q^T, pick n-m independent columns of I - Q.Q^T
      vpMatrix IQQt = -Q*Q.t();
      for(unsigned int j = 0; j < n; ++j)
        IQQt[j][j] += 1;
      // most of the time the first n-m columns are just fine
      A = IQQt.extract(0,0,n,n-m);
      if(A.qr(Q, R, false, false, tol) != n-m)
      {
        // rank deficiency, manually find n-m independent columns
        unsigned int j0;
        for(j0 = 0; j0 < n; ++j0)
        {
          if(!allZero(IQQt.getCol(j0)))
          {
            A = IQQt.getCol(j0);
            break;
          }
        }
        // fill up
        unsigned int j = j0+1;
        while(A.getCols() < n-m)
        {
          // add next column and check rank of A^T.A
          if(!allZero(IQQt.getCol(j)))
          {
            A = vpMatrix::juxtaposeMatrices(A, IQQt.getCol(j));
            if(A.qr(Q, R, false, false, tol) != A.getCols())
              A.resize(n,A.getCols()-1, false);
          }
          j++;
        }
      }
      return true;
    }
  }

  // A may be non full rank, go for QR+Pivot
  vpMatrix P;
  r = A.transpose().qrPivot(Q, R, P, false, true);
  Q = Q.extract(0,0,n,r);
  const vpColVector x = Q
      * R.inverseTriangular().t()
      * P * b;
  if(allClose(A, x, b, tol))
  {
    b = x;
    if(r == n)  // no dimension left
    {
      A.resize(n,0);
      return true;
    }
    // build projection to kernel of Q, pick n-r independent columns of I - Q.Q^T
    vpMatrix IQQt = -Q*Q.t();
    for(unsigned int j = 0; j < n; ++j)
      IQQt[j][j] += 1;
    // most of the time the first n-r columns are just fine
    A = IQQt.extract(0,0,n,n-r);
    if(A.qr(Q, R, false, false, tol) != n-r)
    {
      // rank deficiency, manually find n-r independent columns
      unsigned int j0;
      for(j0 = 0; j0 < n; ++j0)
      {
        if(!allZero(IQQt.getCol(j0)))
        {
          A = IQQt.getCol(j0);
          break;
        }
      }
      // fill up
      unsigned int j = j0+1;
      while(A.getCols() < n-r)
      {
        // add next column and check rank of A^T.A
        if(!allZero(IQQt.getCol(j)))
        {
          A = vpMatrix::juxtaposeMatrices(A, IQQt.getCol(j));
          if(A.qr(Q, R, false, false, tol) != A.getCols())
            A.resize(n,A.getCols()-1, false);
        }
        j++;
      }
    }
    return true;
  }
  return false;
}

bool vpLinProg::rowReduction(vpMatrix &A, vpColVector &b, const double &tol)
{
  unsigned int m = A.getRows();
  unsigned int n = A.getCols();

  // degeneracy if A is actually null
  if(A.infinityNorm() < tol)
  {
    if(b.infinityNorm() < tol)
    {
      b.resize(0);
      A.resize(0,n);
      return true;
    }
    else
      return false;
  }

  vpMatrix Q, R, P;
  const unsigned int r = A.qrPivot(Q, R, P, false, false, tol);
  const vpColVector x = P.transpose() *
      vpMatrix::stack(R.extract(0, 0, r, r).inverseTriangular(), vpMatrix(n-r, r))
      * Q.extract(0, 0, m, r).transpose() * b;

  if(allClose(A, x, b, tol))
  {
    if(r < m) // if r == m then (A,b) is not changed
    {
      A = R.extract(0, 0, r, n)*P;
      b = Q.extract(0, 0, m, r).transpose() * b;
    }
    return true;
  }
  return false;
}

#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)

bool vpLinProg::solveLP(const vpColVector &c, vpMatrix A, vpColVector b,
                        const vpMatrix &C, const vpColVector &d, vpColVector &x,
                        std::vector<BoundedIndex> l, std::vector<BoundedIndex> u,
                        const double &tol)
{
  unsigned int n = c.getRows();
  unsigned int m = A.getRows();
  const unsigned int p = C.getRows();

  // check if we should forward a feasible point to the next solver
  const bool feasible =
      (x.getRows() == c.getRows())
      && (A.getRows() == 0 || allClose(A, x, b, tol))
      && (C.getRows() == 0 || allLesser(C, x, d, tol))
      && (find_if(l.begin(), l.end(),
                  [&](BoundedIndex &i){return x[i.first] < i.second-tol;})
      == l.end())
      && (find_if(u.begin(), u.end(),
                  [&](BoundedIndex &i){return x[i.first] > i.second+tol;})
      == u.end());

  // shortcut for unbounded variables with equality
  if(!feasible && m && l.size() == 0 && u.size() == 0)
  {
    // changes A.x = b to x = b + A.z
    if(colReduction(A, b, false, tol))
    {
      if(A.getCols())
      {
        if(solveLP(A.transpose()*c, vpMatrix(0, n), vpColVector(0), C*A, d - C*b, x, {}, {}, tol))
        {
          x = b + A*x;
          return true;
        }
      }
      else if(C.getRows() && allLesser(C, b, d, tol))
      {   // A.x = b has only 1 solution (the new b) and C.b <= d
        x = b;
        return true;
      }
    }
    std::cout << "vpLinProg::simplex: equality constraints not feasible" << std::endl;
    return false;
  }

  // count how many additional variables are needed to deal with bounds
  unsigned int s1 = 0, s2 = 0;
  for(unsigned int i = 0; i < n; ++i)
  {
    const auto cmp = [&](const BoundedIndex &bi) {
      return bi.first == i;
    };
    // look for lower bound
    const bool has_low = find_if(l.begin(), l.end(), cmp) != l.end();
    // look for upper bound
    const bool has_up = find_if(u.begin(), u.end(), cmp) != u.end();
    if(has_low == has_up)
    {
      s1++;       // additional variable (double-bounded or unbounded variable)
      if(has_low)
        s2++;   // additional equality constraint (double-bounded)
    }
  }

  // build equality matrix with slack variables
  A.resize(m+p+s2, n+p+s1, false);
  b.resize(A.getRows(),false);
  if(feasible)
    x.resize(n+p+s1, false);

  // deal with slack variables for inequality
  // Cx <= d <=> Cx + y = d
  for(unsigned int i = 0; i < p; ++i)
  {
    A[m+i][n+i] = 1;
    b[m+i] = d[i];
    for(unsigned int j = 0; j < n; ++j)
      A[m+i][j] = C[i][j];
    if(feasible)
      x[n+i] = d[i] - C.getRow(i)*x.extract(0,n);
  }
  // x = P.(z - z0)
  vpMatrix P;P.eye(n, n+p+s1);
  vpColVector z0(n+p+s1);

  // slack variables for bounded terms
  // base slack variable is z1 (replaces x)
  // additional is z2
  unsigned int k1 = 0, k2 = 0;
  for(unsigned int i = 0; i < n; ++i)
  {
    // lambda to find a bound for this index
    const auto cmp = [&](const BoundedIndex &bi) {
      return bi.first == i;
    };

    // look for lower bound
    const auto low = find_if(l.begin(), l.end(), cmp);
    // look for upper bound
    const auto up = find_if(u.begin(), u.end(), cmp);

    if(low == l.end())  // no lower bound
    {
      if(up == u.end())   // no bounds, x = z1 - z2
      {
        P[i][n+p+k1] = -1;
        for(unsigned int j = 0; j < m+p; ++j)
          A[j][n+p+k1] = -A[j][i];
        if(feasible)
        {
          x[i] = std::max(x[i], 0.);
          x[n+p+k1] = std::max(-x[i], 0.);
        }
        k1++;
      }
      else                // upper bound x <= u <=> z1 = -x + u >= 0
      {
        z0[i] = up->second;
        P[i][i] = -1;
        for(unsigned int j = 0; j < m+p; ++j)
          A[j][i] *= -1;
        if(feasible)
          x[i] = up->second - x[i];
        u.erase(up);
      }
    }
    else                    // lower bound  x >= l <=> z1 = x - l >= 0
    {
      z0[i] = -low->second;
      if(up != u.end())   // both bounds  z1 + z2 = u - l
      {
        A[m+p+k2][i] = A[m+p+k2][n+p+k1] = 1;
        b[m+p+k2] = up->second - low->second;
        if(feasible)
        {
          x[i] = up->second - x[i];
          x[n+p+k1] = x[i] - low->second;
        }
        k1++;
        k2++;
        u.erase(up);
      }
      else if(feasible)             // only lower bound
        x[i] = x[i] - low->second;
      l.erase(low);
    }
  }

  // x = P.(z-z0)
  // c^T.x = (P^T.c)^T.z
  // A.x - b = A.P.Z - (b + A.P.z0)
  // A is already A.P
  if(vpLinProg::simplex(P.transpose()*c, A, b+A*z0, x, tol))
  {
    x = P*(x - z0);
    return true;
  }
  return false;
}

bool vpLinProg::simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol)
{
  unsigned int n = c.getRows();
  unsigned int m = A.getRows();

  // find a feasible point is passed x is not
  if(    (x.getRows() != c.getRows()) ||
         !allGreater(x, -tol) ||
         (m != 0 && !allClose(A, x, b, tol)) ||
         [&x,&n](){unsigned int non0 = 0; // count non-0 in x, feasible if <= m
         for(unsigned int i = 0; i < n; ++i)
         if(x[i] > 0) non0++;
         return non0;}() > m
         )
  {
    // min sum(z)
    //  st A.x + D.z =  with diag(D) = sign(b)
    // feasible = (0, |b|)
    // z should be minimized to 0 to get a feasible point for this simplex
    vpColVector cz(n+m);
    vpMatrix AD(m,n+m);
    x.resize(n+m);
    for(unsigned int i = 0; i < m; ++i)
    {
      // build AD and x
      if(b[i] > -tol)
      {
        AD[i][n+i] = 1;
        x[n+i] = b[i];
      }
      else
      {
        AD[i][n+i] = -1;
        x[n+i] = -b[i];
      }
      cz[n+i] = 1;

      // copy A
      for(unsigned int j = 0; j < n; ++j)
        AD[i][j] = A[i][j];
    }
    // solve to get feasibility
    vpLinProg::simplex(cz, AD, b, x, tol);
    if(!allLesser(x.extract(n,m), tol))    // no feasible starting point found
    {
      std::cout << "vpLinProg::simplex: constraints not feasible" << std::endl;
      x.resize(n);
      return false;
    }
    // extract feasible x
    x = x.extract(0, n);
  }
  // feasible part  x is >= 0 and Ax = b

  // try to reduce number of rows to remove rank deficiency
  if(!rowReduction(A, b, tol))
  {
    std::cout << "vpLinProg::simplex: equality constraint not feasible" << std::endl;
    return false;
  }
  m = A.getRows();
  if(m == 0)
  {
    // no constraints, solution is either unbounded or 0
    x = 0;
    return true;
  }

  // build base index from feasible point
  vpMatrix Ab(m,m), An(m,n-m), Abi;
  vpColVector cb(m), cn(n-m);
  std::vector<unsigned int> B, N;
  // add all non-0 indices to B
  for(unsigned int i = 0; i < n; ++i)
  {
    if(std::abs(x[i]) > tol)
      B.push_back(i);
  }
  // if B not full then try to get Ab without null rows
  // get null rows of current Ab = A[B]
  std::vector<unsigned int> null_rows;
  for(unsigned int i = 0; i < m; ++i)
  {
    bool is_0 = true;
    for(unsigned int j = 0; j < B.size(); ++j)
    {
      if(std::abs(A[i][B[j]]) > tol)
      {
        is_0 = false;
        break;
      }
    }
    if(is_0)
      null_rows.push_back(i);
  }

  // add columns to B only if make Ab non-null
  // start from the end as it makes vpQuadProg faster
  for(unsigned int j = n; j-- > 0; )
  {
    if(std::abs(x[j]) < tol)
    {
      bool in_N = true;
      if(B.size() != m)
      {
        in_N = false;
        for(unsigned int i = 0; i < null_rows.size(); ++i)
        {
          in_N = true;
          if(std::abs(A[null_rows[i]][j]) > tol)
          {
            null_rows.erase(null_rows.begin()+i);
            in_N = false;
            break;
          }
        }
        // update empty for next time
        if(!in_N && null_rows.size())
        {
          bool redo = true;
          while(redo)
          {
            redo = false;
            for(unsigned int i = 0; i < null_rows.size(); ++i)
            {
              if(std::abs(A[null_rows[i]][j]) > tol)
              {
                redo = true;
                null_rows.erase(null_rows.begin()+i);
                break;
              }
            }
          }
        }
      }
      if(in_N)
        N.push_back(j);
      else
        B.push_back(j);
    }
  }
  // split A into (Ab, An) and c into (cb, cn)
  for(unsigned int j = 0; j < m; ++j)
  {
    cb[j] = c[B[j]];
    for(unsigned int i = 0; i < m; ++i)
      Ab[i][j] = A[i][B[j]];
  }
  for(unsigned int j = 0; j < n-m; ++j)
  {
    cn[j] = c[N[j]];
    for(unsigned int i = 0; i < m; ++i)
      An[i][j] = A[i][N[j]];
  }

  std::vector<std::pair<double, unsigned int>> a;
  a.reserve(n);

  vpColVector R(n-m), db(m);

  while(true)
  {
    Abi = Ab.inverseByQR();
    // find negative residual
    R  = cn - An.t()*Abi.t()*cb;
    unsigned int j;
    for(j = 0; j < N.size(); ++j)
    {
      if(R[j] < -tol)
        break;
    }

    // no negative residual -> optimal point
    if(j == N.size())
      return true;

    // j will be added as a constraint, find the one to remove
    db = -Abi * An.getCol(j);

    if(allGreater(db, -tol))  // unbounded
      return true;

    // compute alpha / step length to next constraint
    a.resize(0);
    for(unsigned int k = 0; k < m; ++k)
    {
      if(db[k] < -tol)
        a.push_back({-x[B[k]]/db[k], k});
    }
    // get smallest index for smallest alpha
    const auto amin = std::min_element(a.begin(), a.end());
    const double alpha = amin->first;
    const unsigned int k = amin->second;

    // pivot between B[k] and N[j]
    // update x
    for(unsigned int i = 0; i < B.size(); ++i)
      x[B[i]] += alpha * db[i];
    // N part of x
    x[N[j]] = alpha;

    // update A and c
    std::swap(cb[k], cn[j]);
    for(unsigned int i = 0; i < m; ++i)
      std::swap(Ab[i][k], An[i][j]);

    // update B and N
    std::swap(B[k],N[j]);
  }
}
#endif