Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
Static Public Member Functions | Static Protected Member Functions | Protected Attributes | List of all members
vpQuadProg Class Reference
core: Core module » Tools » Optimization

#include <visp3/core/vpQuadProg.h>

Public Member Functions

Instanciated solvers
bool solveQPe (const vpMatrix &Q, const vpColVector &r, vpColVector &x, const double &tol=1e-6) const
 
bool solveQPi (const vpMatrix &Q, const vpColVector &r, const vpMatrix &C, const vpColVector &d, vpColVector &x, const bool use_equality=false, const double &tol=1e-6)
 
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)
 
Managing sequential calls to solvers
bool setEqualityConstraint (const vpMatrix &A, const vpColVector &b, const double &tol=1e-6)
 
void resetActiveSet ()
 

Static Public Member Functions

static void fromCanonicalCost (const vpMatrix &H, const vpColVector &c, vpMatrix &Q, vpColVector &r, const double &tol=1e-6)
 
static bool solveQPe (const vpMatrix &Q, const vpColVector &r, vpMatrix A, vpColVector b, vpColVector &x, const double &tol=1e-6)
 

Static Protected Member Functions

static bool solveByProjection (const vpMatrix &Q, const vpColVector &r, vpMatrix &A, vpColVector &b, vpColVector &x, const double &tol=1e-6)
 
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)
 

Protected Attributes

std::vector< unsigned int > active
 
std::vector< unsigned int > inactive
 
vpColVector x1
 
vpMatrix Z
 

Detailed Description

This class provides a solver for Quadratic Programs.

The cost function is written under the form $ \min ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2$.

If a cost function is written under the canonical form $\min \frac{1}{2}\mathbf{x}^T\mathbf{H}\mathbf{x} + \mathbf{c}^T\mathbf{x}$ then fromCanonicalCost() can be used to retrieve Q and r from H and c.

Equality constraints are solved through projection into the kernel.

Inequality constraints are solved with active sets.

In order to be used sequentially, the decomposition of the equality constraint may be stored. The last active set is always stored and used to warm start the next call.

Warning
The solvers are only available if C++11 is activated during compilation. Configure ViSP using cmake -DUSE_CPP11=ON.
Examples:
quadprog.cpp, and quadprog_eq.cpp.

Definition at line 73 of file vpQuadProg.h.

Member Function Documentation

static unsigned int vpQuadProg::checkDimensions ( const vpMatrix &  Q,
const vpColVector &  r,
const vpMatrix *  A,
const vpColVector *  b,
const vpMatrix *  C,
const vpColVector *  d,
const std::string  fct 
)
inlinestaticprotected

Performs a dimension check of passed QP matrices and vectors.

If any inconsistency is detected, displays a summary and throws an exception.

Parameters
Q: cost matrix (dimension c x n)
r: cost vector (dimension c)
A: pointer to the equality matrix (if any, dimension m x n)
b: pointer to the equality vector (if any, dimension m)
C: pointer to the inequality matrix (if any, dimension p x n)
d: pointer to the inequality vector (if any, dimension p)
fct: name of the solver that called this function
Returns
the dimension of the search space.

Definition at line 148 of file vpQuadProg.h.

References vpException::dimensionError, vpArray2D< Type >::getCols(), and vpArray2D< Type >::getRows().

Referenced by solveQP(), solveQPe(), and solveQPi().

void vpQuadProg::fromCanonicalCost ( const vpMatrix &  H,
const vpColVector &  c,
vpMatrix &  Q,
vpColVector &  r,
const double &  tol = 1e-6 
)
static

Changes a canonical quadratic cost $\min \frac{1}{2}\mathbf{x}^T\mathbf{H}\mathbf{x} + \mathbf{c}^T\mathbf{x}$ to the formulation used by this class $ \min ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2$.

Computes $(\mathbf{Q}, \mathbf{r})$ such that $\mathbf{H} = \mathbf{Q}^T\mathbf{Q}$ and $\mathbf{c} = -\mathbf{Q}^T\mathbf{r}$.

Parameters
H: canonical symmetric positive cost matrix (dimension n x n)
c: canonical cost vector (dimension n)
Q: custom cost matrix (dimension rank(H) x n)
r: custom cost vector (dimension rank(H))
tol: tolerance to test ranks
Warning
This method is only available if the Gnu Scientific Library (GSL) is detected as a third party library.

Here is an example:

$\begin{array}{lll} \mathbf{x} = & \arg\min & 2x_1^2 + x_2^2 + x_1x_2 + x_1 + x_2\\ & \text{s.t.}& x_1 + x_2 = 1 \end{array} \Leftrightarrow \begin{array}{lll} \mathbf{x} = & \arg\min & \frac{1}{2}\mathbf{x}^T\left[\begin{array}{cc}4 & 1 \\ 1 & 2\end{array}\right]\mathbf{x} + [1~1]\mathbf{x}\\ & \text{s.t.}& [1~1]\mathbf{x} = 1 \end{array} $

#include <visp3/core/vpLinProg.h>
int main()
{
vpMatrix H(2,2), A(1,2);
vpColVector c(2), b(1);
H[0][0] = 4;
H[0][1] = H[1][0] = 1;
H[1][1] = 2;
c[0] = c[1] = 1;
A[0][0] = A[0][1] = 1;
b[0] = 1;
vpMatrix Q;vpColVector r;
vpQuadProg::fromCanonicalCost(H, c, Q, r);
vpColVector x;
vpQuadProg::solveQPe(Q, r, A, b, x);
std::cout << x.t() << std::endl; // prints (0.25 0.75)
}

Definition at line 98 of file vpQuadProg.cpp.

References vpMatrix::diag(), vpException::dimensionError, vpMatrix::eigenValues(), vpColVector::extract(), vpException::functionNotImplementedError, vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), vpMatrixException::matrixError, vpMatrix::pseudoInverse(), vpMatrix::t(), and vpMatrix::transpose().

void vpQuadProg::resetActiveSet ( )
inline

Resets the active set that was found by a previous call to solveQP() or solveQPi(), if any.

Examples:
quadprog.cpp.

Definition at line 100 of file vpQuadProg.h.

bool vpQuadProg::setEqualityConstraint ( const vpMatrix &  A,
const vpColVector &  b,
const double &  tol = 1e-6 
)

Saves internally the column reduction of the passed equality constraint:

$\mathbf{A}\mathbf{x} = \mathbf{b} \Leftrightarrow \mathbf{x} = \mathbf{x}_1 + \mathbf{Z}\mathbf{z} $

Parameters
A: equality matrix (dimension m x n)
b: equality vector (dimension m)
tol: tolerance to test the ranks
Returns
True if $\mathbf{A}\mathbf{x} = \mathbf{b}$ has a solution.
See also
solveQPi(), solveQP()
Examples:
quadprog_eq.cpp.

Definition at line 155 of file vpQuadProg.cpp.

References vpLinProg::colReduction(), vpArray2D< Type >::getRows(), x1, and Z.

bool vpQuadProg::solveByProjection ( const vpMatrix &  Q,
const vpColVector &  r,
vpMatrix &  A,
vpColVector &  b,
vpColVector &  x,
const double &  tol = 1e-6 
)
staticprotected

Solves a Quadratic Program under equality constraints.

$\begin{array}{lll} \mathbf{x} = & \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\ & \text{s.t.}& \mathbf{A}\mathbf{x} = \mathbf{b}\end{array} $

Parameters
Q: cost matrix (dimension c x n)
r: cost vector (dimension c)
A: equality matrix (dimension m x n)
b: equality vector (dimension m)
x: solution (dimension n)
tol: tolerance to test the ranks
Returns
True if the solution was found.

This function is for internal use, no dimension check is performed and A and b may be modified.

Definition at line 184 of file vpQuadProg.cpp.

References vpLinProg::colReduction(), vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), and vpMatrix::solveBySVD().

Referenced by solveQPe(), and solveQPi().

bool vpQuadProg::solveQP ( const vpMatrix &  Q,
const vpColVector &  r,
vpMatrix  A,
vpColVector  b,
const vpMatrix &  C,
const vpColVector &  d,
vpColVector &  x,
const double &  tol = 1e-6 
)

Solves a Quadratic Program under equality and inequality constraints.

If the equality constraints $(\mathbf{A}, \mathbf{b})$ are the same between two calls, it is more efficient to use setEqualityConstraint() and solveQPi().

$\begin{array}{lll} \mathbf{x} = & \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\ & \text{s.t.}& \mathbf{A}\mathbf{x} = \mathbf{b} \\ & \text{s.t.}& \mathbf{C}\mathbf{x} \leq \mathbf{d} \end{array} $

Parameters
Q: cost matrix (dimension c x n)
r: cost vector (dimension c)
A: equality matrix (dimension m x n)
b: equality vector (dimension m)
C: inequality matrix (dimension p x n)
d: inequality vector (dimension p)
x: solution (dimension n)
tol: tolerance to test the ranks
Returns
True if the solution was found.

Here is an example:

$\begin{array}{lll} \mathbf{x} = & \arg\min & (x_1-1)^2 + x_2^2 \\ & \text{s.t.}& x_1 + x_2 = 1 \\ & \text{s.t.} & x_2 \geq 1\end{array}$

#include <visp3/core/vpLinProg.h>
int main()
{
vpMatrix Q(2,2), A(1,2), C(1,2);
vpColVector r(2), b(1), d(1);
Q[0][0] = Q[1][1] = 1; r[0] = 1;
A[0][0] = A[0][1] = 1; b[0] = 1;
C[0][1] = -1; d[0] = -1;
vpColVector x;
vpQuadProg qp;
qp.solveQP(Q, r, A, b, C, d, x);
std::cout << x.t() << std::endl; // prints (0 1)
}
See also
resetActiveSet()
Examples:
quadprog.cpp, and quadprog_eq.cpp.

Definition at line 389 of file vpQuadProg.cpp.

References vpLinProg::allLesser(), checkDimensions(), vpLinProg::colReduction(), vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), and solveQPi().

bool vpQuadProg::solveQPe ( const vpMatrix &  Q,
const vpColVector &  r,
vpColVector &  x,
const double &  tol = 1e-6 
) const

Solves a Quadratic Program under previously stored equality constraints (setEqualityConstraint())

$\begin{array}{lll} \mathbf{x} = & \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\ & \text{s.t.}& \mathbf{A}\mathbf{x} = \mathbf{b}\end{array} $

Parameters
Q: cost matrix (dimension c x n)
r: cost vector (dimension c)
x: solution (dimension n)
tol: Tolerance.
Returns
True if the solution was found.

Here is an example where the two following QPs are solved:

$\begin{array}{lll} \mathbf{x} = & \arg\min & x_1^2 + x_2^2 \\ & \text{s.t.}& x_1 + x_2 = 1\end{array}$

$\begin{array}{lll} \mathbf{x} = & \arg\min & (x_1-1)^2 + x_2^2 \\ & \text{s.t.}& x_1 + x_2 = 1\end{array}$

#include <visp3/core/vpLinProg.h>
int main()
{
vpMatrix Q(2,2), A(1,2);
vpColVector r(2), b(1);
Q[0][0] = Q[1][1] = 1;
A[0][0] = A[0][1] = b[0] = 1;
vpQuadProg qp;
qp.setEqualityConstraint(A, b);
vpColVector x;
// solve x_1^2 + x_2^2
qp.solveQPe(Q, r, x);
std::cout << x.t() << std::endl; // prints (0.5 0.5)
// solve (x_1-1)^2 + x_2^2
r[0] = 1;
qp.solveQPe(Q, r, x);
std::cout << x.t() << std::endl; // prints (1 0)
}
See also
setEqualityConstraint(), solveQPe()
Examples:
quadprog_eq.cpp.

Definition at line 255 of file vpQuadProg.cpp.

References vpException::dimensionError, vpArray2D< Type >::getCols(), vpArray2D< Type >::getRows(), vpMatrix::solveBySVD(), x1, and Z.

bool vpQuadProg::solveQPe ( const vpMatrix &  Q,
const vpColVector &  r,
vpMatrix  A,
vpColVector  b,
vpColVector &  x,
const double &  tol = 1e-6 
)
static

Solves a Quadratic Program under equality constraints.

$\begin{array}{lll} \mathbf{x} = & \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\ & \text{s.t.}& \mathbf{A}\mathbf{x} = \mathbf{b}\end{array}$

If the equality constraints $(\mathbf{A}, \mathbf{b})$ are the same between two calls, it is more efficient to use setEqualityConstraint() and solveQPe().

Parameters
Q: cost matrix (dimension c x n)
r: cost vector (dimension c)
A: equality matrix (dimension m x n)
b: equality vector (dimension m)
x: solution (dimension n)
tol: tolerance to test the ranks
Returns
True if the solution was found.

Here is an example:

$\begin{array}{lll} \mathbf{x} = & \arg\min & (x_1-1)^2 + x_2^2 \\ & \text{s.t.}& x_1 + x_2 = 1\end{array}$

#include <visp3/core/vpLinProg.h>
int main()
{
vpMatrix Q(2,2), A(1,2);
vpColVector r(2), b(1);
Q[0][0] = Q[1][1] = 1;
r[0] = 1;
A[0][0] = A[0][1] = b[0] = 1;
vpColVector x;
vpQuadProg::solveQPe(Q, r, A, b, x);
std::cout << x.t() << std::endl; // prints (1 0)
}
See also
setEqualityConstraint(), solveQP(), solveQPi()

Definition at line 325 of file vpQuadProg.cpp.

References checkDimensions(), and solveByProjection().

bool vpQuadProg::solveQPi ( const vpMatrix &  Q,
const vpColVector &  r,
const vpMatrix &  C,
const vpColVector &  d,
vpColVector &  x,
const bool  use_equality = false,
const double &  tol = 1e-6 
)

Solves a Quadratic Program under inequality constraints

$\begin{array}{lll} \mathbf{x} = & \arg\min & ||\mathbf{Q}\mathbf{x} - \mathbf{r}||^2 \\ & \text{s.t.}& \mathbf{C}\mathbf{x} \leq \mathbf{d} \end{array} $

Parameters
Q: cost matrix (dimension c x n)
r: cost vector (dimension c)
C: inequality matrix (dimension p x n)
d: inequality vector (dimension p)
x: solution (dimension n)
use_equality: if a previously saved equality constraint (setEqualityConstraint()) should be considered
tol: tolerance to test the ranks
Returns
True if the solution was found.

Here is an example:

$\begin{array}{lll} \mathbf{x} = & \arg\min & (x_1-1)^2 + x_2^2 \\ & \text{s.t.}& x_1 + x_2 \leq 1 \\ & \text{s.t.} & x_1, x_2 \geq 0\end{array}$

#include <visp3/core/vpLinProg.h>
int main()
{
vpMatrix Q(2,2), C(3,2);
vpColVector r(2), d(1);
Q[0][0] = Q[1][1] = 1; r[0] = 1;
C[0][0] = C[0][1] = 1; d[0] = 1;
C[1][0] = C[2][1] = -1;
vpColVector x;
vpQuadProg qp;
qp.solveQPi(Q, r, C, d, x);
std::cout << x.t() << std::endl; // prints (1 0)
}
Examples:
quadprog_eq.cpp.

Definition at line 465 of file vpQuadProg.cpp.

References active, vpLinProg::allGreater(), vpLinProg::allLesser(), vpLinProg::allZero(), checkDimensions(), vpColVector::clear(), vpColVector::extract(), vpArray2D< Type >::getCols(), vpMatrix::getRow(), vpArray2D< Type >::getRows(), inactive, vpMatrix::pseudoInverse(), vpArray2D< Type >::resize(), vpColVector::resize(), vpLinProg::simplex(), vpArray2D< Type >::size(), solveByProjection(), vpMatrix::transpose(), x1, and Z.

Referenced by solveQP().

Member Data Documentation

std::vector<unsigned int> vpQuadProg::active
protected

Active set from the last call to solveQP() or solveQPi(). Used for warm starting the next call.

Definition at line 115 of file vpQuadProg.h.

Referenced by solveQPi().

std::vector<unsigned int> vpQuadProg::inactive
protected

Inactive set from the last call to solveQP() or solveQPi(). Used for warm starting the next call.

Definition at line 119 of file vpQuadProg.h.

Referenced by solveQPi().

vpColVector vpQuadProg::x1
protected

Stored particular solution from the last call to setEqualityConstraint().

Definition at line 123 of file vpQuadProg.h.

Referenced by setEqualityConstraint(), solveQPe(), and solveQPi().

vpMatrix vpQuadProg::Z
protected

Stored projection to the kernel from the last call to setEqualityConstraint().

Definition at line 127 of file vpQuadProg.h.

Referenced by setEqualityConstraint(), solveQPe(), and solveQPi().