Skip to content

Commit 5ab45e0

Browse files
jcarpentjcarpent
committed
core: apply new signature and remove some memory allocation
1 parent 1cd8e93 commit 5ab45e0

File tree

1 file changed

+112
-56
lines changed

1 file changed

+112
-56
lines changed

‎src/eiquadprog.cpp‎

Lines changed: 112 additions & 56 deletions
Original file line numberDiff line numberDiff line change
@@ -5,8 +5,25 @@ namespace solvers {
55
using namespace Eigen;
66

77
/* solve_quadprog is used for on-demand QP solving */
8-
double solve_quadprog(MatrixXd& G, VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const MatrixXd& CI,
9-
const VectorXd& ci0, VectorXd& x, VectorXi& activeSet, size_t& activeSetSize) {
8+
9+
double solve_quadprog(MatrixXd &G, VectorXd &g0, const MatrixXd &CE,
10+
const VectorXd &ce0, const MatrixXd &CI,
11+
const VectorXd &ci0, VectorXd &x, VectorXi &activeSet,
12+
size_t &activeSetSize) {
13+
14+
Eigen::DenseIndex p = CE.cols();
15+
Eigen::DenseIndex m = CI.cols();
16+
17+
VectorXd y(p + m);
18+
19+
return solve_quadprog(G, g0, CE, ce0, CI, ci0, x, y, activeSet,
20+
activeSetSize);
21+
}
22+
23+
double solve_quadprog(MatrixXd &G, VectorXd &g0, const MatrixXd &CE,
24+
const VectorXd &ce0, const MatrixXd &CI,
25+
const VectorXd &ci0, VectorXd &x, VectorXd &y,
26+
VectorXi &activeSet, size_t &activeSetSize) {
1027
LLT<MatrixXd, Lower> chol(G.cols());
1128
double c1;
1229
/* compute the trace of the original matrix G */
@@ -15,37 +32,58 @@ double solve_quadprog(MatrixXd& G, VectorXd& g0, const MatrixXd& CE, const Vecto
1532
/* decompose the matrix G in the form LL^T */
1633
chol.compute(G);
1734

18-
return solve_quadprog2(chol, c1, g0, CE, ce0, CI, ci0, x, activeSet, activeSetSize);
35+
return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, y, activeSet,
36+
activeSetSize);
37+
}
38+
39+
double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
40+
const MatrixXd &CE, const VectorXd &ce0,
41+
const MatrixXd &CI, const VectorXd &ci0, VectorXd &x,
42+
VectorXi &activeSet, size_t &activeSetSize) {
43+
44+
Eigen::DenseIndex p = CE.cols();
45+
Eigen::DenseIndex m = CI.cols();
46+
47+
VectorXd y(p + m);
48+
49+
return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, y, activeSet,
50+
activeSetSize);
1951
}
2052

21-
/* solve_quadprog2 is used for when the Cholesky decomposition of G is pre-computed
53+
/* solve_quadprog2 is used for when the Cholesky decomposition of G is
54+
* pre-computed
2255
* @param A Output vector containing the indexes of the active constraints.
2356
* @param q Output value representing the size of the active set.
2457
*/
25-
double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0,
26-
const MatrixXd& CI, const VectorXd& ci0, VectorXd& x, VectorXi& A, size_t& q) {
58+
double solve_quadprog(LLT<MatrixXd, Lower> &chol, double c1, VectorXd &g0,
59+
const MatrixXd &CE, const VectorXd &ce0,
60+
const MatrixXd &CI, const VectorXd &ci0, VectorXd &x,
61+
VectorXd &u, VectorXi &A, size_t &q) {
2762
size_t i, k, l; /* indices */
2863
size_t ip, me, mi;
2964
size_t n = g0.size();
3065
size_t p = CE.cols();
3166
size_t m = CI.cols();
3267
MatrixXd R(g0.size(), g0.size()), J(g0.size(), g0.size());
3368

34-
VectorXd s(m + p), z(n), r(m + p), d(n), np(n), u(m + p);
69+
VectorXd s(m + p), z(n), r(m + p), d(n), np(n);
3570
VectorXd x_old(n), u_old(m + p);
3671
double f_value, psi, c2, sum, ss, R_norm;
3772
const double inf = std::numeric_limits<double>::infinity();
38-
double t, t1, t2; /* t is the step length, which is the minimum of the partial step length t1
39-
* and the full step length t2 */
40-
// VectorXi A(m + p); // Del Prete: active set is now an output parameter
41-
if (static_cast<size_t>(A.size()) != m + p) A.resize(m + p);
73+
double t, t1, t2; /* t is the step length, which is the minimum of the partial
74+
* step length t1 and the full step length t2 */
75+
// VectorXi A(m + p); // Del Prete: active set is now an output
76+
// parameter
77+
if (static_cast<size_t>(A.size()) != m + p)
78+
A.resize(m + p);
4279
VectorXi A_old(m + p), iai(m + p), iaexcl(m + p);
4380
// int q;
4481
size_t iq, iter = 0;
4582

4683
me = p; /* number of equality constraints */
4784
mi = m; /* number of inequality constraints */
48-
q = 0; /* size of the active set A (containing the indices of the active constraints) */
85+
q = 0; /* size of the active set A (containing the indices of the active
86+
constraints) */
4987

5088
/*
5189
* Preprocessing phase
@@ -56,12 +94,13 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
5694
R.setZero();
5795
R_norm = 1.0; /* this variable will hold the norm of the matrix R */
5896

59-
/* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
97+
/* compute the inverse of the factorized matrix G^-1, this is the initial
98+
* value for H */
6099
// J = L^-T
61100
J.setIdentity();
62-
J = chol.matrixU().solve(J);
101+
chol.matrixU().solveInPlace(J);
63102
c2 = J.trace();
64-
#ifdef TRACE_SOLVER
103+
#ifdef EIQGUADPROG_TRACE_SOLVER
65104
print_matrix("J", J, n);
66105
#endif
67106

@@ -72,11 +111,11 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
72111
* this is a feasible point in the dual space
73112
* x = G^-1 * g0
74113
*/
75-
x = chol.solve(g0);
76-
x = -x;
114+
x = -g0;
115+
chol.solveInPlace(x);
77116
/* and compute the current solution value */
78117
f_value = 0.5 * g0.dot(x);
79-
#ifdef TRACE_SOLVER
118+
#ifdef EIQGUADPROG_TRACE_SOLVER
80119
std::cerr << "Unconstrained solution: " << f_value << std::endl;
81120
print_vector("x", x, n);
82121
#endif
@@ -88,17 +127,18 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
88127
compute_d(d, J, np);
89128
update_z(z, J, d, iq);
90129
update_r(R, r, d, iq);
91-
#ifdef TRACE_SOLVER
130+
#ifdef EIQGUADPROG_TRACE_SOLVER
92131
print_matrix("R", R, iq);
93132
print_vector("z", z, n);
94133
print_vector("r", r, iq);
95134
print_vector("d", d, n);
96135
#endif
97136

98-
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
99-
becomes feasible */
137+
/* compute full step length t2: i.e., the minimum step in primal space s.t.
138+
the contraint becomes feasible */
100139
t2 = 0.0;
101-
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
140+
if (std::abs(z.dot(z)) >
141+
std::numeric_limits<double>::epsilon()) // i.e. z != 0
102142
t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
103143

104144
x += t2 * z;
@@ -119,11 +159,12 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
119159
}
120160

121161
/* set iai = K \ A */
122-
for (i = 0; i < mi; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
162+
for (i = 0; i < mi; i++)
163+
iai(i) = static_cast<VectorXi::Scalar>(i);
123164

124165
l1:
125166
iter++;
126-
#ifdef TRACE_SOLVER
167+
#ifdef EIQGUADPROG_TRACE_SOLVER
127168
print_vector("x", x, n);
128169
#endif
129170
/* step 1: choose a violated constraint */
@@ -142,11 +183,13 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
142183
s(i) = sum;
143184
psi += std::min(0.0, sum);
144185
}
145-
#ifdef TRACE_SOLVER
186+
#ifdef EIQGUADPROG_TRACE_SOLVER
146187
print_vector("s", s, mi);
147188
#endif
148189

149-
if (std::abs(psi) <= static_cast<double>(mi) * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
190+
if (std::abs(psi) <= static_cast<double>(mi) *
191+
std::numeric_limits<double>::epsilon() * c1 * c2 *
192+
100.0) {
150193
/* numerically there are not infeasibilities anymore */
151194
q = iq;
152195
return f_value;
@@ -176,18 +219,20 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
176219
/* add ip to the active set A */
177220
A(iq) = static_cast<VectorXi::Scalar>(ip);
178221

179-
#ifdef TRACE_SOLVER
222+
#ifdef EIQGUADPROG_TRACE_SOLVER
180223
std::cerr << "Trying with constraint " << ip << std::endl;
181224
print_vector("np", np, n);
182225
#endif
183226

184227
l2a: /* Step 2a: determine step direction */
185-
/* compute z = H np: the step direction in the primal space (through J, see the paper) */
228+
/* compute z = H np: the step direction in the primal space (through J, see
229+
* the paper) */
186230
compute_d(d, J, np);
187231
update_z(z, J, d, iq);
188-
/* compute N* np (if q > 0): the negative of the step direction in the dual space */
232+
/* compute N* np (if q > 0): the negative of the step direction in the dual
233+
* space */
189234
update_r(R, r, d, iq);
190-
#ifdef TRACE_SOLVER
235+
#ifdef EIQGUADPROG_TRACE_SOLVER
191236
std::cerr << "Step direction z" << std::endl;
192237
print_vector("z", z, n);
193238
print_vector("r", r, iq + 1);
@@ -198,7 +243,8 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
198243

199244
/* Step 2b: compute step length */
200245
l = 0;
201-
/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
246+
/* Compute t1: partial step length (maximum step in dual space without
247+
* violating dual feasibility */
202248
t1 = inf; /* +inf */
203249
/* find the index l s.t. it reaches the minimum of u+(x) / r */
204250
for (k = me; k < iq; k++) {
@@ -208,16 +254,19 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
208254
l = A(k);
209255
}
210256
}
211-
/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
212-
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
257+
/* Compute t2: full step length (minimum step in primal space such that the
258+
* constraint ip becomes feasible */
259+
if (std::abs(z.dot(z)) >
260+
std::numeric_limits<double>::epsilon()) // i.e. z != 0
213261
t2 = -s(ip) / z.dot(np);
214262
else
215263
t2 = inf; /* +inf */
216264

217265
/* the step is chosen as the minimum of t1 and t2 */
218266
t = std::min(t1, t2);
219-
#ifdef TRACE_SOLVER
220-
std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
267+
#ifdef EIQGUADPROG_TRACE_SOLVER
268+
std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
269+
<< ") ";
221270
#endif
222271

223272
/* Step 2c: determine new S-pair and take step: */
@@ -236,7 +285,7 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
236285
u(iq) += t;
237286
iai(l) = static_cast<VectorXi::Scalar>(l);
238287
delete_constraint(R, J, A, u, p, iq, l);
239-
#ifdef TRACE_SOLVER
288+
#ifdef EIQGUADPROG_TRACE_SOLVER
240289
std::cerr << " in dual space: " << f_value << std::endl;
241290
print_vector("x", x, n);
242291
print_vector("z", z, n);
@@ -253,7 +302,7 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
253302

254303
u.head(iq) -= t * r.head(iq);
255304
u(iq) += t;
256-
#ifdef TRACE_SOLVER
305+
#ifdef EIQGUADPROG_TRACE_SOLVER
257306
std::cerr << " in both spaces: " << f_value << std::endl;
258307
print_vector("x", x, n);
259308
print_vector("u", u, iq + 1);
@@ -262,7 +311,7 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
262311
#endif
263312

264313
if (t == t2) {
265-
#ifdef TRACE_SOLVER
314+
#ifdef EIQGUADPROG_TRACE_SOLVER
266315
std::cerr << "Full step has taken " << t << std::endl;
267316
print_vector("x", x, n);
268317
#endif
@@ -271,11 +320,12 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
271320
if (!add_constraint(R, J, d, iq, R_norm)) {
272321
iaexcl(ip) = 0;
273322
delete_constraint(R, J, A, u, p, iq, ip);
274-
#ifdef TRACE_SOLVER
323+
#ifdef EIQGUADPROG_TRACE_SOLVER
275324
print_matrix("R", R, n);
276325
print_vector("A", A, iq);
277326
#endif
278-
for (i = 0; i < m; i++) iai(i) = static_cast<VectorXi::Scalar>(i);
327+
for (i = 0; i < m; i++)
328+
iai(i) = static_cast<VectorXi::Scalar>(i);
279329
for (i = 0; i < iq; i++) {
280330
A(i) = A_old(i);
281331
iai(A(i)) = -1;
@@ -285,37 +335,38 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
285335
goto l2; /* go to step 2 */
286336
} else
287337
iai(ip) = -1;
288-
#ifdef TRACE_SOLVER
338+
#ifdef EIQGUADPROG_TRACE_SOLVER
289339
print_matrix("R", R, n);
290340
print_vector("A", A, iq);
291341
#endif
292342
goto l1;
293343
}
294344

295345
/* a patial step has taken */
296-
#ifdef TRACE_SOLVER
346+
#ifdef EIQGUADPROG_TRACE_SOLVER
297347
std::cerr << "Partial step has taken " << t << std::endl;
298348
print_vector("x", x, n);
299349
#endif
300350
/* drop constraint l */
301351
iai(l) = static_cast<VectorXi::Scalar>(l);
302352
delete_constraint(R, J, A, u, p, iq, l);
303-
#ifdef TRACE_SOLVER
353+
#ifdef EIQGUADPROG_TRACE_SOLVER
304354
print_matrix("R", R, n);
305355
print_vector("A", A, iq);
306356
#endif
307357

308358
s(ip) = CI.col(ip).dot(x) + ci0(ip);
309359

310-
#ifdef TRACE_SOLVER
360+
#ifdef EIQGUADPROG_TRACE_SOLVER
311361
print_vector("s", s, mi);
312362
#endif
313363
goto l2a;
314364
}
315365

316-
bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm) {
366+
bool add_constraint(MatrixXd &R, MatrixXd &J, VectorXd &d, size_t &iq,
367+
double &R_norm) {
317368
size_t n = J.rows();
318-
#ifdef TRACE_SOLVER
369+
#ifdef EIQGUADPROG_TRACE_SOLVER
319370
std::cerr << "Add constraint " << iq << '/';
320371
#endif
321372
size_t j, k;
@@ -337,7 +388,8 @@ bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R
337388
cc = d(j - 1);
338389
ss = d(j);
339390
h = distance(cc, ss);
340-
if (h == 0.0) continue;
391+
if (h == 0.0)
392+
continue;
341393
d(j) = 0.0;
342394
ss = ss / h;
343395
cc = cc / h;
@@ -361,7 +413,7 @@ bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R
361413
into column iq - 1 of R
362414
*/
363415
R.col(iq - 1).head(iq) = d.head(iq);
364-
#ifdef TRACE_SOLVER
416+
#ifdef EIQGUADPROG_TRACE_SOLVER
365417
std::cerr << iq << std::endl;
366418
#endif
367419

@@ -372,9 +424,10 @@ bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R
372424
return true;
373425
}
374426

375-
void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t p, size_t& iq, size_t l) {
427+
void delete_constraint(MatrixXd &R, MatrixXd &J, VectorXi &A, VectorXd &u,
428+
size_t p, size_t &iq, size_t l) {
376429
size_t n = R.rows();
377-
#ifdef TRACE_SOLVER
430+
#ifdef EIQGUADPROG_TRACE_SOLVER
378431
std::cerr << "Delete constraint " << l << ' ' << iq;
379432
#endif
380433
size_t i, j, k, qq = 0;
@@ -398,20 +451,23 @@ void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_
398451
u(iq - 1) = u(iq);
399452
A(iq) = 0;
400453
u(iq) = 0.0;
401-
for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0;
454+
for (j = 0; j < iq; j++)
455+
R(j, iq - 1) = 0.0;
402456
/* constraint has been fully removed */
403457
iq--;
404-
#ifdef TRACE_SOLVER
458+
#ifdef EIQGUADPROG_TRACE_SOLVER
405459
std::cerr << '/' << iq << std::endl;
406460
#endif
407461

408-
if (iq == 0) return;
462+
if (iq == 0)
463+
return;
409464

410465
for (j = qq; j < iq; j++) {
411466
cc = R(j, j);
412467
ss = R(j + 1, j);
413468
h = distance(cc, ss);
414-
if (h == 0.0) continue;
469+
if (h == 0.0)
470+
continue;
415471
cc = cc / h;
416472
ss = ss / h;
417473
R(j + 1, j) = 0.0;
@@ -438,5 +494,5 @@ void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_
438494
}
439495
}
440496

441-
} // namespace solvers
442-
} // namespace eiquadprog
497+
} // namespace solvers
498+
} // namespace eiquadprog

0 commit comments

Comments
 (0)