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) {
672 void dummy_vpQuadProg(){};
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
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)