Visual Servoing Platform version 3.5.0
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
147bool 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
244bool 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
373bool 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
443bool 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
672void dummy_vpQuadProg(){};
673#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
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ functionNotImplementedError
Function not implemented.
Definition: vpException.h:90
@ dimensionError
Bad dimension.
Definition: vpException.h:95
static bool allGreater(const vpColVector &x, const double &thr=1e-6)
Definition: vpLinProg.h:230
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 simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
Definition: vpLinProg.cpp:532
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
vpColVector eigenValues() const
Definition: vpMatrix.cpp:6040
vpRowVector getRow(unsigned int i) const
Definition: vpMatrix.cpp:5215
vpMatrix t() const
Definition: vpMatrix.cpp:464
void solveBySVD(const vpColVector &B, vpColVector &x) const
Definition: vpMatrix.cpp:1908
void solveByQR(const vpColVector &b, vpColVector &x) const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Definition: vpMatrix.cpp:2241
vpMatrix transpose() const
Definition: vpMatrix.cpp:474
std::vector< unsigned int > inactive
Definition: vpQuadProg.h:120
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:116
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
vpMatrix Z
Definition: vpQuadProg.h:128
vpColVector x1
Definition: vpQuadProg.h:124
bool solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol=1e-6) const
Definition: vpQuadProg.cpp:244
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 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:664
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
static bool solveByProjection(const vpMatrix &Q, const vpColVector &r, vpMatrix &A, vpColVector &b, vpColVector &x, const double &tol=1e-6)
Definition: vpQuadProg.cpp:176