Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
vpLinProg.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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  }
109  else
110  return false;
111  }
112 
113  // try with standard QR
114  vpMatrix Q, R;
115  unsigned int r;
116 
117  if (full_rank) // caller thinks rank is n or m, can use basic QR
118  {
119  r = A.transpose().qr(Q, R, false, true);
120 
121  // degenerate but easy case - rank is number of columns
122  if (r == n) {
123  // full rank a priori not feasible (A is high thin matrix)
124  const vpColVector x = Q * R.inverseTriangular().t() * b.extract(0, r);
125  if (allClose(A, x, b, tol)) {
126  b = x;
127  A.resize(n, 0);
128  return true;
129  }
130  return false;
131  }
132  else if (r == m) // most common use case - rank is number of rows
133  {
134  b = Q * R.inverseTriangular().t() * b;
135  // build projection to kernel of Q^T, pick n-m independent columns of I - Q.Q^T
136  vpMatrix IQQt = -Q * Q.t();
137  for (unsigned int j = 0; j < n; ++j)
138  IQQt[j][j] += 1;
139  // most of the time the first n-m columns are just fine
140  A = IQQt.extract(0, 0, n, n - m);
141  if (A.qr(Q, R, false, false, tol) != n - m) {
142  // rank deficiency, manually find n-m independent columns
143  unsigned int j0;
144  for (j0 = 0; j0 < n; ++j0) {
145  if (!allZero(IQQt.getCol(j0))) {
146  A = IQQt.getCol(j0);
147  break;
148  }
149  }
150  // fill up
151  unsigned int j = j0 + 1;
152  while (A.getCols() < n - m) {
153  // add next column and check rank of A^T.A
154  if (!allZero(IQQt.getCol(j))) {
155  A = vpMatrix::juxtaposeMatrices(A, IQQt.getCol(j));
156  if (A.qr(Q, R, false, false, tol) != A.getCols())
157  A.resize(n, A.getCols() - 1, false);
158  }
159  j++;
160  }
161  }
162  return true;
163  }
164  }
165 
166  // A may be non full rank, go for QR+Pivot
167  vpMatrix P;
168  r = A.transpose().qrPivot(Q, R, P, false, true);
169  Q = Q.extract(0, 0, n, r);
170  const vpColVector x = Q * R.inverseTriangular().t() * P * b;
171  if (allClose(A, x, b, tol)) {
172  b = x;
173  if (r == n) // no dimension left
174  {
175  A.resize(n, 0);
176  return true;
177  }
178  // build projection to kernel of Q, pick n-r independent columns of I - Q.Q^T
179  vpMatrix IQQt = -Q * Q.t();
180  for (unsigned int j = 0; j < n; ++j)
181  IQQt[j][j] += 1;
182  // most of the time the first n-r columns are just fine
183  A = IQQt.extract(0, 0, n, n - r);
184  if (A.qr(Q, R, false, false, tol) != n - r) {
185  // rank deficiency, manually find n-r independent columns
186  unsigned int j0;
187  for (j0 = 0; j0 < n; ++j0) {
188  if (!allZero(IQQt.getCol(j0))) {
189  A = IQQt.getCol(j0);
190  break;
191  }
192  }
193  // fill up
194  unsigned int j = j0 + 1;
195  while (A.getCols() < n - r) {
196  // add next column and check rank of A^T.A
197  if (!allZero(IQQt.getCol(j))) {
198  A = vpMatrix::juxtaposeMatrices(A, IQQt.getCol(j));
199  if (A.qr(Q, R, false, false, tol) != A.getCols())
200  A.resize(n, A.getCols() - 1, false);
201  }
202  j++;
203  }
204  }
205  return true;
206  }
207  return false;
208 }
209 
248 bool vpLinProg::rowReduction(vpMatrix &A, vpColVector &b, const double &tol)
249 {
250  unsigned int m = A.getRows();
251  unsigned int n = A.getCols();
252 
253  // degeneracy if A is actually null
254  if (A.infinityNorm() < tol) {
255  if (b.infinityNorm() < tol) {
256  b.resize(0);
257  A.resize(0, n);
258  return true;
259  }
260  else
261  return false;
262  }
263 
264  vpMatrix Q, R, P;
265  const unsigned int r = A.qrPivot(Q, R, P, false, false, tol);
266  const vpColVector x = P.transpose() * vpMatrix::stack(R.extract(0, 0, r, r).inverseTriangular(), vpMatrix(n - r, r)) *
267  Q.extract(0, 0, m, r).transpose() * b;
268 
269  if (allClose(A, x, b, tol)) {
270  if (r < m) // if r == m then (A,b) is not changed
271  {
272  A = R.extract(0, 0, r, n) * P;
273  b = Q.extract(0, 0, m, r).transpose() * b;
274  }
275  return true;
276  }
277  return false;
278 }
279 
280 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
342 bool vpLinProg::solveLP(const vpColVector &c, vpMatrix A, vpColVector b, const vpMatrix &C, const vpColVector &d,
343  vpColVector &x, std::vector<BoundedIndex> l, std::vector<BoundedIndex> u, const double &tol)
344 {
345  unsigned int n = c.getRows();
346  unsigned int m = A.getRows();
347  const unsigned int p = C.getRows();
348 
349  // check if we should forward a feasible point to the next solver
350  const bool feasible =
351  (x.getRows() == c.getRows()) && (A.getRows() == 0 || allClose(A, x, b, tol)) &&
352  (C.getRows() == 0 || allLesser(C, x, d, tol)) &&
353  (find_if(l.begin(), l.end(), [&](BoundedIndex &i) { return x[i.first] < i.second - tol; }) == l.end()) &&
354  (find_if(u.begin(), u.end(), [&](BoundedIndex &i) { return x[i.first] > i.second + tol; }) == u.end());
355 
356  // shortcut for unbounded variables with equality
357  if (!feasible && m && l.size() == 0 && u.size() == 0) {
358  // changes A.x = b to x = b + A.z
359  if (colReduction(A, b, false, tol)) {
360  if (A.getCols()) {
361  if (solveLP(A.transpose() * c, vpMatrix(0, n), vpColVector(0), C * A, d - C * b, x, {}, {}, tol)) {
362  x = b + A * x;
363  return true;
364  }
365  }
366  else if (C.getRows() && allLesser(C, b, d, tol)) { // A.x = b has only 1 solution (the new b) and C.b <= d
367  x = b;
368  return true;
369  }
370  }
371  std::cout << "vpLinProg::simplex: equality constraints not feasible" << std::endl;
372  return false;
373  }
374 
375  // count how many additional variables are needed to deal with bounds
376  unsigned int s1 = 0, s2 = 0;
377  for (unsigned int i = 0; i < n; ++i) {
378  const auto cmp = [&](const BoundedIndex &bi) {
379  return bi.first == i;
380  };
381  // look for lower bound
382  const bool has_low = find_if(l.begin(), l.end(), cmp) != l.end();
383  // look for upper bound
384  const bool has_up = find_if(u.begin(), u.end(), cmp) != u.end();
385  if (has_low == has_up) {
386  s1++; // additional variable (double-bounded or unbounded variable)
387  if (has_low)
388  s2++; // additional equality constraint (double-bounded)
389  }
390  }
391 
392  // build equality matrix with slack variables
393  A.resize(m + p + s2, n + p + s1, false);
394  b.resize(A.getRows(), false);
395  if (feasible)
396  x.resize(n + p + s1, false);
397 
398  // deal with slack variables for inequality
399  // Cx <= d <=> Cx + y = d
400  for (unsigned int i = 0; i < p; ++i) {
401  A[m + i][n + i] = 1;
402  b[m + i] = d[i];
403  for (unsigned int j = 0; j < n; ++j)
404  A[m + i][j] = C[i][j];
405  if (feasible)
406  x[n + i] = d[i] - C.getRow(i) * x.extract(0, n);
407  }
408  // x = P.(z - z0)
409  vpMatrix P;
410  P.eye(n, n + p + s1);
411  vpColVector z0(n + p + s1);
412 
413  // slack variables for bounded terms
414  // base slack variable is z1 (replaces x)
415  // additional is z2
416  unsigned int k1 = 0, k2 = 0;
417  for (unsigned int i = 0; i < n; ++i) {
418  // lambda to find a bound for this index
419  const auto cmp = [&](const BoundedIndex &bi) {
420  return bi.first == i;
421  };
422 
423  // look for lower bound
424  const auto low = find_if(l.begin(), l.end(), cmp);
425  // look for upper bound
426  const auto up = find_if(u.begin(), u.end(), cmp);
427 
428  if (low == l.end()) // no lower bound
429  {
430  if (up == u.end()) // no bounds, x = z1 - z2
431  {
432  P[i][n + p + k1] = -1;
433  for (unsigned int j = 0; j < m + p; ++j)
434  A[j][n + p + k1] = -A[j][i];
435  if (feasible) {
436  x[i] = std::max<double>(x[i], 0.);
437  x[n + p + k1] = std::max<double>(-x[i], 0.);
438  }
439  k1++;
440  }
441  else // upper bound x <= u <=> z1 = -x + u >= 0
442  {
443  z0[i] = up->second;
444  P[i][i] = -1;
445  for (unsigned int j = 0; j < m + p; ++j)
446  A[j][i] *= -1;
447  if (feasible)
448  x[i] = up->second - x[i];
449  u.erase(up);
450  }
451  }
452  else // lower bound x >= l <=> z1 = x - l >= 0
453  {
454  z0[i] = -low->second;
455  if (up != u.end()) // both bounds z1 + z2 = u - l
456  {
457  A[m + p + k2][i] = A[m + p + k2][n + p + k1] = 1;
458  b[m + p + k2] = up->second - low->second;
459  if (feasible) {
460  x[i] = up->second - x[i];
461  x[n + p + k1] = x[i] - low->second;
462  }
463  k1++;
464  k2++;
465  u.erase(up);
466  }
467  else if (feasible) // only lower bound
468  x[i] = x[i] - low->second;
469  l.erase(low);
470  }
471  }
472 
473  // x = P.(z-z0)
474  // c^T.x = (P^T.c)^T.z
475  // A.x - b = A.P.Z - (b + A.P.z0)
476  // A is already A.P
477  if (vpLinProg::simplex(P.transpose() * c, A, b + A * z0, x, tol)) {
478  x = P * (x - z0);
479  return true;
480  }
481  return false;
482 }
483 
543 bool vpLinProg::simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol)
544 {
545  unsigned int n = c.getRows();
546  unsigned int m = A.getRows();
547 
548  // find a feasible point is passed x is not
549  if ((x.getRows() != c.getRows()) || !allGreater(x, -tol) || (m != 0 && !allClose(A, x, b, tol)) || [&x, &n]() {
550  unsigned int non0 = 0; // count non-0 in x, feasible if <= m
551  for (unsigned int i = 0; i < n; ++i) {
552  if (x[i] > 0) {
553  non0++;
554  }
555  }
556  return non0;
557  }() > m) {
558  // min sum(z)
559  // st A.x + D.z = with diag(D) = sign(b)
560  // feasible = (0, |b|)
561  // z should be minimized to 0 to get a feasible point for this simplex
562  vpColVector cz(n + m);
563  vpMatrix AD(m, n + m);
564  x.resize(n + m);
565  for (unsigned int i = 0; i < m; ++i) {
566  // build AD and x
567  if (b[i] > -tol) {
568  AD[i][n + i] = 1;
569  x[n + i] = b[i];
570  }
571  else {
572  AD[i][n + i] = -1;
573  x[n + i] = -b[i];
574  }
575  cz[n + i] = 1;
576 
577  // copy A
578  for (unsigned int j = 0; j < n; ++j)
579  AD[i][j] = A[i][j];
580  }
581  // solve to get feasibility
582  vpLinProg::simplex(cz, AD, b, x, tol);
583  if (!allLesser(x.extract(n, m), tol)) // no feasible starting point found
584  {
585  std::cout << "vpLinProg::simplex: constraints not feasible" << std::endl;
586  x.resize(n);
587  return false;
588  }
589  // extract feasible x
590  x = x.extract(0, n);
591  }
592  // feasible part x is >= 0 and Ax = b
593 
594  // try to reduce number of rows to remove rank deficiency
595  if (!rowReduction(A, b, tol)) {
596  std::cout << "vpLinProg::simplex: equality constraint not feasible" << std::endl;
597  return false;
598  }
599  m = A.getRows();
600  if (m == 0) {
601  // no constraints, solution is either unbounded or 0
602  x = 0;
603  return true;
604  }
605 
606  // build base index from feasible point
607  vpMatrix Ab(m, m), An(m, n - m), Abi;
608  vpColVector cb(m), cn(n - m);
609  std::vector<unsigned int> B, N;
610  // add all non-0 indices to B
611  for (unsigned int i = 0; i < n; ++i) {
612  if (std::abs(x[i]) > tol)
613  B.push_back(i);
614  }
615  // if B not full then try to get Ab without null rows
616  // get null rows of current Ab = A[B]
617  std::vector<unsigned int> null_rows;
618  for (unsigned int i = 0; i < m; ++i) {
619  bool is_0 = true;
620  for (unsigned int j = 0; j < B.size(); ++j) {
621  if (std::abs(A[i][B[j]]) > tol) {
622  is_0 = false;
623  break;
624  }
625  }
626  if (is_0)
627  null_rows.push_back(i);
628  }
629 
630  // add columns to B only if make Ab non-null
631  // start from the end as it makes vpQuadProg faster
632  for (unsigned int j = n; j-- > 0;) {
633  if (std::abs(x[j]) < tol) {
634  bool in_N = true;
635  if (B.size() != m) {
636  in_N = false;
637  for (unsigned int i = 0; i < null_rows.size(); ++i) {
638  in_N = true;
639  if (std::abs(A[null_rows[i]][j]) > tol) {
640  null_rows.erase(null_rows.begin() + i);
641  in_N = false;
642  break;
643  }
644  }
645  // update empty for next time
646  if (!in_N && null_rows.size()) {
647  bool redo = true;
648  while (redo) {
649  redo = false;
650  for (unsigned int i = 0; i < null_rows.size(); ++i) {
651  if (std::abs(A[null_rows[i]][j]) > tol) {
652  redo = true;
653  null_rows.erase(null_rows.begin() + i);
654  break;
655  }
656  }
657  }
658  }
659  }
660  if (in_N)
661  N.push_back(j);
662  else
663  B.push_back(j);
664  }
665  }
666  // split A into (Ab, An) and c into (cb, cn)
667  for (unsigned int j = 0; j < m; ++j) {
668  cb[j] = c[B[j]];
669  for (unsigned int i = 0; i < m; ++i)
670  Ab[i][j] = A[i][B[j]];
671  }
672  for (unsigned int j = 0; j < n - m; ++j) {
673  cn[j] = c[N[j]];
674  for (unsigned int i = 0; i < m; ++i)
675  An[i][j] = A[i][N[j]];
676  }
677 
678  std::vector<std::pair<double, unsigned int> > a;
679  a.reserve(n);
680 
681  vpColVector R(n - m), db(m);
682 
683  while (true) {
684  Abi = Ab.inverseByQR();
685  // find negative residual
686  R = cn - An.t() * Abi.t() * cb;
687  unsigned int j;
688  for (j = 0; j < N.size(); ++j) {
689  if (R[j] < -tol)
690  break;
691  }
692 
693  // no negative residual -> optimal point
694  if (j == N.size())
695  return true;
696 
697  // j will be added as a constraint, find the one to remove
698  db = -Abi * An.getCol(j);
699 
700  if (allGreater(db, -tol)) // unbounded
701  return true;
702 
703  // compute alpha / step length to next constraint
704  a.resize(0);
705  for (unsigned int k = 0; k < m; ++k) {
706  if (db[k] < -tol)
707  a.push_back({ -x[B[k]] / db[k], k });
708  }
709  // get smallest index for smallest alpha
710  const auto amin = std::min_element(a.begin(), a.end());
711  const double alpha = amin->first;
712  const unsigned int k = amin->second;
713 
714  // pivot between B[k] and N[j]
715  // update x
716  for (unsigned int i = 0; i < B.size(); ++i)
717  x[B[i]] += alpha * db[i];
718  // N part of x
719  x[N[j]] = alpha;
720 
721  // update A and c
722  std::swap(cb[k], cn[j]);
723  for (unsigned int i = 0; i < m; ++i)
724  std::swap(Ab[i][k], An[i][j]);
725 
726  // update B and N
727  std::swap(B[k], N[j]);
728  }
729 }
730 
731 #endif
unsigned int getCols() const
Definition: vpArray2D.h:274
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
unsigned int getRows() const
Definition: vpArray2D.h:284
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:367
vpRowVector t() const
double infinityNorm() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
static bool allGreater(const vpColVector &x, const double &thr=1e-6)
Definition: vpLinProg.h:215
static bool rowReduction(vpMatrix &A, vpColVector &b, const double &tol=1e-6)
Definition: vpLinProg.cpp:248
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:342
static bool allZero(const vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.h:144
static bool colReduction(vpMatrix &A, vpColVector &b, bool full_rank=false, const double &tol=1e-6)
Definition: vpLinProg.cpp:97
static bool allLesser(const vpMatrix &C, const vpColVector &x, const vpColVector &d, const double &thr=1e-6)
Definition: vpLinProg.h:181
static bool allClose(const vpMatrix &A, const vpColVector &x, const vpColVector &b, const double &tol=1e-6)
Definition: vpLinProg.h:163
static bool simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.cpp:543
std::pair< unsigned int, double > BoundedIndex
Definition: vpLinProg.h:114
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
unsigned int qr(vpMatrix &Q, vpMatrix &R, bool full=false, bool squareR=false, double tol=1e-6) const
void eye()
Definition: vpMatrix.cpp:442
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5601
vpMatrix inverseTriangular(bool upper=true) const
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:5257
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:4955
unsigned int qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full=false, bool squareR=false, double tol=1e-6) const
vpMatrix t() const
Definition: vpMatrix.cpp:457
double infinityNorm() const
Definition: vpMatrix.cpp:6487
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:4917
vpMatrix inverseByQR() const
vpMatrix transpose() const
Definition: vpMatrix.cpp:464
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:400