Visual Servoing Platform  version 3.3.1 under development (2020-10-22)
vpLinProg.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Linear Programming
33  *
34  * Authors:
35  * Olivier Kermorgant
36  *
37  *****************************************************************************/
38 
39 #include <visp3/core/vpLinProg.h>
40 
97 bool vpLinProg::colReduction(vpMatrix &A, vpColVector &b, bool full_rank, const double &tol)
98 {
99  unsigned int m = A.getRows();
100  unsigned int n = A.getCols();
101 
102  // degeneracy if A is actually null
103  if (A.infinityNorm() < tol) {
104  if (b.infinityNorm() < tol) {
105  b.resize(n);
106  A.eye(n);
107  return true;
108  } else
109  return false;
110  }
111 
112  // try with standard QR
113  vpMatrix Q, R;
114  unsigned int r;
115 
116  if (full_rank) // caller thinks rank is n or m, can use basic QR
117  {
118  r = A.transpose().qr(Q, R, false, true);
119 
120  // degenerate but easy case - rank is number of columns
121  if (r == n) {
122  // full rank a priori not feasible (A is high thin matrix)
123  const vpColVector x = Q * R.inverseTriangular().t() * b.extract(0, r);
124  if (allClose(A, x, b, tol)) {
125  b = x;
126  A.resize(n, 0);
127  return true;
128  }
129  return false;
130  } else if (r == m) // most common use case - rank is number of rows
131  {
132  b = Q * R.inverseTriangular().t() * b;
133  // build projection to kernel of Q^T, pick n-m independent columns of I - Q.Q^T
134  vpMatrix IQQt = -Q * Q.t();
135  for (unsigned int j = 0; j < n; ++j)
136  IQQt[j][j] += 1;
137  // most of the time the first n-m columns are just fine
138  A = IQQt.extract(0, 0, n, n - m);
139  if (A.qr(Q, R, false, false, tol) != n - m) {
140  // rank deficiency, manually find n-m independent columns
141  unsigned int j0;
142  for (j0 = 0; j0 < n; ++j0) {
143  if (!allZero(IQQt.getCol(j0))) {
144  A = IQQt.getCol(j0);
145  break;
146  }
147  }
148  // fill up
149  unsigned int j = j0 + 1;
150  while (A.getCols() < n - m) {
151  // add next column and check rank of A^T.A
152  if (!allZero(IQQt.getCol(j))) {
153  A = vpMatrix::juxtaposeMatrices(A, IQQt.getCol(j));
154  if (A.qr(Q, R, false, false, tol) != A.getCols())
155  A.resize(n, A.getCols() - 1, false);
156  }
157  j++;
158  }
159  }
160  return true;
161  }
162  }
163 
164  // A may be non full rank, go for QR+Pivot
165  vpMatrix P;
166  r = A.transpose().qrPivot(Q, R, P, false, true);
167  Q = Q.extract(0, 0, n, r);
168  const vpColVector x = Q * R.inverseTriangular().t() * P * b;
169  if (allClose(A, x, b, tol)) {
170  b = x;
171  if (r == n) // no dimension left
172  {
173  A.resize(n, 0);
174  return true;
175  }
176  // build projection to kernel of Q, pick n-r independent columns of I - Q.Q^T
177  vpMatrix IQQt = -Q * Q.t();
178  for (unsigned int j = 0; j < n; ++j)
179  IQQt[j][j] += 1;
180  // most of the time the first n-r columns are just fine
181  A = IQQt.extract(0, 0, n, n - r);
182  if (A.qr(Q, R, false, false, tol) != n - r) {
183  // rank deficiency, manually find n-r independent columns
184  unsigned int j0;
185  for (j0 = 0; j0 < n; ++j0) {
186  if (!allZero(IQQt.getCol(j0))) {
187  A = IQQt.getCol(j0);
188  break;
189  }
190  }
191  // fill up
192  unsigned int j = j0 + 1;
193  while (A.getCols() < n - r) {
194  // add next column and check rank of A^T.A
195  if (!allZero(IQQt.getCol(j))) {
196  A = vpMatrix::juxtaposeMatrices(A, IQQt.getCol(j));
197  if (A.qr(Q, R, false, false, tol) != A.getCols())
198  A.resize(n, A.getCols() - 1, false);
199  }
200  j++;
201  }
202  }
203  return true;
204  }
205  return false;
206 }
207 
246 bool vpLinProg::rowReduction(vpMatrix &A, vpColVector &b, const double &tol)
247 {
248  unsigned int m = A.getRows();
249  unsigned int n = A.getCols();
250 
251  // degeneracy if A is actually null
252  if (A.infinityNorm() < tol) {
253  if (b.infinityNorm() < tol) {
254  b.resize(0);
255  A.resize(0, n);
256  return true;
257  } else
258  return false;
259  }
260 
261  vpMatrix Q, R, P;
262  const unsigned int r = A.qrPivot(Q, R, P, false, false, tol);
263  const vpColVector x = P.transpose() * vpMatrix::stack(R.extract(0, 0, r, r).inverseTriangular(), vpMatrix(n - r, r)) *
264  Q.extract(0, 0, m, r).transpose() * b;
265 
266  if (allClose(A, x, b, tol)) {
267  if (r < m) // if r == m then (A,b) is not changed
268  {
269  A = R.extract(0, 0, r, n) * P;
270  b = Q.extract(0, 0, m, r).transpose() * b;
271  }
272  return true;
273  }
274  return false;
275 }
276 
277 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
278 
339 bool vpLinProg::solveLP(const vpColVector &c, vpMatrix A, vpColVector b, const vpMatrix &C, const vpColVector &d,
340  vpColVector &x, std::vector<BoundedIndex> l, std::vector<BoundedIndex> u, const double &tol)
341 {
342  unsigned int n = c.getRows();
343  unsigned int m = A.getRows();
344  const unsigned int p = C.getRows();
345 
346  // check if we should forward a feasible point to the next solver
347  const bool feasible =
348  (x.getRows() == c.getRows()) && (A.getRows() == 0 || allClose(A, x, b, tol)) &&
349  (C.getRows() == 0 || allLesser(C, x, d, tol)) &&
350  (find_if(l.begin(), l.end(), [&](BoundedIndex &i) { return x[i.first] < i.second - tol; }) == l.end()) &&
351  (find_if(u.begin(), u.end(), [&](BoundedIndex &i) { return x[i.first] > i.second + tol; }) == u.end());
352 
353  // shortcut for unbounded variables with equality
354  if (!feasible && m && l.size() == 0 && u.size() == 0) {
355  // changes A.x = b to x = b + A.z
356  if (colReduction(A, b, false, tol)) {
357  if (A.getCols()) {
358  if (solveLP(A.transpose() * c, vpMatrix(0, n), vpColVector(0), C * A, d - C * b, x, {}, {}, tol)) {
359  x = b + A * x;
360  return true;
361  }
362  } else if (C.getRows() && allLesser(C, b, d, tol)) { // A.x = b has only 1 solution (the new b) and C.b <= d
363  x = b;
364  return true;
365  }
366  }
367  std::cout << "vpLinProg::simplex: equality constraints not feasible" << std::endl;
368  return false;
369  }
370 
371  // count how many additional variables are needed to deal with bounds
372  unsigned int s1 = 0, s2 = 0;
373  for (unsigned int i = 0; i < n; ++i) {
374  const auto cmp = [&](const BoundedIndex &bi) { return bi.first == i; };
375  // look for lower bound
376  const bool has_low = find_if(l.begin(), l.end(), cmp) != l.end();
377  // look for upper bound
378  const bool has_up = find_if(u.begin(), u.end(), cmp) != u.end();
379  if (has_low == has_up) {
380  s1++; // additional variable (double-bounded or unbounded variable)
381  if (has_low)
382  s2++; // additional equality constraint (double-bounded)
383  }
384  }
385 
386  // build equality matrix with slack variables
387  A.resize(m + p + s2, n + p + s1, false);
388  b.resize(A.getRows(), false);
389  if (feasible)
390  x.resize(n + p + s1, false);
391 
392  // deal with slack variables for inequality
393  // Cx <= d <=> Cx + y = d
394  for (unsigned int i = 0; i < p; ++i) {
395  A[m + i][n + i] = 1;
396  b[m + i] = d[i];
397  for (unsigned int j = 0; j < n; ++j)
398  A[m + i][j] = C[i][j];
399  if (feasible)
400  x[n + i] = d[i] - C.getRow(i) * x.extract(0, n);
401  }
402  // x = P.(z - z0)
403  vpMatrix P;
404  P.eye(n, n + p + s1);
405  vpColVector z0(n + p + s1);
406 
407  // slack variables for bounded terms
408  // base slack variable is z1 (replaces x)
409  // additional is z2
410  unsigned int k1 = 0, k2 = 0;
411  for (unsigned int i = 0; i < n; ++i) {
412  // lambda to find a bound for this index
413  const auto cmp = [&](const BoundedIndex &bi) { return bi.first == i; };
414 
415  // look for lower bound
416  const auto low = find_if(l.begin(), l.end(), cmp);
417  // look for upper bound
418  const auto up = find_if(u.begin(), u.end(), cmp);
419 
420  if (low == l.end()) // no lower bound
421  {
422  if (up == u.end()) // no bounds, x = z1 - z2
423  {
424  P[i][n + p + k1] = -1;
425  for (unsigned int j = 0; j < m + p; ++j)
426  A[j][n + p + k1] = -A[j][i];
427  if (feasible) {
428  x[i] = std::max(x[i], 0.);
429  x[n + p + k1] = std::max(-x[i], 0.);
430  }
431  k1++;
432  } else // upper bound x <= u <=> z1 = -x + u >= 0
433  {
434  z0[i] = up->second;
435  P[i][i] = -1;
436  for (unsigned int j = 0; j < m + p; ++j)
437  A[j][i] *= -1;
438  if (feasible)
439  x[i] = up->second - x[i];
440  u.erase(up);
441  }
442  } else // lower bound x >= l <=> z1 = x - l >= 0
443  {
444  z0[i] = -low->second;
445  if (up != u.end()) // both bounds z1 + z2 = u - l
446  {
447  A[m + p + k2][i] = A[m + p + k2][n + p + k1] = 1;
448  b[m + p + k2] = up->second - low->second;
449  if (feasible) {
450  x[i] = up->second - x[i];
451  x[n + p + k1] = x[i] - low->second;
452  }
453  k1++;
454  k2++;
455  u.erase(up);
456  } else if (feasible) // only lower bound
457  x[i] = x[i] - low->second;
458  l.erase(low);
459  }
460  }
461 
462  // x = P.(z-z0)
463  // c^T.x = (P^T.c)^T.z
464  // A.x - b = A.P.Z - (b + A.P.z0)
465  // A is already A.P
466  if (vpLinProg::simplex(P.transpose() * c, A, b + A * z0, x, tol)) {
467  x = P * (x - z0);
468  return true;
469  }
470  return false;
471 }
472 
532 bool vpLinProg::simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol)
533 {
534  unsigned int n = c.getRows();
535  unsigned int m = A.getRows();
536 
537  // find a feasible point is passed x is not
538  if ((x.getRows() != c.getRows()) || !allGreater(x, -tol) || (m != 0 && !allClose(A, x, b, tol)) || [&x, &n]() {
539  unsigned int non0 = 0; // count non-0 in x, feasible if <= m
540  for (unsigned int i = 0; i < n; ++i)
541  if (x[i] > 0)
542  non0++;
543  return non0;
544  }() > m) {
545  // min sum(z)
546  // st A.x + D.z = with diag(D) = sign(b)
547  // feasible = (0, |b|)
548  // z should be minimized to 0 to get a feasible point for this simplex
549  vpColVector cz(n + m);
550  vpMatrix AD(m, n + m);
551  x.resize(n + m);
552  for (unsigned int i = 0; i < m; ++i) {
553  // build AD and x
554  if (b[i] > -tol) {
555  AD[i][n + i] = 1;
556  x[n + i] = b[i];
557  } else {
558  AD[i][n + i] = -1;
559  x[n + i] = -b[i];
560  }
561  cz[n + i] = 1;
562 
563  // copy A
564  for (unsigned int j = 0; j < n; ++j)
565  AD[i][j] = A[i][j];
566  }
567  // solve to get feasibility
568  vpLinProg::simplex(cz, AD, b, x, tol);
569  if (!allLesser(x.extract(n, m), tol)) // no feasible starting point found
570  {
571  std::cout << "vpLinProg::simplex: constraints not feasible" << std::endl;
572  x.resize(n);
573  return false;
574  }
575  // extract feasible x
576  x = x.extract(0, n);
577  }
578  // feasible part x is >= 0 and Ax = b
579 
580  // try to reduce number of rows to remove rank deficiency
581  if (!rowReduction(A, b, tol)) {
582  std::cout << "vpLinProg::simplex: equality constraint not feasible" << std::endl;
583  return false;
584  }
585  m = A.getRows();
586  if (m == 0) {
587  // no constraints, solution is either unbounded or 0
588  x = 0;
589  return true;
590  }
591 
592  // build base index from feasible point
593  vpMatrix Ab(m, m), An(m, n - m), Abi;
594  vpColVector cb(m), cn(n - m);
595  std::vector<unsigned int> B, N;
596  // add all non-0 indices to B
597  for (unsigned int i = 0; i < n; ++i) {
598  if (std::abs(x[i]) > tol)
599  B.push_back(i);
600  }
601  // if B not full then try to get Ab without null rows
602  // get null rows of current Ab = A[B]
603  std::vector<unsigned int> null_rows;
604  for (unsigned int i = 0; i < m; ++i) {
605  bool is_0 = true;
606  for (unsigned int j = 0; j < B.size(); ++j) {
607  if (std::abs(A[i][B[j]]) > tol) {
608  is_0 = false;
609  break;
610  }
611  }
612  if (is_0)
613  null_rows.push_back(i);
614  }
615 
616  // add columns to B only if make Ab non-null
617  // start from the end as it makes vpQuadProg faster
618  for (unsigned int j = n; j-- > 0;) {
619  if (std::abs(x[j]) < tol) {
620  bool in_N = true;
621  if (B.size() != m) {
622  in_N = false;
623  for (unsigned int i = 0; i < null_rows.size(); ++i) {
624  in_N = true;
625  if (std::abs(A[null_rows[i]][j]) > tol) {
626  null_rows.erase(null_rows.begin() + i);
627  in_N = false;
628  break;
629  }
630  }
631  // update empty for next time
632  if (!in_N && null_rows.size()) {
633  bool redo = true;
634  while (redo) {
635  redo = false;
636  for (unsigned int i = 0; i < null_rows.size(); ++i) {
637  if (std::abs(A[null_rows[i]][j]) > tol) {
638  redo = true;
639  null_rows.erase(null_rows.begin() + i);
640  break;
641  }
642  }
643  }
644  }
645  }
646  if (in_N)
647  N.push_back(j);
648  else
649  B.push_back(j);
650  }
651  }
652  // split A into (Ab, An) and c into (cb, cn)
653  for (unsigned int j = 0; j < m; ++j) {
654  cb[j] = c[B[j]];
655  for (unsigned int i = 0; i < m; ++i)
656  Ab[i][j] = A[i][B[j]];
657  }
658  for (unsigned int j = 0; j < n - m; ++j) {
659  cn[j] = c[N[j]];
660  for (unsigned int i = 0; i < m; ++i)
661  An[i][j] = A[i][N[j]];
662  }
663 
664  std::vector<std::pair<double, unsigned int> > a;
665  a.reserve(n);
666 
667  vpColVector R(n - m), db(m);
668 
669  while (true) {
670  Abi = Ab.inverseByQR();
671  // find negative residual
672  R = cn - An.t() * Abi.t() * cb;
673  unsigned int j;
674  for (j = 0; j < N.size(); ++j) {
675  if (R[j] < -tol)
676  break;
677  }
678 
679  // no negative residual -> optimal point
680  if (j == N.size())
681  return true;
682 
683  // j will be added as a constraint, find the one to remove
684  db = -Abi * An.getCol(j);
685 
686  if (allGreater(db, -tol)) // unbounded
687  return true;
688 
689  // compute alpha / step length to next constraint
690  a.resize(0);
691  for (unsigned int k = 0; k < m; ++k) {
692  if (db[k] < -tol)
693  a.push_back({-x[B[k]] / db[k], k});
694  }
695  // get smallest index for smallest alpha
696  const auto amin = std::min_element(a.begin(), a.end());
697  const double alpha = amin->first;
698  const unsigned int k = amin->second;
699 
700  // pivot between B[k] and N[j]
701  // update x
702  for (unsigned int i = 0; i < B.size(); ++i)
703  x[B[i]] += alpha * db[i];
704  // N part of x
705  x[N[j]] = alpha;
706 
707  // update A and c
708  std::swap(cb[k], cn[j]);
709  for (unsigned int i = 0; i < m; ++i)
710  std::swap(Ab[i][k], An[i][j]);
711 
712  // update B and N
713  std::swap(B[k], N[j]);
714  }
715 }
716 #endif
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:469
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:156
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:5751
static bool colReduction(vpMatrix &A, vpColVector &b, bool full_rank=false, const double &tol=1e-6)
Definition: vpLinProg.cpp:97
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
static bool allClose(const vpMatrix &A, const vpColVector &x, const vpColVector &b, const double &tol=1e-6)
Definition: vpLinProg.h:175
vpMatrix inverseByQR() const
unsigned int qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full=false, bool squareR=false, double tol=1e-6) const
std::pair< unsigned int, double > BoundedIndex
Definition: vpLinProg.h:122
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:6099
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:220
unsigned int getRows() const
Definition: vpArray2D.h:289
vpRowVector t() const
static bool allGreater(const vpColVector &x, const double &thr=1e-6)
Definition: vpLinProg.h:230
vpMatrix inverseTriangular(bool upper=true) const
double infinityNorm() const
Definition: vpMatrix.cpp:6873
unsigned int getCols() const
Definition: vpArray2D.h:279
static bool simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.cpp:532
vpMatrix t() const
Definition: vpMatrix.cpp:526
double infinityNorm() const
unsigned int qr(vpMatrix &Q, vpMatrix &R, bool full=false, bool squareR=false, double tol=1e-6) const
static bool 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=1e-6)
Definition: vpLinProg.cpp:339
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:5435
static bool rowReduction(vpMatrix &A, vpColVector &b, const double &tol=1e-6)
Definition: vpLinProg.cpp:246
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5395
vpMatrix transpose() const
Definition: vpMatrix.cpp:536
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
static bool allLesser(const vpMatrix &C, const vpColVector &x, const vpColVector &d, const double &thr=1e-6)
Definition: vpLinProg.h:194
static bool allZero(const vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.h:155
void eye()
Definition: vpMatrix.cpp:511