Visual Servoing Platform  version 3.6.1 under development (2024-12-12)
vpLinProg.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Linear Programming
32  */
33 
34 #include <visp3/core/vpLinProg.h>
35 
36 BEGIN_VISP_NAMESPACE
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))) {
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))) {
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 
252 bool vpLinProg::rowReduction(vpMatrix &A, vpColVector &b, const double &tol)
253 {
254  unsigned int m = A.getRows();
255  unsigned int n = A.getCols();
256 
257  // degeneracy if A is actually null
258  if (A.infinityNorm() < tol) {
259  if (b.infinityNorm() < tol) {
260  b.resize(0);
261  A.resize(0, n);
262  return true;
263  }
264  else
265  return false;
266  }
267 
268  vpMatrix Q, R, P;
269  const unsigned int r = A.qrPivot(Q, R, P, false, false, tol);
270  const vpColVector x = P.transpose() * vpMatrix::stack(R.extract(0, 0, r, r).inverseTriangular(), vpMatrix(n - r, r)) *
271  Q.extract(0, 0, m, r).transpose() * b;
272 
273  if (allClose(A, x, b, tol)) {
274  if (r < m) // if r == m then (A,b) is not changed
275  {
276  A = R.extract(0, 0, r, n) * P;
277  b = Q.extract(0, 0, m, r).transpose() * b;
278  }
279  return true;
280  }
281  return false;
282 }
283 
284 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
349 bool vpLinProg::solveLP(const vpColVector &c, vpMatrix A, vpColVector b, const vpMatrix &C, const vpColVector &d,
350  vpColVector &x, std::vector<BoundedIndex> l, std::vector<BoundedIndex> u, const double &tol)
351 {
352  unsigned int n = c.getRows();
353  unsigned int m = A.getRows();
354  const unsigned int p = C.getRows();
355 
356  // check if we should forward a feasible point to the next solver
357  const bool feasible =
358  (x.getRows() == c.getRows()) && (A.getRows() == 0 || allClose(A, x, b, tol)) &&
359  (C.getRows() == 0 || allLesser(C, x, d, tol)) &&
360  (find_if(l.begin(), l.end(), [&](BoundedIndex &i) { return x[i.first] < i.second - tol; }) == l.end()) &&
361  (find_if(u.begin(), u.end(), [&](BoundedIndex &i) { return x[i.first] > i.second + tol; }) == u.end());
362 
363  // shortcut for unbounded variables with equality
364  if (!feasible && m && l.size() == 0 && u.size() == 0) {
365  // changes A.x = b to x = b + A.z
366  if (colReduction(A, b, false, tol)) {
367  if (A.getCols()) {
368  if (solveLP(A.transpose() * c, vpMatrix(0, n), vpColVector(0), C * A, d - C * b, x, {}, {}, tol)) {
369  x = b + A * x;
370  return true;
371  }
372  }
373  else if (C.getRows() && allLesser(C, b, d, tol)) { // A.x = b has only 1 solution (the new b) and C.b <= d
374  x = b;
375  return true;
376  }
377  }
378  std::cout << "vpLinProg::simplex: equality constraints not feasible" << std::endl;
379  return false;
380  }
381 
382  // count how many additional variables are needed to deal with bounds
383  unsigned int s1 = 0, s2 = 0;
384  for (unsigned int i = 0; i < n; ++i) {
385  const auto cmp = [&](const BoundedIndex &bi) {
386  return bi.first == i;
387  };
388  // look for lower bound
389  const bool has_low = find_if(l.begin(), l.end(), cmp) != l.end();
390  // look for upper bound
391  const bool has_up = find_if(u.begin(), u.end(), cmp) != u.end();
392  if (has_low == has_up) {
393  s1++; // additional variable (double-bounded or unbounded variable)
394  if (has_low)
395  s2++; // additional equality constraint (double-bounded)
396  }
397  }
398 
399  // build equality matrix with slack variables
400  A.resize(m + p + s2, n + p + s1, false);
401  b.resize(A.getRows(), false);
402  if (feasible)
403  x.resize(n + p + s1, false);
404 
405  // deal with slack variables for inequality
406  // Cx <= d <=> Cx + y = d
407  for (unsigned int i = 0; i < p; ++i) {
408  A[m + i][n + i] = 1;
409  b[m + i] = d[i];
410  for (unsigned int j = 0; j < n; ++j)
411  A[m + i][j] = C[i][j];
412  if (feasible)
413  x[n + i] = d[i] - C.getRow(i) * x.extract(0, n);
414  }
415  // x = P.(z - z0)
416  vpMatrix P;
417  P.eye(n, n + p + s1);
418  vpColVector z0(n + p + s1);
419 
420  // slack variables for bounded terms
421  // base slack variable is z1 (replaces x)
422  // additional is z2
423  unsigned int k1 = 0, k2 = 0;
424  for (unsigned int i = 0; i < n; ++i) {
425  // lambda to find a bound for this index
426  const auto cmp = [&](const BoundedIndex &bi) {
427  return bi.first == i;
428  };
429 
430  // look for lower bound
431  const auto low = find_if(l.begin(), l.end(), cmp);
432  // look for upper bound
433  const auto up = find_if(u.begin(), u.end(), cmp);
434 
435  if (low == l.end()) // no lower bound
436  {
437  if (up == u.end()) // no bounds, x = z1 - z2
438  {
439  P[i][n + p + k1] = -1;
440  for (unsigned int j = 0; j < m + p; ++j)
441  A[j][n + p + k1] = -A[j][i];
442  if (feasible) {
443  x[i] = std::max<double>(x[i], 0.);
444  x[n + p + k1] = std::max<double>(-x[i], 0.);
445  }
446  k1++;
447  }
448  else // upper bound x <= u <=> z1 = -x + u >= 0
449  {
450  z0[i] = up->second;
451  P[i][i] = -1;
452  for (unsigned int j = 0; j < m + p; ++j)
453  A[j][i] *= -1;
454  if (feasible)
455  x[i] = up->second - x[i];
456  u.erase(up);
457  }
458  }
459  else // lower bound x >= l <=> z1 = x - l >= 0
460  {
461  z0[i] = -low->second;
462  if (up != u.end()) // both bounds z1 + z2 = u - l
463  {
464  A[m + p + k2][i] = A[m + p + k2][n + p + k1] = 1;
465  b[m + p + k2] = up->second - low->second;
466  if (feasible) {
467  x[i] = up->second - x[i];
468  x[n + p + k1] = x[i] - low->second;
469  }
470  k1++;
471  k2++;
472  u.erase(up);
473  }
474  else if (feasible) // only lower bound
475  x[i] = x[i] - low->second;
476  l.erase(low);
477  }
478  }
479 
480  // x = P.(z-z0)
481  // c^T.x = (P^T.c)^T.z
482  // A.x - b = A.P.Z - (b + A.P.z0)
483  // A is already A.P
484  if (vpLinProg::simplex(P.transpose() * c, A, b + A * z0, x, tol)) {
485  x = P * (x - z0);
486  return true;
487  }
488  return false;
489 }
490 
553 bool vpLinProg::simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol)
554 {
555  unsigned int n = c.getRows();
556  unsigned int m = A.getRows();
557 
558  // find a feasible point is passed x is not
559  if ((x.getRows() != c.getRows()) || !allGreater(x, -tol) || (m != 0 && !allClose(A, x, b, tol)) || [&x, &n]() {
560  unsigned int non0 = 0; // count non-0 in x, feasible if <= m
561  for (unsigned int i = 0; i < n; ++i) {
562  if (x[i] > 0) {
563  non0++;
564  }
565  }
566  return non0;
567  }() > m) {
568  // min sum(z)
569  // st A.x + D.z = with diag(D) = sign(b)
570  // feasible = (0, |b|)
571  // z should be minimized to 0 to get a feasible point for this simplex
572  vpColVector cz(n + m);
573  vpMatrix AD(m, n + m);
574  x.resize(n + m);
575  for (unsigned int i = 0; i < m; ++i) {
576  // build AD and x
577  if (b[i] > -tol) {
578  AD[i][n + i] = 1;
579  x[n + i] = b[i];
580  }
581  else {
582  AD[i][n + i] = -1;
583  x[n + i] = -b[i];
584  }
585  cz[n + i] = 1;
586 
587  // copy A
588  for (unsigned int j = 0; j < n; ++j)
589  AD[i][j] = A[i][j];
590  }
591  // solve to get feasibility
592  vpLinProg::simplex(cz, AD, b, x, tol);
593  if (!allLesser(x.extract(n, m), tol)) // no feasible starting point found
594  {
595  std::cout << "vpLinProg::simplex: constraints not feasible" << std::endl;
596  x.resize(n);
597  return false;
598  }
599  // extract feasible x
600  x = x.extract(0, n);
601  }
602  // feasible part x is >= 0 and Ax = b
603 
604  // try to reduce number of rows to remove rank deficiency
605  if (!rowReduction(A, b, tol)) {
606  std::cout << "vpLinProg::simplex: equality constraint not feasible" << std::endl;
607  return false;
608  }
609  m = A.getRows();
610  if (m == 0) {
611  // no constraints, solution is either unbounded or 0
612  x = 0;
613  return true;
614  }
615 
616  // build base index from feasible point
617  vpMatrix Ab(m, m), An(m, n - m), Abi;
618  vpColVector cb(m), cn(n - m);
619  std::vector<unsigned int> B, N;
620  // add all non-0 indices to B
621  for (unsigned int i = 0; i < n; ++i) {
622  if (std::abs(x[i]) > tol)
623  B.push_back(i);
624  }
625  // if B not full then try to get Ab without null rows
626  // get null rows of current Ab = A[B]
627  std::vector<unsigned int> null_rows;
628  for (unsigned int i = 0; i < m; ++i) {
629  bool is_0 = true;
630  for (unsigned int j = 0; j < B.size(); ++j) {
631  if (std::abs(A[i][B[j]]) > tol) {
632  is_0 = false;
633  break;
634  }
635  }
636  if (is_0)
637  null_rows.push_back(i);
638  }
639 
640  // add columns to B only if make Ab non-null
641  // start from the end as it makes vpQuadProg faster
642  for (unsigned int j = n; j-- > 0;) {
643  if (std::abs(x[j]) < tol) {
644  bool in_N = true;
645  if (B.size() != m) {
646  in_N = false;
647  for (unsigned int i = 0; i < null_rows.size(); ++i) {
648  in_N = true;
649  if (std::abs(A[null_rows[i]][j]) > tol) {
650  null_rows.erase(null_rows.begin() + i);
651  in_N = false;
652  break;
653  }
654  }
655  // update empty for next time
656  if (!in_N && null_rows.size()) {
657  bool redo = true;
658  while (redo) {
659  redo = false;
660  for (unsigned int i = 0; i < null_rows.size(); ++i) {
661  if (std::abs(A[null_rows[i]][j]) > tol) {
662  redo = true;
663  null_rows.erase(null_rows.begin() + i);
664  break;
665  }
666  }
667  }
668  }
669  }
670  if (in_N)
671  N.push_back(j);
672  else
673  B.push_back(j);
674  }
675  }
676  // split A into (Ab, An) and c into (cb, cn)
677  for (unsigned int j = 0; j < m; ++j) {
678  cb[j] = c[B[j]];
679  for (unsigned int i = 0; i < m; ++i)
680  Ab[i][j] = A[i][B[j]];
681  }
682  for (unsigned int j = 0; j < n - m; ++j) {
683  cn[j] = c[N[j]];
684  for (unsigned int i = 0; i < m; ++i)
685  An[i][j] = A[i][N[j]];
686  }
687 
688  std::vector<std::pair<double, unsigned int> > a;
689  a.reserve(n);
690 
691  vpColVector R(n - m), db(m);
692 
693  while (true) {
694  Abi = Ab.inverseByQR();
695  // find negative residual
696  R = cn - An.t() * Abi.t() * cb;
697  unsigned int j;
698  for (j = 0; j < N.size(); ++j) {
699  if (R[j] < -tol)
700  break;
701  }
702 
703  // no negative residual -> optimal point
704  if (j == N.size())
705  return true;
706 
707  // j will be added as a constraint, find the one to remove
708  db = -Abi * An.getCol(j);
709 
710  if (allGreater(db, -tol)) // unbounded
711  return true;
712 
713  // compute alpha / step length to next constraint
714  a.resize(0);
715  for (unsigned int k = 0; k < m; ++k) {
716  if (db[k] < -tol)
717  a.push_back({ -x[B[k]] / db[k], k });
718  }
719  // get smallest index for smallest alpha
720  const auto amin = std::min_element(a.begin(), a.end());
721  const double alpha = amin->first;
722  const unsigned int k = amin->second;
723 
724  // pivot between B[k] and N[j]
725  // update x
726  for (unsigned int i = 0; i < B.size(); ++i)
727  x[B[i]] += alpha * db[i];
728  // N part of x
729  x[N[j]] = alpha;
730 
731  // update A and c
732  std::swap(cb[k], cn[j]);
733  for (unsigned int i = 0; i < m; ++i)
734  std::swap(Ab[i][k], An[i][j]);
735 
736  // update B and N
737  std::swap(B[k], N[j]);
738  }
739 }
740 #endif
741 END_VISP_NAMESPACE
unsigned int getCols() const
Definition: vpArray2D.h:337
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:362
unsigned int getRows() const
Definition: vpArray2D.h:347
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:405
vpRowVector t() const
double infinityNorm() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1143
static bool allGreater(const vpColVector &x, const double &thr=1e-6)
Definition: vpLinProg.h:220
static bool rowReduction(vpMatrix &A, vpColVector &b, const double &tol=1e-6)
Definition: vpLinProg.cpp:252
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:349
static bool allZero(const vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.h:149
static bool allLesser(const vpMatrix &C, const vpColVector &x, const vpColVector &d, const double &thr=1e-6)
Definition: vpLinProg.h:186
static bool allClose(const vpMatrix &A, const vpColVector &x, const vpColVector &b, const double &tol=1e-6)
Definition: vpLinProg.h:168
static bool colReduction(vpMatrix &A, vpColVector &b, bool full_rank=false, const double &tol=1e-6)
Definition: vpLinProg.cpp:97
static bool simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.cpp:553
std::pair< unsigned int, double > BoundedIndex
Definition: vpLinProg.h:119
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
unsigned int qr(vpMatrix &Q, vpMatrix &R, bool full=false, bool squareR=false, double tol=1e-6) const
void stack(const vpMatrix &A)
vpMatrix inverseTriangular(bool upper=true) const
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:757
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:590
unsigned int qrPivot(vpMatrix &Q, vpMatrix &R, vpMatrix &P, bool full=false, bool squareR=false, double tol=1e-6) const
double infinityNorm() const
Definition: vpMatrix.cpp:1950
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:548
vpMatrix inverseByQR() const
vpMatrix transpose() const
vpMatrix t() const
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:424