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);
249 std::cout <<
"vpQuadProg::solveQPe: wrong dimension\n"
255 if ((Q *
Z).infinityNorm() > tol)
256 x =
x1 +
Z * (Q *
Z).solveBySVD(r - Q *
x1);
317 std::cout <<
"vpQuadProg::solveQPe: equality constraint infeasible" << std::endl;
379 return solveQPi(Q, r, C, d, x,
false, tol);
384 std::cout <<
"vpQuadProg::solveQP: equality constraint infeasible" << std::endl;
388 if (A.
getCols() &&
solveQPi(Q * A, r - Q * b, C * A, d - C * b, x,
false, tol)) {
397 std::cout <<
"vpQuadProg::solveQP: inequality constraint infeasible" << std::endl;
447 vpColVector &x,
bool use_equality,
const double &tol)
449 unsigned int n =
checkDimensions(Q, r,
nullptr,
nullptr, &C, &d,
"solveQPi");
462 std::cout <<
"vpQuadProg::solveQPi: inequality constraint infeasible" << std::endl;
466 std::cout <<
"vpQuadProg::solveQPi: use_equality before setEqualityConstraint" << std::endl;
469 const unsigned int p = C.
getRows();
486 std::cout <<
"vpQuadProg::solveQPi: some constraints have been removed since last call\n";
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];
514 for (
unsigned int i = 0; i < p; ++i) {
520 for (
unsigned int i = 0; i < k; ++i)
521 c[2 * n + p + i] = 1;
527 for (
unsigned int i = 0; i < p; ++i) {
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];
534 A_lp[i][2 * n + i] = 1;
537 A_lp[i][2 * n + p + l] = -1;
538 xc[2 * n + p + l] = -e[i];
542 xc[2 * n + i] = e[i];
548 std::cout <<
"vpQuadProg::solveQPi: inequality constraints not feasible" << std::endl;
558 for (
unsigned int i = 0; i < p; ++i) {
559 if (C.
getRow(i) * x - d[i] < -tol)
570 for (
unsigned int i = 0; i < p; ++i) {
578 bool update_Ap =
true;
579 unsigned int last_active = C.
getRows();
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];
592 if (update_Ap &&
active.size())
596 std::cout <<
"vpQuadProg::solveQPi: QP seems infeasible, too many constraints activated\n";
603 unsigned int ineqInd = (
unsigned int)
active.size();
607 double ineqMax = -tol;
608 for (
unsigned int i = 0; i < mu.getRows(); ++i) {
609 if (mu[i] < ineqMax &&
active[i] != last_active) {
616 if (ineqInd ==
active.size())
629 unsigned int ineqInd = 0;
632 for (
unsigned int i = 0; i <
inactive.size(); ++i) {
683 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)