Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
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  {
105  if(b.infinityNorm() < tol)
106  {
107  b.resize(n);
108  A.eye(n);
109  return true;
110  }
111  else
112  return false;
113  }
114 
115  // try with standard QR
116  vpMatrix Q, R;
117  unsigned int r;
118 
119  if(full_rank) // caller thinks rank is n or m, can use basic QR
120  {
121  r = A.transpose().qr(Q, R, false, true);
122  // degenerate but easy case - rank is number of columns
123  if(r == n)
124  {
125  // full rank a priori not feasible (A is high thin matrix)
126  const vpColVector x = Q * R.inverseTriangular().t() * b.extract(0, r);
127  if(allClose(A, x, b, tol))
128  {
129  b = x;
130  A.resize(n,0);
131  return true;
132  }
133  return false;
134  }
135  else if (r == m) // most common use case - rank is number of rows
136  {
137  b = Q * R.inverseTriangular().t() * b;
138  // build projection to kernel of Q^T, pick n-m independent columns of I - Q.Q^T
139  vpMatrix IQQt = -Q*Q.t();
140  for(unsigned int j = 0; j < n; ++j)
141  IQQt[j][j] += 1;
142  // most of the time the first n-m columns are just fine
143  A = IQQt.extract(0,0,n,n-m);
144  if(A.qr(Q, R, false, false, tol) != n-m)
145  {
146  // rank deficiency, manually find n-m independent columns
147  unsigned int j0;
148  for(j0 = 0; j0 < n; ++j0)
149  {
150  if(!allZero(IQQt.getCol(j0)))
151  {
152  A = IQQt.getCol(j0);
153  break;
154  }
155  }
156  // fill up
157  unsigned int j = j0+1;
158  while(A.getCols() < n-m)
159  {
160  // add next column and check rank of A^T.A
161  if(!allZero(IQQt.getCol(j)))
162  {
163  A = vpMatrix::juxtaposeMatrices(A, IQQt.getCol(j));
164  if(A.qr(Q, R, false, false, tol) != A.getCols())
165  A.resize(n,A.getCols()-1, false);
166  }
167  j++;
168  }
169  }
170  return true;
171  }
172  }
173 
174  // A may be non full rank, go for QR+Pivot
175  vpMatrix P;
176  r = A.transpose().qrPivot(Q, R, P, false, true);
177  Q = Q.extract(0,0,n,r);
178  const vpColVector x = Q
179  * R.inverseTriangular().t()
180  * P * b;
181  if(allClose(A, x, b, tol))
182  {
183  b = x;
184  if(r == n) // no dimension left
185  {
186  A.resize(n,0);
187  return true;
188  }
189  // build projection to kernel of Q, pick n-r independent columns of I - Q.Q^T
190  vpMatrix IQQt = -Q*Q.t();
191  for(unsigned int j = 0; j < n; ++j)
192  IQQt[j][j] += 1;
193  // most of the time the first n-r columns are just fine
194  A = IQQt.extract(0,0,n,n-r);
195  if(A.qr(Q, R, false, false, tol) != n-r)
196  {
197  // rank deficiency, manually find n-r independent columns
198  unsigned int j0;
199  for(j0 = 0; j0 < n; ++j0)
200  {
201  if(!allZero(IQQt.getCol(j0)))
202  {
203  A = IQQt.getCol(j0);
204  break;
205  }
206  }
207  // fill up
208  unsigned int j = j0+1;
209  while(A.getCols() < n-r)
210  {
211  // add next column and check rank of A^T.A
212  if(!allZero(IQQt.getCol(j)))
213  {
214  A = vpMatrix::juxtaposeMatrices(A, IQQt.getCol(j));
215  if(A.qr(Q, R, false, false, tol) != A.getCols())
216  A.resize(n,A.getCols()-1, false);
217  }
218  j++;
219  }
220  }
221  return true;
222  }
223  return false;
224 }
225 
264 bool vpLinProg::rowReduction(vpMatrix &A, vpColVector &b, const double &tol)
265 {
266  unsigned int m = A.getRows();
267  unsigned int n = A.getCols();
268 
269  // degeneracy if A is actually null
270  if(A.infinityNorm() < tol)
271  {
272  if(b.infinityNorm() < tol)
273  {
274  b.resize(0);
275  A.resize(0,n);
276  return true;
277  }
278  else
279  return false;
280  }
281 
282  vpMatrix Q, R, P;
283  const unsigned int r = A.qrPivot(Q, R, P, false, false, tol);
284  const vpColVector x = P.transpose() *
285  vpMatrix::stack(R.extract(0, 0, r, r).inverseTriangular(), vpMatrix(n-r, r))
286  * Q.extract(0, 0, m, r).transpose() * b;
287 
288  if(allClose(A, x, b, tol))
289  {
290  if(r < m) // if r == m then (A,b) is not changed
291  {
292  A = R.extract(0, 0, r, n)*P;
293  b = Q.extract(0, 0, m, r).transpose() * b;
294  }
295  return true;
296  }
297  return false;
298 }
299 
300 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
301 
362  const vpMatrix &C, const vpColVector &d, vpColVector &x,
363  std::vector<BoundedIndex> l, std::vector<BoundedIndex> u,
364  const double &tol)
365 {
366  unsigned int n = c.getRows();
367  unsigned int m = A.getRows();
368  const unsigned int p = C.getRows();
369 
370  // check if we should forward a feasible point to the next solver
371  const bool feasible =
372  (x.getRows() == c.getRows())
373  && (A.getRows() == 0 || allClose(A, x, b, tol))
374  && (C.getRows() == 0 || allLesser(C, x, d, tol))
375  && (find_if(l.begin(), l.end(),
376  [&](BoundedIndex &i){return x[i.first] < i.second-tol;})
377  == l.end())
378  && (find_if(u.begin(), u.end(),
379  [&](BoundedIndex &i){return x[i.first] > i.second+tol;})
380  == u.end());
381 
382  // shortcut for unbounded variables with equality
383  if(!feasible && m && l.size() == 0 && u.size() == 0)
384  {
385  // changes A.x = b to x = b + A.z
386  if(colReduction(A, b, false, tol))
387  {
388  if(A.getCols())
389  {
390  if(solveLP(A.transpose()*c, vpMatrix(0, n), vpColVector(0), C*A, d - C*b, x, {}, {}, tol))
391  {
392  x = b + A*x;
393  return true;
394  }
395  }
396  else if(C.getRows() && allLesser(C, b, d, tol))
397  { // A.x = b has only 1 solution (the new b) and C.b <= d
398  x = b;
399  return true;
400  }
401  }
402  std::cout << "vpLinProg::simplex: equality constraints not feasible" << std::endl;
403  return false;
404  }
405 
406  // count how many additional variables are needed to deal with bounds
407  unsigned int s1 = 0, s2 = 0;
408  for(unsigned int i = 0; i < n; ++i)
409  {
410  const auto cmp = [&](const BoundedIndex &bi) {
411  return bi.first == i;
412  };
413  // look for lower bound
414  const bool has_low = find_if(l.begin(), l.end(), cmp) != l.end();
415  // look for upper bound
416  const bool has_up = find_if(u.begin(), u.end(), cmp) != u.end();
417  if(has_low == has_up)
418  {
419  s1++; // additional variable (double-bounded or unbounded variable)
420  if(has_low)
421  s2++; // additional equality constraint (double-bounded)
422  }
423  }
424 
425  // build equality matrix with slack variables
426  A.resize(m+p+s2, n+p+s1, false);
427  b.resize(A.getRows(),false);
428  if(feasible)
429  x.resize(n+p+s1, false);
430 
431  // deal with slack variables for inequality
432  // Cx <= d <=> Cx + y = d
433  for(unsigned int i = 0; i < p; ++i)
434  {
435  A[m+i][n+i] = 1;
436  b[m+i] = d[i];
437  for(unsigned int j = 0; j < n; ++j)
438  A[m+i][j] = C[i][j];
439  if(feasible)
440  x[n+i] = d[i] - C.getRow(i)*x.extract(0,n);
441  }
442  // x = P.(z - z0)
443  vpMatrix P;P.eye(n, n+p+s1);
444  vpColVector z0(n+p+s1);
445 
446  // slack variables for bounded terms
447  // base slack variable is z1 (replaces x)
448  // additional is z2
449  unsigned int k1 = 0, k2 = 0;
450  for(unsigned int i = 0; i < n; ++i)
451  {
452  // lambda to find a bound for this index
453  const auto cmp = [&](const BoundedIndex &bi) {
454  return bi.first == i;
455  };
456 
457  // look for lower bound
458  const auto low = find_if(l.begin(), l.end(), cmp);
459  // look for upper bound
460  const auto up = find_if(u.begin(), u.end(), cmp);
461 
462  if(low == l.end()) // no lower bound
463  {
464  if(up == u.end()) // no bounds, x = z1 - z2
465  {
466  P[i][n+p+k1] = -1;
467  for(unsigned int j = 0; j < m+p; ++j)
468  A[j][n+p+k1] = -A[j][i];
469  if(feasible)
470  {
471  x[i] = std::max(x[i], 0.);
472  x[n+p+k1] = std::max(-x[i], 0.);
473  }
474  k1++;
475  }
476  else // upper bound x <= u <=> z1 = -x + u >= 0
477  {
478  z0[i] = up->second;
479  P[i][i] = -1;
480  for(unsigned int j = 0; j < m+p; ++j)
481  A[j][i] *= -1;
482  if(feasible)
483  x[i] = up->second - x[i];
484  u.erase(up);
485  }
486  }
487  else // lower bound x >= l <=> z1 = x - l >= 0
488  {
489  z0[i] = -low->second;
490  if(up != u.end()) // both bounds z1 + z2 = u - l
491  {
492  A[m+p+k2][i] = A[m+p+k2][n+p+k1] = 1;
493  b[m+p+k2] = up->second - low->second;
494  if(feasible)
495  {
496  x[i] = up->second - x[i];
497  x[n+p+k1] = x[i] - low->second;
498  }
499  k1++;
500  k2++;
501  u.erase(up);
502  }
503  else if(feasible) // only lower bound
504  x[i] = x[i] - low->second;
505  l.erase(low);
506  }
507  }
508 
509  // x = P.(z-z0)
510  // c^T.x = (P^T.c)^T.z
511  // A.x - b = A.P.Z - (b + A.P.z0)
512  // A is already A.P
513  if(vpLinProg::simplex(P.transpose()*c, A, b+A*z0, x, tol))
514  {
515  x = P*(x - z0);
516  return true;
517  }
518  return false;
519 }
520 
580 bool vpLinProg::simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol)
581 {
582  unsigned int n = c.getRows();
583  unsigned int m = A.getRows();
584 
585  // find a feasible point is passed x is not
586  if( (x.getRows() != c.getRows()) ||
587  !allGreater(x, -tol) ||
588  (m != 0 && !allClose(A, x, b, tol)) ||
589  [&x,&n](){unsigned int non0 = 0; // count non-0 in x, feasible if <= m
590  for(unsigned int i = 0; i < n; ++i)
591  if(x[i] > 0) non0++;
592  return non0;}() > m
593  )
594  {
595  // min sum(z)
596  // st A.x + D.z = with diag(D) = sign(b)
597  // feasible = (0, |b|)
598  // z should be minimized to 0 to get a feasible point for this simplex
599  vpColVector cz(n+m);
600  vpMatrix AD(m,n+m);
601  x.resize(n+m);
602  for(unsigned int i = 0; i < m; ++i)
603  {
604  // build AD and x
605  if(b[i] > -tol)
606  {
607  AD[i][n+i] = 1;
608  x[n+i] = b[i];
609  }
610  else
611  {
612  AD[i][n+i] = -1;
613  x[n+i] = -b[i];
614  }
615  cz[n+i] = 1;
616 
617  // copy A
618  for(unsigned int j = 0; j < n; ++j)
619  AD[i][j] = A[i][j];
620  }
621  // solve to get feasibility
622  vpLinProg::simplex(cz, AD, b, x, tol);
623  if(!allLesser(x.extract(n,m), tol)) // no feasible starting point found
624  {
625  std::cout << "vpLinProg::simplex: constraints not feasible" << std::endl;
626  x.resize(n);
627  return false;
628  }
629  // extract feasible x
630  x = x.extract(0, n);
631  }
632  // feasible part x is >= 0 and Ax = b
633 
634  // try to reduce number of rows to remove rank deficiency
635  if(!rowReduction(A, b, tol))
636  {
637  std::cout << "vpLinProg::simplex: equality constraint not feasible" << std::endl;
638  return false;
639  }
640  m = A.getRows();
641  if(m == 0)
642  {
643  // no constraints, solution is either unbounded or 0
644  x = 0;
645  return true;
646  }
647 
648  // build base index from feasible point
649  vpMatrix Ab(m,m), An(m,n-m), Abi;
650  vpColVector cb(m), cn(n-m);
651  std::vector<unsigned int> B, N;
652  // add all non-0 indices to B
653  for(unsigned int i = 0; i < n; ++i)
654  {
655  if(std::abs(x[i]) > tol)
656  B.push_back(i);
657  }
658  // if B not full then try to get Ab without null rows
659  // get null rows of current Ab = A[B]
660  std::vector<unsigned int> null_rows;
661  for(unsigned int i = 0; i < m; ++i)
662  {
663  bool is_0 = true;
664  for(unsigned int j = 0; j < B.size(); ++j)
665  {
666  if(std::abs(A[i][B[j]]) > tol)
667  {
668  is_0 = false;
669  break;
670  }
671  }
672  if(is_0)
673  null_rows.push_back(i);
674  }
675 
676  // add columns to B only if make Ab non-null
677  // start from the end as it makes vpQuadProg faster
678  for(unsigned int j = n; j-- > 0; )
679  {
680  if(std::abs(x[j]) < tol)
681  {
682  bool in_N = true;
683  if(B.size() != m)
684  {
685  in_N = false;
686  for(unsigned int i = 0; i < null_rows.size(); ++i)
687  {
688  in_N = true;
689  if(std::abs(A[null_rows[i]][j]) > tol)
690  {
691  null_rows.erase(null_rows.begin()+i);
692  in_N = false;
693  break;
694  }
695  }
696  // update empty for next time
697  if(!in_N && null_rows.size())
698  {
699  bool redo = true;
700  while(redo)
701  {
702  redo = false;
703  for(unsigned int i = 0; i < null_rows.size(); ++i)
704  {
705  if(std::abs(A[null_rows[i]][j]) > tol)
706  {
707  redo = true;
708  null_rows.erase(null_rows.begin()+i);
709  break;
710  }
711  }
712  }
713  }
714  }
715  if(in_N)
716  N.push_back(j);
717  else
718  B.push_back(j);
719  }
720  }
721  // split A into (Ab, An) and c into (cb, cn)
722  for(unsigned int j = 0; j < m; ++j)
723  {
724  cb[j] = c[B[j]];
725  for(unsigned int i = 0; i < m; ++i)
726  Ab[i][j] = A[i][B[j]];
727  }
728  for(unsigned int j = 0; j < n-m; ++j)
729  {
730  cn[j] = c[N[j]];
731  for(unsigned int i = 0; i < m; ++i)
732  An[i][j] = A[i][N[j]];
733  }
734 
735  std::vector<std::pair<double, unsigned int>> a;
736  a.reserve(n);
737 
738  vpColVector R(n-m), db(m);
739 
740  while(true)
741  {
742  Abi = Ab.inverseByQR();
743  // find negative residual
744  R = cn - An.t()*Abi.t()*cb;
745  unsigned int j;
746  for(j = 0; j < N.size(); ++j)
747  {
748  if(R[j] < -tol)
749  break;
750  }
751 
752  // no negative residual -> optimal point
753  if(j == N.size())
754  return true;
755 
756  // j will be added as a constraint, find the one to remove
757  db = -Abi * An.getCol(j);
758 
759  if(allGreater(db, -tol)) // unbounded
760  return true;
761 
762  // compute alpha / step length to next constraint
763  a.resize(0);
764  for(unsigned int k = 0; k < m; ++k)
765  {
766  if(db[k] < -tol)
767  a.push_back({-x[B[k]]/db[k], k});
768  }
769  // get smallest index for smallest alpha
770  const auto amin = std::min_element(a.begin(), a.end());
771  const double alpha = amin->first;
772  const unsigned int k = amin->second;
773 
774  // pivot between B[k] and N[j]
775  // update x
776  for(unsigned int i = 0; i < B.size(); ++i)
777  x[B[i]] += alpha * db[i];
778  // N part of x
779  x[N[j]] = alpha;
780 
781  // update A and c
782  std::swap(cb[k], cn[j]);
783  for(unsigned int i = 0; i < m; ++i)
784  std::swap(Ab[i][k], An[i][j]);
785 
786  // update B and N
787  std::swap(B[k],N[j]);
788  }
789 }
790 #endif
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:450
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:4452
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:305
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:4800
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:5547
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:580
vpMatrix t() const
Definition: vpMatrix.cpp:507
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:361
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:4136
static bool rowReduction(vpMatrix &A, vpColVector &b, const double &tol=1e-6)
Definition: vpLinProg.cpp:264
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:4096
vpMatrix transpose() const
Definition: vpMatrix.cpp:517
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:492