Visual Servoing Platform  version 3.2.1 under development (2019-08-21) under development (2019-08-21)
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  const unsigned int m = A.getRows();
100  const 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  const unsigned int m = A.getRows();
267  const unsigned int n = A.getCols();
268 
269  vpMatrix Q, R, P;
270  const unsigned int r = A.qrPivot(Q, R, P, false, false, tol);
271  const vpColVector x = P.transpose() *
272  vpMatrix::stack(R.extract(0, 0, r, r).inverseTriangular(), vpMatrix(n-r, r))
273  * Q.extract(0, 0, m, r).transpose() * b;
274 
275  if(allClose(A, x, b, tol))
276  {
277  if(r < m) // if r == m then (A,b) is not changed
278  {
279  A = R.extract(0, 0, r, n)*P;
280  b = Q.extract(0, 0, m, r).transpose() * b;
281  }
282  return true;
283  }
284  return false;
285 }
286 
287 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
288 
349  const vpMatrix &C, const vpColVector &d, vpColVector &x,
350  std::vector<BoundedIndex> l, std::vector<BoundedIndex> u,
351  const double &tol)
352 {
353  const unsigned int n = c.getRows();
354  const unsigned int m = A.getRows();
355  const unsigned int p = C.getRows();
356 
357  // check if we should forward a feasible point to the next solver
358  const bool feasible =
359  (x.getRows() == c.getRows())
360  && (A.getRows() == 0 || allClose(A, x, b, tol))
361  && (C.getRows() == 0 || allLesser(C, x, d, tol))
362  && (find_if(l.begin(), l.end(),
363  [&](BoundedIndex &i){return x[i.first] < i.second-tol;})
364  == l.end())
365  && (find_if(u.begin(), u.end(),
366  [&](BoundedIndex &i){return x[i.first] > i.second+tol;})
367  == u.end());
368 
369  // shortcut for unbounded variables with equality
370  if(!feasible && m && l.size() == 0 && u.size() == 0)
371  {
372  // changes A.x = b to x = b + A.z
373  if(colReduction(A, b, false, tol))
374  {
375  if(A.getCols())
376  {
377  if(solveLP(A.transpose()*c, vpMatrix(0, n), vpColVector(0), C*A, d - C*b, x, {}, {}, tol))
378  {
379  x = b + A*x;
380  return true;
381  }
382  }
383  else if(C.getRows() && allLesser(C, b, d, tol))
384  { // A.x = b has only 1 solution (the new b) and C.b <= d
385  x = b;
386  return true;
387  }
388  }
389  std::cout << "vpLinProg::simplex: equality constraints not feasible" << std::endl;
390  return false;
391  }
392 
393  // count how many additional variables are needed to deal with bounds
394  unsigned int s1 = 0, s2 = 0;
395  for(unsigned int i = 0; i < n; ++i)
396  {
397  const auto cmp = [&](const BoundedIndex &p){return p.first == i;};
398  // look for lower bound
399  const bool has_low = find_if(l.begin(), l.end(), cmp) != l.end();
400  // look for upper bound
401  const bool has_up = find_if(u.begin(), u.end(), cmp) != u.end();
402  if(has_low == has_up)
403  {
404  s1++; // additional variable (double-bounded or unbounded variable)
405  if(has_low)
406  s2++; // additional equality constraint (double-bounded)
407  }
408  }
409 
410  // build equality matrix with slack variables
411  A.resize(m+p+s2, n+p+s1, false);
412  b.resize(A.getRows(),false);
413  if(feasible)
414  x.resize(n+p+s1, false);
415 
416  // deal with slack variables for inequality
417  // Cx <= d <=> Cx + y = d
418  for(unsigned int i = 0; i < p; ++i)
419  {
420  A[m+i][n+i] = 1;
421  b[m+i] = d[i];
422  for(unsigned int j = 0; j < n; ++j)
423  A[m+i][j] = C[i][j];
424  if(feasible)
425  x[n+i] = d[i] - C.getRow(i)*x.extract(0,n);
426  }
427  // x = P.(z - z0)
428  vpMatrix P;P.eye(n, n+p+s1);
429  vpColVector z0(n+p+s1);
430 
431  // slack variables for bounded terms
432  // base slack variable is z1 (replaces x)
433  // additional is z2
434  unsigned int k1 = 0, k2 = 0;
435  for(unsigned int i = 0; i < n; ++i)
436  {
437  // lambda to find a bound for this index
438  const auto cmp = [&](const BoundedIndex &p)
439  {return p.first == i;};
440 
441  // look for lower bound
442  const auto low = find_if(l.begin(), l.end(), cmp);
443  // look for upper bound
444  const auto up = find_if(u.begin(), u.end(), cmp);
445 
446  if(low == l.end()) // no lower bound
447  {
448  if(up == u.end()) // no bounds, x = z1 - z2
449  {
450  P[i][n+p+k1] = -1;
451  for(unsigned int j = 0; j < m+p; ++j)
452  A[j][n+p+k1] = -A[j][i];
453  if(feasible)
454  {
455  x[i] = std::max(x[i], 0.);
456  x[n+p+k1] = std::max(-x[i], 0.);
457  }
458  k1++;
459  }
460  else // upper bound x <= u <=> z1 = -x + u >= 0
461  {
462  z0[i] = up->second;
463  P[i][i] = -1;
464  for(unsigned int j = 0; j < m+p; ++j)
465  A[j][i] *= -1;
466  if(feasible)
467  x[i] = up->second - x[i];
468  u.erase(up);
469  }
470  }
471  else // lower bound x >= l <=> z1 = x - l >= 0
472  {
473  z0[i] = -low->second;
474  if(up != u.end()) // both bounds z1 + z2 = u - l
475  {
476  A[m+p+k2][i] = A[m+p+k2][n+p+k1] = 1;
477  b[m+p+k2] = up->second - low->second;
478  if(feasible)
479  {
480  x[i] = up->second - x[i];
481  x[n+p+k1] = x[i] - low->second;
482  }
483  k1++;
484  k2++;
485  u.erase(up);
486  }
487  else if(feasible) // only lower bound
488  x[i] = x[i] - low->second;
489  l.erase(low);
490  }
491  }
492 
493  // x = P.(z-z0)
494  // c^T.x = (P^T.c)^T.z
495  // A.x - b = A.P.Z - (b + A.P.z0)
496  // A is already A.P
497  if(vpLinProg::simplex(P.transpose()*c, A, b+A*z0, x, tol))
498  {
499  x = P*(x - z0);
500  return true;
501  }
502  return false;
503 }
504 
564 bool vpLinProg::simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol)
565 {
566  const unsigned int n = c.getRows();
567  unsigned int m = A.getRows();
568 
569  // find a feasible point is passed x is not
570  if( (x.getRows() != c.getRows()) ||
571  !allGreater(x, -tol) ||
572  (m != 0 && !allClose(A, x, b, tol)) ||
573  [&x,&n](){unsigned int non0 = 0; // count non-0 in x, feasible if <= m
574  for(unsigned int i = 0; i < n; ++i)
575  if(x[i] > 0) non0++;
576  return non0;}() > m
577  )
578  {
579  // min sum(z)
580  // st A.x + D.z = with diag(D) = sign(b)
581  // feasible = (0, |b|)
582  // z should be minimized to 0 to get a feasible point for this simplex
583  vpColVector cz(n+m);
584  vpMatrix AD(m,n+m);
585  x.resize(n+m);
586  for(unsigned int i = 0; i < m; ++i)
587  {
588  // build AD and x
589  if(b[i] > -tol)
590  {
591  AD[i][n+i] = 1;
592  x[n+i] = b[i];
593  }
594  else
595  {
596  AD[i][n+i] = -1;
597  x[n+i] = -b[i];
598  }
599  cz[n+i] = 1;
600 
601  // copy A
602  for(unsigned int j = 0; j < n; ++j)
603  AD[i][j] = A[i][j];
604  }
605  // solve to get feasibility
606  vpLinProg::simplex(cz, AD, b, x, tol);
607  if(!allLesser(x.extract(n,m), tol)) // no feasible starting point found
608  {
609  std::cout << "vpLinProg::simplex: constraints not feasible" << std::endl;
610  x.resize(n);
611  return false;
612  }
613  // extract feasible x
614  x = x.extract(0, n);
615  }
616  // feasible part x is >= 0 and Ax = b
617 
618  // try to reduce number of rows to remove rank deficiency
619  if(!rowReduction(A, b, tol))
620  {
621  std::cout << "vpLinProg::simplex: equality constraint not feasible" << std::endl;
622  return false;
623  }
624  m = A.getRows();
625  if(m == 0)
626  {
627  // no constraints, solution is either unbounded or 0
628  x = 0;
629  return true;
630  }
631 
632  // build base index from feasible point
633  vpMatrix Ab(m,m), An(m,n-m), Abi;
634  vpColVector cb(m), cn(n-m);
635  std::vector<unsigned int> B, N;
636  // add all non-0 indices to B
637  for(unsigned int i = 0; i < n; ++i)
638  {
639  if(std::abs(x[i]) > tol)
640  B.push_back(i);
641  }
642  // if B not full then try to get Ab without null rows
643  // get null rows of current Ab = A[B]
644  std::vector<unsigned int> null_rows;
645  for(unsigned int i = 0; i < m; ++i)
646  {
647  bool is_0 = true;
648  for(unsigned int j = 0; j < B.size(); ++j)
649  {
650  if(std::abs(A[i][B[j]]) > tol)
651  {
652  is_0 = false;
653  break;
654  }
655  }
656  if(is_0)
657  null_rows.push_back(i);
658  }
659 
660  // add columns to B only if make Ab non-null
661  // start from the end as it makes vpQuadProg faster
662  for(unsigned int j = n; j-- > 0; )
663  {
664  if(std::abs(x[j]) < tol)
665  {
666  bool in_N = true;
667  if(B.size() != m)
668  {
669  in_N = false;
670  for(unsigned int i = 0; i < null_rows.size(); ++i)
671  {
672  in_N = true;
673  if(std::abs(A[null_rows[i]][j]) > tol)
674  {
675  null_rows.erase(null_rows.begin()+i);
676  in_N = false;
677  break;
678  }
679  }
680  // update empty for next time
681  if(!in_N && null_rows.size())
682  {
683  bool redo = true;
684  while(redo)
685  {
686  redo = false;
687  for(unsigned int i = 0; i < null_rows.size(); ++i)
688  {
689  if(std::abs(A[null_rows[i]][j]) > tol)
690  {
691  redo = true;
692  null_rows.erase(null_rows.begin()+i);
693  break;
694  }
695  }
696  }
697  }
698  }
699  if(in_N)
700  N.push_back(j);
701  else
702  B.push_back(j);
703  }
704  }
705  // split A into (Ab, An) and c into (cb, cn)
706  for(unsigned int j = 0; j < m; ++j)
707  {
708  cb[j] = c[B[j]];
709  for(unsigned int i = 0; i < m; ++i)
710  Ab[i][j] = A[i][B[j]];
711  }
712  for(unsigned int j = 0; j < n-m; ++j)
713  {
714  cn[j] = c[N[j]];
715  for(unsigned int i = 0; i < m; ++i)
716  An[i][j] = A[i][N[j]];
717  }
718 
719  std::vector<std::pair<double, unsigned int>> a;
720  a.reserve(n);
721 
722  vpColVector R(n-m), db(m);
723 
724  while(true)
725  {
726  Abi = Ab.inverseByQR();
727  // find negative residual
728  R = cn - An.t()*Abi.t()*cb;
729  unsigned int j;
730  for(j = 0; j < N.size(); ++j)
731  {
732  if(R[j] < -tol)
733  break;
734  }
735 
736  // no negative residual -> optimal point
737  if(j == N.size())
738  return true;
739 
740  // j will be added as a constraint, find the one to remove
741  db = -Abi * An.getCol(j);
742 
743  if(allGreater(db, -tol)) // unbounded
744  return true;
745 
746  // compute alpha / step length to next constraint
747  a.resize(0);
748  for(unsigned int k = 0; k < m; ++k)
749  {
750  if(db[k] < -tol)
751  a.push_back({-x[B[k]]/db[k], k});
752  }
753  // get smallest index for smallest alpha
754  const auto amin = std::min_element(a.begin(), a.end());
755  const double alpha = amin->first;
756  const unsigned int k = amin->second;
757 
758  // pivot between B[k] and N[j]
759  // update x
760  for(unsigned int i = 0; i < B.size(); ++i)
761  x[B[i]] += alpha * db[i];
762  // N part of x
763  x[N[j]] = alpha;
764 
765  // update A and c
766  std::swap(cb[k], cn[j]);
767  for(unsigned int i = 0; i < m; ++i)
768  std::swap(Ab[i][k], An[i][j]);
769 
770  // update B and N
771  std::swap(B[k],N[j]);
772  }
773 }
774 #endif
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:319
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:164
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:4236
static bool colReduction(vpMatrix &A, vpColVector &b, bool full_rank=false, const double &tol=1e-6)
Definition: vpLinProg.cpp:97
vpRowVector getRow(const unsigned int i) const
Definition: vpMatrix.cpp:3920
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
void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify=true, const bool recopy_=true)
Definition: vpArray2D.h:305
std::pair< unsigned int, double > BoundedIndex
Definition: vpLinProg.h:122
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:4584
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:5331
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:564
vpMatrix t() const
Definition: vpMatrix.cpp:375
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:348
static bool rowReduction(vpMatrix &A, vpColVector &b, const double &tol=1e-6)
Definition: vpLinProg.cpp:264
vpMatrix transpose() const
Definition: vpMatrix.cpp:394
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
vpColVector getCol(const unsigned int j) const
Definition: vpMatrix.cpp:3880
static bool allZero(const vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.h:155
void eye()
Definition: vpMatrix.cpp:360
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:310