Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
alta
alta
Commits
78aacae7
Commit
78aacae7
authored
Jan 21, 2013
by
Laurent Belcour
Browse files
Adding in-progress Eigen quadractic programming code
parent
a2ee7d45
Changes
6
Hide whitespace changes
Inline
Side-by-side
sources/libs/rational_1d/eiquadprog.hpp
0 → 100644
View file @
78aacae7
#ifndef _EIGEN_QUADSOLVE_HPP_
#define _EIGEN_QUADSOLVE_HPP_
/*
FILE uquadprog.hh
NOTE: this is a modified of QuadProg++ package, originally developed by
Luca Di Gaspero, working with ublas data structures.
The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
for the solution of a (convex) Quadratic Programming problem
by means of a dual method.
The problem is in the form:
min 0.5 * x G x + g0 x
s.t.
CE^T x + ce0 = 0
CI^T x + ci0 >= 0
The matrix and vectors dimensions are as follows:
G: n * n
g0: n
CE: n * p
ce0: p
CI: n * m
ci0: m
x: n
The function will return the cost of the solution written in the x vector or
std::numeric_limits::infinity() if the problem is infeasible. In the latter case
the value of the x vector is not correct.
References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.
Notes:
1. pay attention in setting up the vectors ce0 and ci0.
If the constraints of your problem are specified in the form
A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
2. The matrix G is modified within the function since it is used to compute
the G = L^T L cholesky factorization for further computations inside the function.
If you need the original matrix G you should make a copy of it and pass the copy
to the function.
Author: Angelo Furfaro
DEIS - University of Calabria, Italy
a.furfaro@deis.unical.it
http://www.lis.deis.unical.it/~furfaro
The author will be grateful if the researchers using this software will
acknowledge the contribution of this modified function and of Di Gaspero's
original version in their research papers.
LICENSE
Copyright (2008) Angelo Furfaro
Copyright (2006) Luca Di Gaspero
This file is a porting of QuadProg++ routine, originally developed
by Luca Di Gaspero, exploiting uBlas data structures for vectors and
matrices instead of native C++ array.
uquadprog is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
uquadprog is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with uquadprog; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include
<Eigen/Dense>
namespace
Eigen
{
// namespace internal {
template
<
typename
Scalar
>
inline
Scalar
distance
(
Scalar
a
,
Scalar
b
)
{
Scalar
a1
,
b1
,
t
;
a1
=
internal
::
abs
(
a
);
b1
=
internal
::
abs
(
b
);
if
(
a1
>
b1
)
{
t
=
(
b1
/
a1
);
return
a1
*
internal
::
sqrt
(
1.0
+
t
*
t
);
}
else
if
(
b1
>
a1
)
{
t
=
(
a1
/
b1
);
return
b1
*
internal
::
sqrt
(
1.0
+
t
*
t
);
}
return
a1
*
internal
::
sqrt
(
2.0
);
}
// }
inline
void
compute_d
(
VectorXd
&
d
,
const
MatrixXd
&
J
,
const
VectorXd
&
np
)
{
d
=
J
.
adjoint
()
*
np
;
}
inline
void
update_z
(
VectorXd
&
z
,
const
MatrixXd
&
J
,
const
VectorXd
&
d
,
int
iq
)
{
z
=
J
.
rightCols
(
z
.
size
()
-
iq
)
*
d
.
tail
(
d
.
size
()
-
iq
);
}
inline
void
update_r
(
const
MatrixXd
&
R
,
VectorXd
&
r
,
const
VectorXd
&
d
,
int
iq
)
{
r
.
head
(
iq
)
=
R
.
topLeftCorner
(
iq
,
iq
).
triangularView
<
Upper
>
().
solve
(
d
.
head
(
iq
));
}
bool
add_constraint
(
MatrixXd
&
R
,
MatrixXd
&
J
,
VectorXd
&
d
,
int
&
iq
,
double
&
R_norm
);
void
delete_constraint
(
MatrixXd
&
R
,
MatrixXd
&
J
,
VectorXi
&
A
,
VectorXd
&
u
,
int
p
,
int
&
iq
,
int
l
);
inline
double
solve_quadprog
(
MatrixXd
&
G
,
VectorXd
&
g0
,
const
MatrixXd
&
CE
,
const
VectorXd
&
ce0
,
const
MatrixXd
&
CI
,
const
VectorXd
&
ci0
,
VectorXd
&
x
)
{
int
i
,
j
,
k
,
l
;
/* indices */
int
ip
,
me
,
mi
;
int
n
=
g0
.
size
();
int
p
=
ce0
.
size
();
int
m
=
ci0
.
size
();
MatrixXd
R
(
G
.
rows
(),
G
.
cols
()),
J
(
G
.
rows
(),
G
.
cols
());
LLT
<
MatrixXd
,
Lower
>
chol
(
G
.
cols
());
VectorXd
s
(
m
+
p
),
z
(
n
),
r
(
m
+
p
),
d
(
n
),
np
(
n
),
u
(
m
+
p
);
VectorXd
x_old
(
n
),
u_old
(
m
+
p
);
double
f_value
,
psi
,
c1
,
c2
,
sum
,
ss
,
R_norm
;
const
double
inf
=
std
::
numeric_limits
<
double
>::
infinity
();
double
t
,
t1
,
t2
;
/* t is the step length, which is the minimum of the partial step length t1
* and the full step length t2 */
VectorXi
A
(
m
+
p
),
A_old
(
m
+
p
),
iai
(
m
+
p
);
int
q
;
int
iq
,
iter
=
0
;
bool
iaexcl
[
m
+
p
];
me
=
p
;
/* number of equality constraints */
mi
=
m
;
/* number of inequality constraints */
q
=
0
;
/* size of the active set A (containing the indices of the active constraints) */
/*
* Preprocessing phase
*/
/* compute the trace of the original matrix G */
c1
=
G
.
trace
();
/* decompose the matrix G in the form LL^T */
chol
.
compute
(
G
);
/* initialize the matrix R */
d
.
setZero
();
R
.
setZero
();
R_norm
=
1.0
;
/* this variable will hold the norm of the matrix R */
/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
// J = L^-T
J
.
setIdentity
();
J
=
chol
.
matrixU
().
solve
(
J
);
c2
=
J
.
trace
();
#ifdef TRACE_SOLVER
print_matrix
(
"J"
,
J
,
n
);
#endif
/* c1 * c2 is an estimate for cond(G) */
/*
* Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
* this is a feasible point in the dual space
* x = G^-1 * g0
*/
x
=
chol
.
solve
(
g0
);
x
=
-
x
;
/* and compute the current solution value */
f_value
=
0.5
*
g0
.
dot
(
x
);
#ifdef TRACE_SOLVER
std
::
cerr
<<
"Unconstrained solution: "
<<
f_value
<<
std
::
endl
;
print_vector
(
"x"
,
x
,
n
);
#endif
/* Add equality constraints to the working set A */
iq
=
0
;
for
(
i
=
0
;
i
<
me
;
i
++
)
{
np
=
CE
.
col
(
i
);
compute_d
(
d
,
J
,
np
);
update_z
(
z
,
J
,
d
,
iq
);
update_r
(
R
,
r
,
d
,
iq
);
#ifdef TRACE_SOLVER
print_matrix
(
"R"
,
R
,
iq
);
print_vector
(
"z"
,
z
,
n
);
print_vector
(
"r"
,
r
,
iq
);
print_vector
(
"d"
,
d
,
n
);
#endif
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
t2
=
0.0
;
if
(
internal
::
abs
(
z
.
dot
(
z
))
>
std
::
numeric_limits
<
double
>::
epsilon
())
// i.e. z != 0
t2
=
(
-
np
.
dot
(
x
)
-
ce0
(
i
))
/
z
.
dot
(
np
);
x
+=
t2
*
z
;
/* set u = u+ */
u
(
iq
)
=
t2
;
u
.
head
(
iq
)
-=
t2
*
r
.
head
(
iq
);
/* compute the new solution value */
f_value
+=
0.5
*
(
t2
*
t2
)
*
z
.
dot
(
np
);
A
(
i
)
=
-
i
-
1
;
if
(
!
add_constraint
(
R
,
J
,
d
,
iq
,
R_norm
))
{
// FIXME: it should raise an error
// Equality constraints are linearly dependent
return
f_value
;
}
}
/* set iai = K \ A */
for
(
i
=
0
;
i
<
mi
;
i
++
)
iai
(
i
)
=
i
;
l1:
iter
++
;
#ifdef TRACE_SOLVER
print_vector
(
"x"
,
x
,
n
);
#endif
/* step 1: choose a violated constraint */
for
(
i
=
me
;
i
<
iq
;
i
++
)
{
ip
=
A
(
i
);
iai
(
ip
)
=
-
1
;
}
/* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
ss
=
0.0
;
psi
=
0.0
;
/* this value will contain the sum of all infeasibilities */
ip
=
0
;
/* ip will be the index of the chosen violated constraint */
for
(
i
=
0
;
i
<
mi
;
i
++
)
{
iaexcl
[
i
]
=
true
;
sum
=
CI
.
col
(
i
).
dot
(
x
)
+
ci0
(
i
);
s
(
i
)
=
sum
;
psi
+=
std
::
min
(
0.0
,
sum
);
}
#ifdef TRACE_SOLVER
print_vector
(
"s"
,
s
,
mi
);
#endif
if
(
internal
::
abs
(
psi
)
<=
mi
*
std
::
numeric_limits
<
double
>::
epsilon
()
*
c1
*
c2
*
100.0
)
{
/* numerically there are not infeasibilities anymore */
q
=
iq
;
return
f_value
;
}
/* save old values for u, x and A */
u_old
.
head
(
iq
)
=
u
.
head
(
iq
);
A_old
.
head
(
iq
)
=
A
.
head
(
iq
);
x_old
=
x
;
l2:
/* Step 2: check for feasibility and determine a new S-pair */
for
(
i
=
0
;
i
<
mi
;
i
++
)
{
if
(
s
(
i
)
<
ss
&&
iai
(
i
)
!=
-
1
&&
iaexcl
[
i
])
{
ss
=
s
(
i
);
ip
=
i
;
}
}
if
(
ss
>=
0.0
)
{
q
=
iq
;
return
f_value
;
}
/* set np = n(ip) */
np
=
CI
.
col
(
ip
);
/* set u = (u 0)^T */
u
(
iq
)
=
0.0
;
/* add ip to the active set A */
A
(
iq
)
=
ip
;
#ifdef TRACE_SOLVER
std
::
cerr
<<
"Trying with constraint "
<<
ip
<<
std
::
endl
;
print_vector
(
"np"
,
np
,
n
);
#endif
l2a:
/* Step 2a: determine step direction */
/* compute z = H np: the step direction in the primal space (through J, see the paper) */
compute_d
(
d
,
J
,
np
);
update_z
(
z
,
J
,
d
,
iq
);
/* compute N* np (if q > 0): the negative of the step direction in the dual space */
update_r
(
R
,
r
,
d
,
iq
);
#ifdef TRACE_SOLVER
std
::
cerr
<<
"Step direction z"
<<
std
::
endl
;
print_vector
(
"z"
,
z
,
n
);
print_vector
(
"r"
,
r
,
iq
+
1
);
print_vector
(
"u"
,
u
,
iq
+
1
);
print_vector
(
"d"
,
d
,
n
);
print_ivector
(
"A"
,
A
,
iq
+
1
);
#endif
/* Step 2b: compute step length */
l
=
0
;
/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
t1
=
inf
;
/* +inf */
/* find the index l s.t. it reaches the minimum of u+(x) / r */
for
(
k
=
me
;
k
<
iq
;
k
++
)
{
double
tmp
;
if
(
r
(
k
)
>
0.0
&&
((
tmp
=
u
(
k
)
/
r
(
k
))
<
t1
)
)
{
t1
=
tmp
;
l
=
A
(
k
);
}
}
/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
if
(
internal
::
abs
(
z
.
dot
(
z
))
>
std
::
numeric_limits
<
double
>::
epsilon
())
// i.e. z != 0
t2
=
-
s
(
ip
)
/
z
.
dot
(
np
);
else
t2
=
inf
;
/* +inf */
/* the step is chosen as the minimum of t1 and t2 */
t
=
std
::
min
(
t1
,
t2
);
#ifdef TRACE_SOLVER
std
::
cerr
<<
"Step sizes: "
<<
t
<<
" (t1 = "
<<
t1
<<
", t2 = "
<<
t2
<<
") "
;
#endif
/* Step 2c: determine new S-pair and take step: */
/* case (i): no step in primal or dual space */
if
(
t
>=
inf
)
{
/* QPP is infeasible */
// FIXME: unbounded to raise
q
=
iq
;
return
inf
;
}
/* case (ii): step in dual space */
if
(
t2
>=
inf
)
{
/* set u = u + t * [-r 1) and drop constraint l from the active set A */
u
.
head
(
iq
)
-=
t
*
r
.
head
(
iq
);
u
(
iq
)
+=
t
;
iai
(
l
)
=
l
;
delete_constraint
(
R
,
J
,
A
,
u
,
p
,
iq
,
l
);
#ifdef TRACE_SOLVER
std
::
cerr
<<
" in dual space: "
<<
f_value
<<
std
::
endl
;
print_vector
(
"x"
,
x
,
n
);
print_vector
(
"z"
,
z
,
n
);
print_ivector
(
"A"
,
A
,
iq
+
1
);
#endif
goto
l2a
;
}
/* case (iii): step in primal and dual space */
x
+=
t
*
z
;
/* update the solution value */
f_value
+=
t
*
z
.
dot
(
np
)
*
(
0.5
*
t
+
u
(
iq
));
u
.
head
(
iq
)
-=
t
*
r
.
head
(
iq
);
u
(
iq
)
+=
t
;
#ifdef TRACE_SOLVER
std
::
cerr
<<
" in both spaces: "
<<
f_value
<<
std
::
endl
;
print_vector
(
"x"
,
x
,
n
);
print_vector
(
"u"
,
u
,
iq
+
1
);
print_vector
(
"r"
,
r
,
iq
+
1
);
print_ivector
(
"A"
,
A
,
iq
+
1
);
#endif
if
(
t
==
t2
)
{
#ifdef TRACE_SOLVER
std
::
cerr
<<
"Full step has taken "
<<
t
<<
std
::
endl
;
print_vector
(
"x"
,
x
,
n
);
#endif
/* full step has taken */
/* add constraint ip to the active set*/
if
(
!
add_constraint
(
R
,
J
,
d
,
iq
,
R_norm
))
{
iaexcl
[
ip
]
=
false
;
delete_constraint
(
R
,
J
,
A
,
u
,
p
,
iq
,
ip
);
#ifdef TRACE_SOLVER
print_matrix
(
"R"
,
R
,
n
);
print_ivector
(
"A"
,
A
,
iq
);
#endif
for
(
i
=
0
;
i
<
m
;
i
++
)
iai
(
i
)
=
i
;
for
(
i
=
0
;
i
<
iq
;
i
++
)
{
A
(
i
)
=
A_old
(
i
);
iai
(
A
(
i
))
=
-
1
;
u
(
i
)
=
u_old
(
i
);
}
x
=
x_old
;
goto
l2
;
/* go to step 2 */
}
else
iai
(
ip
)
=
-
1
;
#ifdef TRACE_SOLVER
print_matrix
(
"R"
,
R
,
n
);
print_ivector
(
"A"
,
A
,
iq
);
#endif
goto
l1
;
}
/* a patial step has taken */
#ifdef TRACE_SOLVER
std
::
cerr
<<
"Partial step has taken "
<<
t
<<
std
::
endl
;
print_vector
(
"x"
,
x
,
n
);
#endif
/* drop constraint l */
iai
(
l
)
=
l
;
delete_constraint
(
R
,
J
,
A
,
u
,
p
,
iq
,
l
);
#ifdef TRACE_SOLVER
print_matrix
(
"R"
,
R
,
n
);
print_ivector
(
"A"
,
A
,
iq
);
#endif
s
(
ip
)
=
CI
.
col
(
ip
).
dot
(
x
)
+
ci0
(
ip
);
#ifdef TRACE_SOLVER
print_vector
(
"s"
,
s
,
mi
);
#endif
goto
l2a
;
}
inline
bool
add_constraint
(
MatrixXd
&
R
,
MatrixXd
&
J
,
VectorXd
&
d
,
int
&
iq
,
double
&
R_norm
)
{
int
n
=
J
.
rows
();
#ifdef TRACE_SOLVER
std
::
cerr
<<
"Add constraint "
<<
iq
<<
'/'
;
#endif
int
i
,
j
,
k
;
double
cc
,
ss
,
h
,
t1
,
t2
,
xny
;
/* we have to find the Givens rotation which will reduce the element
d(j) to zero.
if it is already zero we don't have to do anything, except of
decreasing j */
for
(
j
=
n
-
1
;
j
>=
iq
+
1
;
j
--
)
{
/* The Givens rotation is done with the matrix (cc cs, cs -cc).
If cc is one, then element (j) of d is zero compared with element
(j - 1). Hence we don't have to do anything.
If cc is zero, then we just have to switch column (j) and column (j - 1)
of J. Since we only switch columns in J, we have to be careful how we
update d depending on the sign of gs.
Otherwise we have to apply the Givens rotation to these columns.
The i - 1 element of d has to be updated to h. */
cc
=
d
(
j
-
1
);
ss
=
d
(
j
);
h
=
distance
(
cc
,
ss
);
if
(
h
==
0.0
)
continue
;
d
(
j
)
=
0.0
;
ss
=
ss
/
h
;
cc
=
cc
/
h
;
if
(
cc
<
0.0
)
{
cc
=
-
cc
;
ss
=
-
ss
;
d
(
j
-
1
)
=
-
h
;
}
else
d
(
j
-
1
)
=
h
;
xny
=
ss
/
(
1.0
+
cc
);
for
(
k
=
0
;
k
<
n
;
k
++
)
{
t1
=
J
(
k
,
j
-
1
);
t2
=
J
(
k
,
j
);
J
(
k
,
j
-
1
)
=
t1
*
cc
+
t2
*
ss
;
J
(
k
,
j
)
=
xny
*
(
t1
+
J
(
k
,
j
-
1
))
-
t2
;
}
}
/* update the number of constraints added*/
iq
++
;
/* To update R we have to put the iq components of the d vector
into column iq - 1 of R
*/
R
.
col
(
iq
-
1
).
head
(
iq
)
=
d
.
head
(
iq
);
#ifdef TRACE_SOLVER
std
::
cerr
<<
iq
<<
std
::
endl
;
#endif
if
(
internal
::
abs
(
d
(
iq
-
1
))
<=
std
::
numeric_limits
<
double
>::
epsilon
()
*
R_norm
)
// problem degenerate
return
false
;
R_norm
=
std
::
max
<
double
>
(
R_norm
,
internal
::
abs
(
d
(
iq
-
1
)));
return
true
;
}
inline
void
delete_constraint
(
MatrixXd
&
R
,
MatrixXd
&
J
,
VectorXi
&
A
,
VectorXd
&
u
,
int
p
,
int
&
iq
,
int
l
)
{
int
n
=
R
.
rows
();
#ifdef TRACE_SOLVER
std
::
cerr
<<
"Delete constraint "
<<
l
<<
' '
<<
iq
;
#endif
int
i
,
j
,
k
,
qq
;
double
cc
,
ss
,
h
,
xny
,
t1
,
t2
;
/* Find the index qq for active constraint l to be removed */
for
(
i
=
p
;
i
<
iq
;
i
++
)
if
(
A
(
i
)
==
l
)
{
qq
=
i
;
break
;
}
/* remove the constraint from the active set and the duals */
for
(
i
=
qq
;
i
<
iq
-
1
;
i
++
)
{
A
(
i
)
=
A
(
i
+
1
);
u
(
i
)
=
u
(
i
+
1
);
R
.
col
(
i
)
=
R
.
col
(
i
+
1
);
}
A
(
iq
-
1
)
=
A
(
iq
);
u
(
iq
-
1
)
=
u
(
iq
);
A
(
iq
)
=
0
;
u
(
iq
)
=
0.0
;
for
(
j
=
0
;
j
<
iq
;
j
++
)
R
(
j
,
iq
-
1
)
=
0.0
;
/* constraint has been fully removed */
iq
--
;
#ifdef TRACE_SOLVER
std
::
cerr
<<
'/'
<<
iq
<<
std
::
endl
;
#endif
if
(
iq
==
0
)
return
;
for
(
j
=
qq
;
j
<
iq
;
j
++
)
{
cc
=
R
(
j
,
j
);
ss
=
R
(
j
+
1
,
j
);
h
=
distance
(
cc
,
ss
);
if
(
h
==
0.0
)
continue
;
cc
=
cc
/
h
;
ss
=
ss
/
h
;
R
(
j
+
1
,
j
)
=
0.0
;
if
(
cc
<
0.0
)
{
R
(
j
,
j
)
=
-
h
;
cc
=
-
cc
;
ss
=
-
ss
;
}
else
R
(
j
,
j
)
=
h
;
xny
=
ss
/
(
1.0
+
cc
);
for
(
k
=
j
+
1
;
k
<
iq
;
k
++
)
{
t1
=
R
(
j
,
k
);
t2
=
R
(
j
+
1
,
k
);
R
(
j
,
k
)
=
t1
*
cc
+
t2
*
ss
;
R
(
j
+
1
,
k
)
=
xny
*
(
t1
+
R
(
j
,
k
))
-
t2
;
}
for
(
k
=
0
;
k
<
n
;
k
++
)
{
t1
=
J
(
k
,
j
);
t2
=
J
(
k
,
j
+
1
);
J
(
k
,
j
)
=
t1
*
cc
+
t2
*
ss
;
J
(
k
,
j
+
1
)
=
xny
*
(
J
(
k
,
j
)
+
t1
)
-
t2
;
}
}
}
}
#endif
sources/libs/rational_1d/rational_1d_fitter.cpp
→
sources/libs/rational_1d/rational_1d_fitter
_cgal
.cpp
View file @
78aacae7
#include
"rational_1d_fitter.h"
#include
"rational_1d_fitter
_cgal
.h"