Visual Servoing Platform version 3.5.0
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
97bool 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
246bool 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)
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
532bool 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
unsigned int getCols() const
Definition: vpArray2D.h:279
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:220
vpRowVector t() const
double infinityNorm() const
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
static bool allGreater(const vpColVector &x, const double &thr=1e-6)
Definition: vpLinProg.h:230
static bool rowReduction(vpMatrix &A, vpColVector &b, const double &tol=1e-6)
Definition: vpLinProg.cpp:246
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
static bool allZero(const vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.h:155
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:194
static bool allClose(const vpMatrix &A, const vpColVector &x, const vpColVector &b, const double &tol=1e-6)
Definition: vpLinProg.h:175
static bool simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.cpp:532
std::pair< unsigned int, double > BoundedIndex
Definition: vpLinProg.h:122
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
unsigned int qr(vpMatrix &Q, vpMatrix &R, bool full=false, bool squareR=false, double tol=1e-6) const
void eye()
Definition: vpMatrix.cpp:449
void stack(const vpMatrix &A)
Definition: vpMatrix.cpp:5879
vpMatrix inverseTriangular(bool upper=true) const
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
Definition: vpMatrix.cpp:5531
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:5215
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:464
double infinityNorm() const
Definition: vpMatrix.cpp:6763
vpColVector getCol(unsigned int j) const
Definition: vpMatrix.cpp:5175
vpMatrix inverseByQR() const
vpMatrix transpose() const
Definition: vpMatrix.cpp:474
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition: vpMatrix.cpp:407