Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
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 }
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
248bool 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)
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(x[i], 0.);
437 x[n + p + k1] = std::max(-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
543bool 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#endif
unsigned int getCols() const
Definition vpArray2D.h:280
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition vpArray2D.h:305
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
vpColVector extract(unsigned int r, unsigned int colsize) const
vpRowVector t() const
double infinityNorm() const
void resize(unsigned int i, bool flagNullify=true)
static bool allGreater(const vpColVector &x, const double &thr=1e-6)
Definition vpLinProg.h:218
static bool rowReduction(vpMatrix &A, vpColVector &b, const double &tol=1e-6)
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)
std::pair< unsigned int, double > BoundedIndex
Definition vpLinProg.h:117
static bool allZero(const vpColVector &x, const double &tol=1e-6)
Definition vpLinProg.h:147
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:184
static bool allClose(const vpMatrix &A, const vpColVector &x, const vpColVector &b, const double &tol=1e-6)
Definition vpLinProg.h:166
static bool simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
Implementation of a matrix and operations on matrices.
Definition vpMatrix.h:152
unsigned int qr(vpMatrix &Q, vpMatrix &R, bool full=false, bool squareR=false, double tol=1e-6) const
void eye()
Definition vpMatrix.cpp:446
void stack(const vpMatrix &A)
vpMatrix inverseTriangular(bool upper=true) const
static vpMatrix juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
vpRowVector getRow(unsigned int i) const
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:461
double infinityNorm() const
vpColVector getCol(unsigned int j) const
vpMatrix inverseByQR() const
vpMatrix transpose() const
Definition vpMatrix.cpp:468
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const
Definition vpMatrix.cpp:404