Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpQuadProg.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  * 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  }
188  else
189  x = Q.solveBySVD(r);
190  return true;
191 }
192 
245 bool vpQuadProg::solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol) const
246 {
247  unsigned int n = Q.getCols();
248  if (Q.getRows() != r.getRows() || Z.getRows() != n || x1.getRows() != n) {
249  std::cout << "vpQuadProg::solveQPe: wrong dimension\n"
250  << "Q: " << Q.getRows() << "x" << Q.getCols() << " - r: " << r.getRows() << std::endl;
251  std::cout << "Z: " << Z.getRows() << "x" << Z.getCols() << " - x1: " << x1.getRows() << std::endl;
253  }
254  if (Z.getCols()) {
255  if ((Q * Z).infinityNorm() > tol)
256  x = x1 + Z * (Q * Z).solveBySVD(r - Q * x1);
257  else
258  x = x1;
259  }
260  else
261  x = Q.solveBySVD(r);
262  return true;
263 }
264 
312  const double &tol)
313 {
314  checkDimensions(Q, r, &A, &b, nullptr, nullptr, "solveQPe");
315 
316  if (!solveByProjection(Q, r, A, b, x, tol)) {
317  std::cout << "vpQuadProg::solveQPe: equality constraint infeasible" << std::endl;
318  return false;
319  }
320  return true;
321 }
322 
375 bool vpQuadProg::solveQP(const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, const vpMatrix &C,
376  const vpColVector &d, vpColVector &x, const double &tol)
377 {
378  if (A.getRows() == 0)
379  return solveQPi(Q, r, C, d, x, false, tol);
380 
381  checkDimensions(Q, r, &A, &b, &C, &d, "solveQP");
382 
383  if (!vpLinProg::colReduction(A, b, false, tol)) {
384  std::cout << "vpQuadProg::solveQP: equality constraint infeasible" << std::endl;
385  return false;
386  }
387 
388  if (A.getCols() && solveQPi(Q * A, r - Q * b, C * A, d - C * b, x, false, tol)) {
389  x = b + A * x;
390  return true;
391  }
392  else if (vpLinProg::allLesser(C, b, d, tol)) // Ax = b has only 1 solution
393  {
394  x = b;
395  return true;
396  }
397  std::cout << "vpQuadProg::solveQP: inequality constraint infeasible" << std::endl;
398  return false;
399 }
400 
446 bool vpQuadProg::solveQPi(const vpMatrix &Q, const vpColVector &r, const vpMatrix &C, const vpColVector &d,
447  vpColVector &x, bool use_equality, const double &tol)
448 {
449  unsigned int n = checkDimensions(Q, r, nullptr, nullptr, &C, &d, "solveQPi");
450 
451  if (use_equality) {
452  if (Z.getRows() == n) {
453  if (Z.getCols() && solveQPi(Q * Z, r - Q * x1, C * Z, d - C * x1, x, false, tol)) {
454  // back to initial solution
455  x = x1 + Z * x;
456  return true;
457  }
458  else if (vpLinProg::allLesser(C, x1, d, tol)) {
459  x = x1;
460  return true;
461  }
462  std::cout << "vpQuadProg::solveQPi: inequality constraint infeasible" << std::endl;
463  return false;
464  }
465  else
466  std::cout << "vpQuadProg::solveQPi: use_equality before setEqualityConstraint" << std::endl;
467  }
468 
469  const unsigned int p = C.getRows();
470 
471  // look for trivial solution
472  // r = 0 and d > 0 -> x = 0
473  if (vpLinProg::allZero(r, tol) && (d.getRows() == 0 || vpLinProg::allGreater(d, -tol))) {
474  x.resize(n);
475  return true;
476  }
477 
478  // go for solver
479  // build feasible point
480  vpMatrix A;
481  vpColVector b;
482  // check active set - all values should be < rows of C
483  for (auto v : active) {
484  if (v >= p) {
485  active.clear();
486  std::cout << "vpQuadProg::solveQPi: some constraints have been removed since last call\n";
487  break;
488  }
489  }
490 
491  // warm start from previous active set
492  A.resize((unsigned int)active.size(), n);
493  b.resize((unsigned int)active.size());
494  for (unsigned int i = 0; i < active.size(); ++i) {
495  for (unsigned int j = 0; j < n; ++j)
496  A[i][j] = C[active[i]][j];
497  b[i] = d[active[i]];
498  }
499  if (!solveByProjection(Q, r, A, b, x, tol))
500  x.resize(n);
501 
502  // or from simplex if we really have no clue
503  if (!vpLinProg::allLesser(C, x, d, tol)) {
504  // feasible point with simplex:
505  // min r
506  // st C.(x + z1 - z2) + y - r = d
507  // st z1, z2, y, r >= 0
508  // dim r = violated constraints
509  // feasible if r can be minimized to 0
510 
511  // count how many violated constraints
512  vpColVector e = d - C * x;
513  unsigned int k = 0;
514  for (unsigned int i = 0; i < p; ++i) {
515  if (e[i] < -tol)
516  k++;
517  }
518  // cost vector
519  vpColVector c(2 * n + p + k);
520  for (unsigned int i = 0; i < k; ++i)
521  c[2 * n + p + i] = 1;
522 
523  vpColVector xc(2 * n + p + k);
524 
525  vpMatrix A_lp(p, 2 * n + p + k);
526  unsigned int l = 0;
527  for (unsigned int i = 0; i < p; ++i) {
528  // copy [C -C] part
529  for (unsigned int j = 0; j < n; ++j) {
530  A_lp[i][j] = C[i][j];
531  A_lp[i][n + j] = -C[i][j];
532  }
533  // y-part
534  A_lp[i][2 * n + i] = 1;
535  if (e[i] < -tol) {
536  // r-part
537  A_lp[i][2 * n + p + l] = -1;
538  xc[2 * n + p + l] = -e[i];
539  l++;
540  }
541  else
542  xc[2 * n + i] = e[i];
543  }
544  vpLinProg::simplex(c, A_lp, e, xc);
545 
546  // r-part should be 0
547  if (!vpLinProg::allLesser(xc.extract(2 * n + p, k), tol)) {
548  std::cout << "vpQuadProg::solveQPi: inequality constraints not feasible" << std::endl;
549  return false;
550  }
551 
552  // update x to feasible point
553  x += xc.extract(0, n) - xc.extract(n, n);
554  // init active/inactive sets from y-part of x
555  active.clear();
556  active.reserve(p);
557  inactive.clear();
558  for (unsigned int i = 0; i < p; ++i) {
559  if (C.getRow(i) * x - d[i] < -tol)
560  inactive.push_back(i);
561  else
562  active.push_back(i);
563  }
564  }
565  else // warm start feasible
566  {
567  // using previous active set, check that inactive is sync
568  if (active.size() + inactive.size() != p) {
569  inactive.clear();
570  for (unsigned int i = 0; i < p; ++i) {
571  if (std::find(active.begin(), active.end(), i) == active.end())
572  inactive.push_back(i);
573  }
574  }
575  }
576 
577  vpMatrix Ap;
578  bool update_Ap = true;
579  unsigned int last_active = C.getRows();
580 
581  vpColVector u, g = r - Q * x, mu;
582 
583  // solve at one iteration
584  while (true) {
585  A.resize((unsigned int)active.size(), n);
586  b.resize((unsigned int)active.size());
587  for (unsigned int i = 0; i < active.size(); ++i) {
588  for (unsigned int j = 0; j < n; ++j)
589  A[i][j] = C[active[i]][j];
590  }
591 
592  if (update_Ap && active.size())
593  Ap = A.pseudoInverse(); // to get Lagrange multipliers if needed
594 
595  if (!solveByProjection(Q, g, A, b, u, tol)) {
596  std::cout << "vpQuadProg::solveQPi: QP seems infeasible, too many constraints activated\n";
597  return false;
598  }
599 
600  // 0-update = optimal or useless activated constraints
601  if (vpLinProg::allZero(u, tol)) {
602  // compute multipliers if any
603  unsigned int ineqInd = (unsigned int)active.size();
604  if (active.size()) {
605  mu = -Ap.transpose() * Q.transpose() * (Q * u - g);
606  // find most negative one if any - except last activated in case of degeneracy
607  double ineqMax = -tol;
608  for (unsigned int i = 0; i < mu.getRows(); ++i) {
609  if (mu[i] < ineqMax && active[i] != last_active) {
610  ineqInd = i;
611  ineqMax = mu[i];
612  }
613  }
614  }
615 
616  if (ineqInd == active.size()) // KKT condition no useless constraint
617  return true;
618 
619  // useless inequality, deactivate
620  inactive.push_back(active[ineqInd]);
621  if (active.size() == 1)
622  active.clear();
623  else
624  active.erase(active.begin() + ineqInd);
625  update_Ap = true;
626  }
627  else // u != 0, can improve xc
628  {
629  unsigned int ineqInd = 0;
630  // step length to next constraint
631  double alpha = 1;
632  for (unsigned int i = 0; i < inactive.size(); ++i) {
633  const double Cu = C.getRow(inactive[i]) * u;
634  if (Cu > tol) {
635  const double a = (d[inactive[i]] - C.getRow(inactive[i]) * x) / Cu;
636  if (a < alpha) {
637  alpha = a;
638  ineqInd = i;
639  }
640  }
641  }
642  if (alpha < 1) {
643  last_active = inactive[ineqInd];
644  if (active.size()) {
645  auto it = active.begin();
646  while (it != active.end() && *it < inactive[ineqInd])
647  it++;
648  active.insert(it, inactive[ineqInd]);
649  }
650  else
651  active.push_back(inactive[ineqInd]);
652  inactive.erase(inactive.begin() + ineqInd);
653  update_Ap = true;
654  }
655  else
656  update_Ap = false;
657  // update x for next iteration
658  x += alpha * u;
659  g -= alpha * Q * u;
660  }
661  }
662 }
663 
675 {
676  // assume A is full rank
677  if (A.getRows() > A.getCols())
678  return A.solveBySVD(b);
679  return A.solveByQR(b);
680 }
681 
682 #else
683 void dummy_vpQuadProg() { };
684 #endif
unsigned int getCols() const
Definition: vpArray2D.h:274
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Definition: vpArray2D.h:299
unsigned int getRows() const
Definition: vpArray2D.h:284
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
vpColVector extract(unsigned int r, unsigned int colsize) const
Definition: vpColVector.h:367
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:1056
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:78
@ dimensionError
Bad dimension.
Definition: vpException.h:83
static bool allGreater(const vpColVector &x, const double &thr=1e-6)
Definition: vpLinProg.h:215
static bool allZero(const vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.h:144
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:181
static bool simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.cpp:543
@ matrixError
Matrix operation error.
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6031
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:5219
vpMatrix t() const
Definition: vpMatrix.cpp:457
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1953
void solveByQR(const vpColVector &b, vpColVector &x) const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2286
vpMatrix transpose() const
Definition: vpMatrix.cpp:464
std::vector< unsigned int > inactive
Definition: vpQuadProg.h:108
static void fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r, const double &tol=1e-6)
Definition: vpQuadProg.cpp:95
std::vector< unsigned int > active
Definition: vpQuadProg.h:103
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:375
vpMatrix Z
Definition: vpQuadProg.h:118
vpColVector x1
Definition: vpQuadProg.h:113
bool solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol=1e-6) const
Definition: vpQuadProg.cpp:245
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:140
bool setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol=1e-6)
Definition: vpQuadProg.cpp:147
static vpColVector solveSVDorQR(const vpMatrix &A, const vpColVector &b)
Definition: vpQuadProg.cpp:674
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:446
static bool solveByProjection(const vpMatrix &Q, const vpColVector &r, vpMatrix &A, vpColVector &b, vpColVector &x, const double &tol=1e-6)
Definition: vpQuadProg.cpp:176