40#include <visp3/core/vpMatrixException.h>
41#include <visp3/core/vpQuadProg.h>
43#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
98#if defined(VISP_HAVE_LAPACK) || defined(VISP_HAVE_EIGEN3) || defined(VISP_HAVE_OPENCV)
102 "vpQuadProg::fromCanonicalCost: H is not square or not the same dimension as c"));
111 for (
unsigned int i = 0; i < n; ++i) {
122 D.diag(d.extract(k, n - k));
123 for (
unsigned int i = 0; i < n - k; ++i)
130 "should install Lapack, Eigen3 or OpenCV 3rd party"));
154 std::cout <<
"vpQuadProg::setEqualityConstraint: equality constraint infeasible" << std::endl;
183 if (A.
getCols() && (Q * A).infinityNorm() > tol)
184 x = b + A * (Q * A).solveBySVD(r - Q * b);
248 std::cout <<
"vpQuadProg::solveQPe: wrong dimension\n"
254 if ((Q *
Z).infinityNorm() > tol)
255 x =
x1 +
Z * (Q *
Z).solveBySVD(r - Q *
x1);
315 std::cout <<
"vpQuadProg::solveQPe: equality constraint infeasible" << std::endl;
377 return solveQPi(Q, r, C, d, x,
false, tol);
382 std::cout <<
"vpQuadProg::solveQP: equality constraint infeasible" << std::endl;
386 if (A.
getCols() &&
solveQPi(Q * A, r - Q * b, C * A, d - C * b, x,
false, tol)) {
394 std::cout <<
"vpQuadProg::solveQP: inequality constraint infeasible" << std::endl;
444 vpColVector &x,
bool use_equality,
const double &tol)
446 unsigned int n =
checkDimensions(Q, r,
nullptr,
nullptr, &C, &d,
"solveQPi");
458 std::cout <<
"vpQuadProg::solveQPi: inequality constraint infeasible" << std::endl;
461 std::cout <<
"vpQuadProg::solveQPi: use_equality before setEqualityConstraint" << std::endl;
464 const unsigned int p = C.
getRows();
481 std::cout <<
"vpQuadProg::solveQPi: some constraints have been removed since last call\n";
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];
509 for (
unsigned int i = 0; i < p; ++i) {
515 for (
unsigned int i = 0; i < k; ++i)
516 c[2 * n + p + i] = 1;
522 for (
unsigned int i = 0; i < p; ++i) {
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];
529 A_lp[i][2 * n + i] = 1;
532 A_lp[i][2 * n + p + l] = -1;
533 xc[2 * n + p + l] = -e[i];
536 xc[2 * n + i] = e[i];
542 std::cout <<
"vpQuadProg::solveQPi: inequality constraints not feasible" << std::endl;
552 for (
unsigned int i = 0; i < p; ++i) {
553 if (C.
getRow(i) * x - d[i] < -tol)
563 for (
unsigned int i = 0; i < p; ++i) {
571 bool update_Ap =
true;
572 unsigned int last_active = C.
getRows();
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];
585 if (update_Ap &&
active.size())
589 std::cout <<
"vpQuadProg::solveQPi: QP seems infeasible, too many constraints activated\n";
596 unsigned int ineqInd = (
unsigned int)
active.
size();
600 double ineqMax = -tol;
601 for (
unsigned int i = 0; i < mu.getRows(); ++i) {
602 if (mu[i] < ineqMax &&
active[i] != last_active) {
609 if (ineqInd ==
active.size())
621 unsigned int ineqInd = 0;
624 for (
unsigned int i = 0; i <
inactive.size(); ++i) {
672void dummy_vpQuadProg(){};
unsigned int getCols() const
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int size() const
Return the number of elements of the 2D array.
unsigned int getRows() const
Implementation of column vector and the associated operations.
vpColVector extract(unsigned int r, unsigned int colsize) const
void insert(unsigned int i, const vpColVector &v)
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
@ functionNotImplementedError
Function not implemented.
@ dimensionError
Bad dimension.
static bool allGreater(const vpColVector &x, const double &thr=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 simplex(const vpColVector &c, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
@ matrixError
Matrix operation error.
Implementation of a matrix and operations on matrices.
vpColVector eigenValues() const
vpRowVector getRow(unsigned int i) const
void solveBySVD(const vpColVector &B, vpColVector &x) const
void solveByQR(const vpColVector &b, vpColVector &x) const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpMatrix transpose() const
std::vector< unsigned int > inactive
static void fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r, const double &tol=1e-6)
std::vector< unsigned int > active
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)
bool solveQPe(const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol=1e-6) const
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)
bool setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol=1e-6)
static vpColVector solveSVDorQR(const vpMatrix &A, const vpColVector &b)
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)
static bool solveByProjection(const vpMatrix &Q, const vpColVector &r, vpMatrix &A, vpColVector &b, vpColVector &x, const double &tol=1e-6)