Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpQuadProg.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  * Quadratic Programming
33  *
34  * Authors:
35  * Olivier Kermorgant
36  *
37  *****************************************************************************/
38 
39 #include <algorithm>
40 #include <visp3/core/vpMatrixException.h>
41 #include <visp3/core/vpQuadProg.h>
42 
43 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
44 
96  const double &tol)
97 {
98 #if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)
99  unsigned int n = H.getCols();
100  if (H.getRows() != n || c.getRows() != n) {
102  "vpQuadProg::fromCanonicalCost: H is not square or not the same dimension as c"));
103  }
104 
105  vpColVector d(n);
106  vpMatrix V(n, n);
107  // Compute the eigenvalues and eigenvectors
108  H.eigenValues(d, V);
109  // find first non-nullptr eigen value
110  unsigned int k = 0;
111  for (unsigned int i = 0; i < n; ++i) {
112  if (d[i] > tol)
113  d[i] = sqrt(d[i]);
114  else if (d[i] < tol)
115  throw(vpException(vpMatrixException::matrixError, "vpQuadProg::fromCanonicalCost: H is not positive"));
116  else
117  k = i + 1;
118  }
119  // build (Q,r) such that H = Q.^T.Q and c = -Q^T.r
120  vpMatrix D(n - k, n - k);
121  vpMatrix P(n - k, n);
122  D.diag(d.extract(k, n - k));
123  for (unsigned int i = 0; i < n - k; ++i)
124  P[i][k + i] = 1;
125 
126  Q = D * P * V.transpose();
127  r = -Q.t().pseudoInverse() * c;
128 #else
129  throw(vpException(vpException::functionNotImplementedError, "Symmetric matrix decomposition is not implemented. You "
130  "should install Lapack, Eigen3 or OpenCV 3rd party"));
131 #endif
132 }
133 
147 bool vpQuadProg::setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol)
148 {
149  x1 = b;
150  Z = A;
151  if (A.getRows() == b.getRows() && vpLinProg::colReduction(Z, x1, false, tol))
152  return true;
153 
154  std::cout << "vpQuadProg::setEqualityConstraint: equality constraint infeasible" << std::endl;
155  return false;
156 }
157 
177  const double &tol)
178 {
179  if (A.getRows()) {
180  if (!vpLinProg::colReduction(A, b, false, tol))
181  return false;
182 
183  if (A.getCols() && (Q * A).infinityNorm() > tol)
184  x = b + A * (Q * A).solveBySVD(r - Q * b);
185  else
186  x = b;
187  } else
188  x = Q.solveBySVD(r);
189  return true;
190 }
191 
244 bool vpQuadProg::solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol) const
245 {
246  unsigned int n = Q.getCols();
247  if (Q.getRows() != r.getRows() || Z.getRows() != n || x1.getRows() != n) {
248  std::cout << "vpQuadProg::solveQPe: wrong dimension\n"
249  << "Q: " << Q.getRows() << "x" << Q.getCols() << " - r: " << r.getRows() << std::endl;
250  std::cout << "Z: " << Z.getRows() << "x" << Z.getCols() << " - x1: " << x1.getRows() << std::endl;
252  }
253  if (Z.getCols()) {
254  if ((Q * Z).infinityNorm() > tol)
255  x = x1 + Z * (Q * Z).solveBySVD(r - Q * x1);
256  else
257  x = x1;
258  } else
259  x = Q.solveBySVD(r);
260  return true;
261 }
262 
310  const double &tol)
311 {
312  checkDimensions(Q, r, &A, &b, nullptr, nullptr, "solveQPe");
313 
314  if (!solveByProjection(Q, r, A, b, x, tol)) {
315  std::cout << "vpQuadProg::solveQPe: equality constraint infeasible" << std::endl;
316  return false;
317  }
318  return true;
319 }
320 
373 bool vpQuadProg::solveQP(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, const vpMatrix &C,
374  const vpColVector &d, vpColVector &x, const double &tol)
375 {
376  if (A.getRows() == 0)
377  return solveQPi(Q, r, C, d, x, false, tol);
378 
379  checkDimensions(Q, r, &A, &b, &C, &d, "solveQP");
380 
381  if (!vpLinProg::colReduction(A, b, false, tol)) {
382  std::cout << "vpQuadProg::solveQP: equality constraint infeasible" << std::endl;
383  return false;
384  }
385 
386  if (A.getCols() && solveQPi(Q * A, r - Q * b, C * A, d - C * b, x, false, tol)) {
387  x = b + A * x;
388  return true;
389  } else if (vpLinProg::allLesser(C, b, d, tol)) // Ax = b has only 1 solution
390  {
391  x = b;
392  return true;
393  }
394  std::cout << "vpQuadProg::solveQP: inequality constraint infeasible" << std::endl;
395  return false;
396 }
397 
443 bool vpQuadProg::solveQPi(const vpMatrix &Q, const vpColVector &r, const vpMatrix &C, const vpColVector &d,
444  vpColVector &x, bool use_equality, const double &tol)
445 {
446  unsigned int n = checkDimensions(Q, r, nullptr, nullptr, &C, &d, "solveQPi");
447 
448  if (use_equality) {
449  if (Z.getRows() == n) {
450  if (Z.getCols() && solveQPi(Q * Z, r - Q * x1, C * Z, d - C * x1, x, false, tol)) {
451  // back to initial solution
452  x = x1 + Z * x;
453  return true;
454  } else if (vpLinProg::allLesser(C, x1, d, tol)) {
455  x = x1;
456  return true;
457  }
458  std::cout << "vpQuadProg::solveQPi: inequality constraint infeasible" << std::endl;
459  return false;
460  } else
461  std::cout << "vpQuadProg::solveQPi: use_equality before setEqualityConstraint" << std::endl;
462  }
463 
464  const unsigned int p = C.getRows();
465 
466  // look for trivial solution
467  // r = 0 and d > 0 -> x = 0
468  if (vpLinProg::allZero(r, tol) && (d.getRows() == 0 || vpLinProg::allGreater(d, -tol))) {
469  x.resize(n);
470  return true;
471  }
472 
473  // go for solver
474  // build feasible point
475  vpMatrix A;
476  vpColVector b;
477  // check active set - all values should be < rows of C
478  for (auto v : active) {
479  if (v >= p) {
480  active.clear();
481  std::cout << "vpQuadProg::solveQPi: some constraints have been removed since last call\n";
482  break;
483  }
484  }
485 
486  // warm start from previous active set
487  A.resize((unsigned int)active.size(), n);
488  b.resize((unsigned int)active.size());
489  for (unsigned int i = 0; i < active.size(); ++i) {
490  for (unsigned int j = 0; j < n; ++j)
491  A[i][j] = C[active[i]][j];
492  b[i] = d[active[i]];
493  }
494  if (!solveByProjection(Q, r, A, b, x, tol))
495  x.resize(n);
496 
497  // or from simplex if we really have no clue
498  if (!vpLinProg::allLesser(C, x, d, tol)) {
499  // feasible point with simplex:
500  // min r
501  // st C.(x + z1 - z2) + y - r = d
502  // st z1, z2, y, r >= 0
503  // dim r = violated constraints
504  // feasible if r can be minimized to 0
505 
506  // count how many violated constraints
507  vpColVector e = d - C * x;
508  unsigned int k = 0;
509  for (unsigned int i = 0; i < p; ++i) {
510  if (e[i] < -tol)
511  k++;
512  }
513  // cost vector
514  vpColVector c(2 * n + p + k);
515  for (unsigned int i = 0; i < k; ++i)
516  c[2 * n + p + i] = 1;
517 
518  vpColVector xc(2 * n + p + k);
519 
520  vpMatrix A_lp(p, 2 * n + p + k);
521  unsigned int l = 0;
522  for (unsigned int i = 0; i < p; ++i) {
523  // copy [C -C] part
524  for (unsigned int j = 0; j < n; ++j) {
525  A_lp[i][j] = C[i][j];
526  A_lp[i][n + j] = -C[i][j];
527  }
528  // y-part
529  A_lp[i][2 * n + i] = 1;
530  if (e[i] < -tol) {
531  // r-part
532  A_lp[i][2 * n + p + l] = -1;
533  xc[2 * n + p + l] = -e[i];
534  l++;
535  } else
536  xc[2 * n + i] = e[i];
537  }
538  vpLinProg::simplex(c, A_lp, e, xc);
539 
540  // r-part should be 0
541  if (!vpLinProg::allLesser(xc.extract(2 * n + p, k), tol)) {
542  std::cout << "vpQuadProg::solveQPi: inequality constraints not feasible" << std::endl;
543  return false;
544  }
545 
546  // update x to feasible point
547  x += xc.extract(0, n) - xc.extract(n, n);
548  // init active/inactive sets from y-part of x
549  active.clear();
550  active.reserve(p);
551  inactive.clear();
552  for (unsigned int i = 0; i < p; ++i) {
553  if (C.getRow(i) * x - d[i] < -tol)
554  inactive.push_back(i);
555  else
556  active.push_back(i);
557  }
558  } else // warm start feasible
559  {
560  // using previous active set, check that inactive is sync
561  if (active.size() + inactive.size() != p) {
562  inactive.clear();
563  for (unsigned int i = 0; i < p; ++i) {
564  if (std::find(active.begin(), active.end(), i) == active.end())
565  inactive.push_back(i);
566  }
567  }
568  }
569 
570  vpMatrix Ap;
571  bool update_Ap = true;
572  unsigned int last_active = C.getRows();
573 
574  vpColVector u, g = r - Q * x, mu;
575 
576  // solve at one iteration
577  while (true) {
578  A.resize((unsigned int)active.size(), n);
579  b.resize((unsigned int)active.size());
580  for (unsigned int i = 0; i < active.size(); ++i) {
581  for (unsigned int j = 0; j < n; ++j)
582  A[i][j] = C[active[i]][j];
583  }
584 
585  if (update_Ap && active.size())
586  Ap = A.pseudoInverse(); // to get Lagrange multipliers if needed
587 
588  if (!solveByProjection(Q, g, A, b, u, tol)) {
589  std::cout << "vpQuadProg::solveQPi: QP seems infeasible, too many constraints activated\n";
590  return false;
591  }
592 
593  // 0-update = optimal or useless activated constraints
594  if (vpLinProg::allZero(u, tol)) {
595  // compute multipliers if any
596  unsigned int ineqInd = (unsigned int)active.size();
597  if (active.size()) {
598  mu = -Ap.transpose() * Q.transpose() * (Q * u - g);
599  // find most negative one if any - except last activated in case of degeneracy
600  double ineqMax = -tol;
601  for (unsigned int i = 0; i < mu.getRows(); ++i) {
602  if (mu[i] < ineqMax && active[i] != last_active) {
603  ineqInd = i;
604  ineqMax = mu[i];
605  }
606  }
607  }
608 
609  if (ineqInd == active.size()) // KKT condition no useless constraint
610  return true;
611 
612  // useless inequality, deactivate
613  inactive.push_back(active[ineqInd]);
614  if (active.size() == 1)
615  active.clear();
616  else
617  active.erase(active.begin() + ineqInd);
618  update_Ap = true;
619  } else // u != 0, can improve xc
620  {
621  unsigned int ineqInd = 0;
622  // step length to next constraint
623  double alpha = 1;
624  for (unsigned int i = 0; i < inactive.size(); ++i) {
625  const double Cu = C.getRow(inactive[i]) * u;
626  if (Cu > tol) {
627  const double a = (d[inactive[i]] - C.getRow(inactive[i]) * x) / Cu;
628  if (a < alpha) {
629  alpha = a;
630  ineqInd = i;
631  }
632  }
633  }
634  if (alpha < 1) {
635  last_active = inactive[ineqInd];
636  if (active.size()) {
637  auto it = active.begin();
638  while (it != active.end() && *it < inactive[ineqInd])
639  it++;
640  active.insert(it, inactive[ineqInd]);
641  } else
642  active.push_back(inactive[ineqInd]);
643  inactive.erase(inactive.begin() + ineqInd);
644  update_Ap = true;
645  } else
646  update_Ap = false;
647  // update x for next iteration
648  x += alpha * u;
649  g -= alpha * Q * u;
650  }
651  }
652 }
653 
665 {
666  // assume A is full rank
667  if (A.getRows() > A.getCols())
668  return A.solveBySVD(b);
669  return A.solveByQR(b);
670 }
671 #else
672 void dummy_vpQuadProg(){};
673 #endif
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1908
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:153
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
static bool colReduction(vpMatrix &A, vpColVector &b, bool full_rank=false, const double &tol=1e-6)
Definition: vpLinProg.cpp:97
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:304
vpColVector x1
Definition: vpQuadProg.h:124
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:220
static bool solveByProjection(const vpMatrix &Q, const vpColVector &r, vpMatrix &A, vpColVector &b, vpColVector &x, const double &tol=1e-6)
Definition: vpQuadProg.cpp:176
error that can be emited by ViSP classes.
Definition: vpException.h:71
bool solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol=1e-6) const
Definition: vpQuadProg.cpp:244
unsigned int getRows() const
Definition: vpArray2D.h:289
static bool allGreater(const vpColVector &x, const double &thr=1e-6)
Definition: vpLinProg.h:230
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
static void fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r, const double &tol=1e-6)
Definition: vpQuadProg.cpp:95
static vpColVector solveSVDorQR(const vpMatrix &A, const vpColVector &b)
Definition: vpQuadProg.cpp:664
unsigned int getCols() const
Definition: vpArray2D.h:279
static bool simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.cpp:532
vpMatrix t() const
Definition: vpMatrix.cpp:464
std::vector< unsigned int > inactive
Definition: vpQuadProg.h:120
void diag(const double &val=1.0)
Definition: vpMatrix.cpp:887
void clear()
Definition: vpColVector.h:175
void solveByQR(const vpColVector &b, vpColVector &x) const
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:5215
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6040
vpMatrix transpose() const
Definition: vpMatrix.cpp:474
bool setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol=1e-6)
Definition: vpQuadProg.cpp:147
std::vector< unsigned int > active
Definition: vpQuadProg.h:116
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
static bool allLesser(const vpMatrix &C, const vpColVector &x, const vpColVector &d, const double &thr=1e-6)
Definition: vpLinProg.h:194
vpMatrix Z
Definition: vpQuadProg.h:128
bool solveQPi(const vpMatrix &Q, const vpColVector &r, const vpMatrix &C, const vpColVector &d, vpColVector &x, bool use_equality=false, const double &tol=1e-6)
Definition: vpQuadProg.cpp:443
Function not implemented.
Definition: vpException.h:90
static bool allZero(const vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.h:155
static unsigned int checkDimensions(const vpMatrix &Q, const vpColVector &r, const vpMatrix *A, const vpColVector *b, const vpMatrix *C, const vpColVector *d, const std::string fct)
Definition: vpQuadProg.h:151
bool solveQP(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, const vpMatrix &C, const vpColVector &d, vpColVector &x, const double &tol=1e-6)
Definition: vpQuadProg.cpp:373