#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, 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 vpColVector	solveSVDorQR (const vpMatrix &A, const vpColVector &b)

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 or higher is activated during build. Configure ViSP using cmake -DUSE_CXX_STANDARD=11.

Examples:: quadprog.cpp, and quadprog_eq.cpp.

Definition at line 74 of file vpQuadProg.h.

Member Function Documentation

◆ checkDimensions()

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 151 of file vpQuadProg.h.

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

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

◆ fromCanonicalCost()

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 95 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().

◆ resetActiveSet()

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 101 of file vpQuadProg.h.

◆ setEqualityConstraint()

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 147 of file vpQuadProg.cpp.

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

◆ solveByProjection()

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 176 of file vpQuadProg.cpp.

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

Referenced by solveQPe(), and solveQPi().

◆ solveQP()

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 373 of file vpQuadProg.cpp.

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

◆ solveQPe() [1/2]

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 244 of file vpQuadProg.cpp.

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

◆ solveQPe() [2/2]

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 309 of file vpQuadProg.cpp.

References checkDimensions(), and solveByProjection().

◆ solveQPi()

bool vpQuadProg::solveQPi	(	const vpMatrix &	Q,
		const vpColVector &	r,
		const vpMatrix &	C,
		const vpColVector &	d,
		vpColVector &	x,
		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 443 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().

◆ solveSVDorQR()

vpColVector vpQuadProg::solveSVDorQR	(	const vpMatrix &	A,
		const vpColVector &	b
	)

staticprotected

Pick either SVD (over-constrained) or QR (square or under-constrained)

Assumes A is full rank, hence uses SVD iif A has more row than columns.

Parameters

A	: matrix (dimension m x n)
b	: vector (dimension m)

Returns: least-norm solution to $\arg\min||\mathbf{A}\mathbf{x} - \mathbf{b}||$

Definition at line 664 of file vpQuadProg.cpp.

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

Member Data Documentation

◆ active

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 116 of file vpQuadProg.h.

Referenced by solveQPi().

◆ inactive

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 120 of file vpQuadProg.h.

Referenced by solveQPi().

◆ x1

vpColVector vpQuadProg::x1

protected

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

Definition at line 124 of file vpQuadProg.h.

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

◆ Z

vpMatrix vpQuadProg::Z

protected

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

Definition at line 128 of file vpQuadProg.h.

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

Public Member Functions

Static Public Member Functions

Static Protected Member Functions

Protected Attributes

Detailed Description

Member Function Documentation

◆ checkDimensions()

◆ fromCanonicalCost()

◆ resetActiveSet()

◆ setEqualityConstraint()

◆ solveByProjection()

◆ solveQP()

◆ solveQPe() [1/2]

◆ solveQPe() [2/2]

◆ solveQPi()

◆ solveSVDorQR()

Member Data Documentation

◆ active

◆ inactive

◆ x1

◆ Z