Visual Servoing Platform version 3.5.0
vpQuadProg.h
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#ifndef vpQuadProgh
40#define vpQuadProgh
41
42#include <vector>
43#include <stdlib.h>
44#include <visp3/core/vpConfig.h>
45#include <visp3/core/vpMatrix.h>
46#include <visp3/core/vpMatrixException.h>
47#include <visp3/core/vpLinProg.h>
48
74class VISP_EXPORT vpQuadProg
75{
76public:
77#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
80 bool solveQPe(const vpMatrix &Q, const vpColVector &r,
81 vpColVector &x, const double &tol = 1e-6) const;
82
83 bool solveQPi(const vpMatrix &Q, const vpColVector &r,
84 const vpMatrix &C, const vpColVector &d,
85 vpColVector &x,
86 bool use_equality = false,
87 const double &tol = 1e-6);
88
89 bool solveQP(const vpMatrix &Q, const vpColVector &r,
91 const vpMatrix &C, const vpColVector &d,
92 vpColVector &x, const double &tol = 1e-6);
94
97 bool setEqualityConstraint(const vpMatrix &A, const vpColVector &b, const double &tol = 1e-6);
102 {
103 active.clear();
104 }
106
107 static void fromCanonicalCost(const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r, const double &tol = 1e-6);
108 static bool solveQPe(const vpMatrix &Q, const vpColVector &r,
110 vpColVector &x, const double &tol = 1e-6);
111
112protected:
116 std::vector<unsigned int> active;
120 std::vector<unsigned int> inactive;
129
130 static vpColVector solveSVDorQR(const vpMatrix &A, const vpColVector &b);
131
132 static bool solveByProjection(const vpMatrix &Q, const vpColVector &r,
133 vpMatrix &A, vpColVector &b,
134 vpColVector &x, const double &tol = 1e-6);
135
151 static unsigned int checkDimensions(const vpMatrix &Q, const vpColVector &r,
152 const vpMatrix* A, const vpColVector* b,
153 const vpMatrix* C, const vpColVector* d,
154 const std::string fct)
155 {
156 // check data consistency
157 unsigned int n = Q.getCols();
158 const bool Ab = (A != nullptr && b != nullptr && A->getRows());
159 const bool Cd = (C != nullptr && d != nullptr && C->getRows());
160
161 if ( (Ab && n != A->getCols()) ||
162 (Cd && n != C->getCols()) ||
163 (Ab && A->getRows() != b->getRows()) ||
164 (Cd && C->getRows() != d->getRows()) ||
165 Q.getRows() != r.getRows())
166 {
167 std::cout << "vpQuadProg::" << fct << ": wrong dimension\n" <<
168 "Q: " << Q.getRows() << "x" << Q.getCols() << " - r: " << r.getRows() << std::endl;
169 if(Ab)
170 std::cout << "A: " << A->getRows() << "x" << A->getCols() << " - b: " << b->getRows() << std::endl;
171 if(Cd)
172 std::cout << "C: " << C->getRows() << "x" << C->getCols() << " - d: " << d->getRows() << std::endl;
174 }
175 return n;
176 }
177#endif
178};
179#endif
unsigned int getCols() const
Definition: vpArray2D.h:279
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
@ dimensionError
Bad dimension.
Definition: vpException.h:95
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
This class provides a solver for Quadratic Programs.
Definition: vpQuadProg.h:75
std::vector< unsigned int > inactive
Definition: vpQuadProg.h:120
std::vector< unsigned int > active
Definition: vpQuadProg.h:116
vpMatrix Z
Definition: vpQuadProg.h:128
vpColVector x1
Definition: vpQuadProg.h:124
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
void resetActiveSet()
Definition: vpQuadProg.h:101