39 #include <visp3/core/vpLinProg.h>
137 for (
unsigned int j = 0; j < n; ++j)
140 A = IQQt.
extract(0, 0, n, n - m);
141 if (A.
qr(Q, R,
false,
false, tol) != n - m) {
144 for (j0 = 0; j0 < n; ++j0) {
151 unsigned int j = j0 + 1;
156 if (A.
qr(Q, R,
false,
false, tol) != A.
getCols())
180 for (
unsigned int j = 0; j < n; ++j)
183 A = IQQt.
extract(0, 0, n, n - r);
184 if (A.
qr(Q, R,
false,
false, tol) != n - r) {
187 for (j0 = 0; j0 < n; ++j0) {
194 unsigned int j = j0 + 1;
199 if (A.
qr(Q, R,
false,
false, tol) != A.
getCols())
265 const unsigned int r = A.
qrPivot(Q, R, P,
false,
false, tol);
280 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
343 vpColVector &x, std::vector<BoundedIndex> l, std::vector<BoundedIndex> u,
const double &tol)
347 const unsigned int p = C.
getRows();
350 const bool feasible =
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());
357 if (!feasible && m && l.size() == 0 && u.size() == 0) {
371 std::cout <<
"vpLinProg::simplex: equality constraints not feasible" << std::endl;
376 unsigned int s1 = 0, s2 = 0;
377 for (
unsigned int i = 0; i < n; ++i) {
379 return bi.first == i;
382 const bool has_low = find_if(l.begin(), l.end(), cmp) != l.end();
384 const bool has_up = find_if(u.begin(), u.end(), cmp) != u.end();
385 if (has_low == has_up) {
393 A.
resize(m + p + s2, n + p + s1,
false);
396 x.
resize(n + p + s1,
false);
400 for (
unsigned int i = 0; i < p; ++i) {
403 for (
unsigned int j = 0; j < n; ++j)
404 A[m + i][j] = C[i][j];
410 P.
eye(n, n + p + s1);
416 unsigned int k1 = 0, k2 = 0;
417 for (
unsigned int i = 0; i < n; ++i) {
420 return bi.first == i;
424 const auto low = find_if(l.begin(), l.end(), cmp);
426 const auto up = find_if(u.begin(), u.end(), cmp);
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];
436 x[i] = std::max<double>(x[i], 0.);
437 x[n + p + k1] = std::max<double>(-x[i], 0.);
445 for (
unsigned int j = 0; j < m + p; ++j)
448 x[i] = up->second - x[i];
454 z0[i] = -low->second;
457 A[m + p + k2][i] = A[m + p + k2][n + p + k1] = 1;
458 b[m + p + k2] = up->second - low->second;
460 x[i] = up->second - x[i];
461 x[n + p + k1] = x[i] - low->second;
468 x[i] = x[i] - low->second;
550 unsigned int non0 = 0;
551 for (
unsigned int i = 0; i < n; ++i) {
565 for (
unsigned int i = 0; i < m; ++i) {
578 for (
unsigned int j = 0; j < n; ++j)
585 std::cout <<
"vpLinProg::simplex: constraints not feasible" << std::endl;
596 std::cout <<
"vpLinProg::simplex: equality constraint not feasible" << std::endl;
607 vpMatrix Ab(m, m), An(m, n - m), Abi;
609 std::vector<unsigned int> B, N;
611 for (
unsigned int i = 0; i < n; ++i) {
612 if (std::abs(x[i]) > tol)
617 std::vector<unsigned int> null_rows;
618 for (
unsigned int i = 0; i < m; ++i) {
620 for (
unsigned int j = 0; j < B.size(); ++j) {
621 if (std::abs(A[i][B[j]]) > tol) {
627 null_rows.push_back(i);
632 for (
unsigned int j = n; j-- > 0;) {
633 if (std::abs(x[j]) < tol) {
637 for (
unsigned int i = 0; i < null_rows.size(); ++i) {
639 if (std::abs(A[null_rows[i]][j]) > tol) {
640 null_rows.erase(null_rows.begin() + i);
646 if (!in_N && null_rows.size()) {
650 for (
unsigned int i = 0; i < null_rows.size(); ++i) {
651 if (std::abs(A[null_rows[i]][j]) > tol) {
653 null_rows.erase(null_rows.begin() + i);
667 for (
unsigned int j = 0; j < m; ++j) {
669 for (
unsigned int i = 0; i < m; ++i)
670 Ab[i][j] = A[i][B[j]];
672 for (
unsigned int j = 0; j < n - m; ++j) {
674 for (
unsigned int i = 0; i < m; ++i)
675 An[i][j] = A[i][N[j]];
678 std::vector<std::pair<double, unsigned int> > a;
686 R = cn - An.
t() * Abi.
t() * cb;
688 for (j = 0; j < N.size(); ++j) {
705 for (
unsigned int k = 0; k < m; ++k) {
707 a.push_back({ -x[B[k]] / db[k], k });
710 const auto amin = std::min_element(a.begin(), a.end());
711 const double alpha = amin->first;
712 const unsigned int k = amin->second;
716 for (
unsigned int i = 0; i < B.size(); ++i)
717 x[B[i]] += alpha * db[i];
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]);
727 std::swap(B[k], N[j]);
unsigned int getCols() const
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int getRows() const
Implementation of column vector and the associated operations.
vpColVector extract(unsigned int r, unsigned int colsize) const
double infinityNorm() const
void resize(unsigned int i, bool flagNullify=true)
static bool allGreater(const vpColVector &x, const double &thr=1e-6)
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)
static bool allZero(const vpColVector &x, const double &tol=1e-6)
static bool colReduction(vpMatrix &A, vpColVector &b, bool full_rank=false, const double &tol=1e-6)
static bool allLesser(const vpMatrix &C, const vpColVector &x, const vpColVector &d, const double &thr=1e-6)
static bool allClose(const vpMatrix &A, const vpColVector &x, const vpColVector &b, const double &tol=1e-6)
static bool simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
std::pair< unsigned int, double > BoundedIndex
Implementation of a matrix and operations on matrices.
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)
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
double infinityNorm() const
vpColVector getCol(unsigned int j) const
vpMatrix inverseByQR() const
vpMatrix transpose() const
vpMatrix extract(unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols) const