|
5290
|
1 /* |
|
|
2 |
|
7361
|
3 Copyright (C) 2000, 2001, 2004, 2005, 2006, 2007, 2008 Gabriele Pannocchia |
|
5290
|
4 |
|
|
5 This file is part of Octave. |
|
|
6 |
|
|
7 Octave is free software; you can redistribute it and/or modify it |
|
|
8 under the terms of the GNU General Public License as published by the |
|
7016
|
9 Free Software Foundation; either version 3 of the License, or (at your |
|
|
10 option) any later version. |
|
5290
|
11 |
|
|
12 Octave is distributed in the hope that it will be useful, but WITHOUT |
|
|
13 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or |
|
|
14 FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License |
|
|
15 for more details. |
|
|
16 |
|
|
17 You should have received a copy of the GNU General Public License |
|
7016
|
18 along with Octave; see the file COPYING. If not, see |
|
|
19 <http://www.gnu.org/licenses/>. |
|
5290
|
20 |
|
|
21 */ |
|
|
22 |
|
5293
|
23 #ifdef HAVE_CONFIG_H |
|
|
24 #include <config.h> |
|
|
25 #endif |
|
|
26 |
|
5290
|
27 #include <cfloat> |
|
|
28 |
|
5293
|
29 #include "dbleCHOL.h" |
|
|
30 #include "dbleSVD.h" |
|
|
31 #include "mx-m-dm.h" |
|
|
32 #include "EIG.h" |
|
5290
|
33 |
|
5293
|
34 #include "defun-dld.h" |
|
|
35 #include "error.h" |
|
|
36 #include "gripes.h" |
|
|
37 #include "oct-obj.h" |
|
|
38 #include "pr-output.h" |
|
|
39 #include "utils.h" |
|
5290
|
40 |
|
|
41 static Matrix |
|
5295
|
42 null (const Matrix& A, octave_idx_type& rank) |
|
5290
|
43 { |
|
|
44 Matrix retval; |
|
|
45 |
|
|
46 rank = 0; |
|
|
47 |
|
|
48 if (! A.is_empty ()) |
|
|
49 { |
|
|
50 SVD A_svd (A); |
|
|
51 |
|
|
52 DiagMatrix S = A_svd.singular_values (); |
|
|
53 |
|
|
54 ColumnVector s = S.diag (); |
|
|
55 |
|
|
56 Matrix V = A_svd.right_singular_matrix (); |
|
|
57 |
|
5295
|
58 octave_idx_type A_nr = A.rows (); |
|
|
59 octave_idx_type A_nc = A.cols (); |
|
5290
|
60 |
|
5295
|
61 octave_idx_type tmp = A_nr > A_nc ? A_nr : A_nc; |
|
5290
|
62 |
|
|
63 double tol = tmp * s(0) * DBL_EPSILON; |
|
|
64 |
|
5295
|
65 octave_idx_type n = s.length (); |
|
5290
|
66 |
|
5295
|
67 for (octave_idx_type i = 0; i < n; i++) |
|
5290
|
68 { |
|
|
69 if (s(i) > tol) |
|
|
70 rank++; |
|
|
71 } |
|
|
72 |
|
|
73 if (rank < A_nc) |
|
|
74 retval = V.extract (0, rank, A_nc-1, A_nc-1); |
|
|
75 else |
|
|
76 retval.resize (A_nc, 0); |
|
6431
|
77 |
|
|
78 for (octave_idx_type i = 0; i < retval.numel (); i++) |
|
|
79 if (std::abs (retval(i)) < DBL_EPSILON) |
|
|
80 retval(i) = 0; |
|
5290
|
81 } |
|
|
82 |
|
|
83 return retval; |
|
|
84 } |
|
|
85 |
|
|
86 static int |
|
|
87 qp (const Matrix& H, const ColumnVector& q, |
|
|
88 const Matrix& Aeq, const ColumnVector& beq, |
|
|
89 const Matrix& Ain, const ColumnVector& bin, |
|
|
90 int maxit, |
|
|
91 ColumnVector& x, ColumnVector& lambda, int& iter) |
|
|
92 { |
|
|
93 int info = 0; |
|
|
94 |
|
|
95 iter = 0; |
|
|
96 |
|
|
97 double rtol = sqrt (DBL_EPSILON); |
|
|
98 |
|
|
99 // Problem dimension. |
|
5295
|
100 octave_idx_type n = x.length (); |
|
5290
|
101 |
|
|
102 // Dimension of constraints. |
|
5295
|
103 octave_idx_type n_eq = beq.length (); |
|
|
104 octave_idx_type n_in = bin.length (); |
|
5290
|
105 |
|
|
106 // Filling the current active set. |
|
|
107 |
|
5295
|
108 octave_idx_type n_act = n_eq; |
|
5290
|
109 |
|
5295
|
110 octave_idx_type n_tot = n_eq + n_in; |
|
5290
|
111 |
|
|
112 // Equality constraints come first. We won't check the sign of the |
|
|
113 // Lagrange multiplier for those. |
|
|
114 |
|
|
115 Matrix Aact = Aeq; |
|
|
116 ColumnVector bact = beq; |
|
|
117 ColumnVector Wact; |
|
|
118 |
|
|
119 if (n_in > 0) |
|
|
120 { |
|
|
121 ColumnVector res = Ain*x - bin; |
|
|
122 |
|
5295
|
123 for (octave_idx_type i = 0; i < n_in; i++) |
|
5290
|
124 { |
|
6431
|
125 res(i) /= (1.0 + std::abs (bin(i))); |
|
5290
|
126 |
|
|
127 if (res(i) < rtol) |
|
|
128 { |
|
|
129 n_act++; |
|
|
130 Aact = Aact.stack (Ain.row (i)); |
|
|
131 bact.resize (n_act, bin(i)); |
|
6848
|
132 Wact.resize (n_act-n_eq, i); |
|
5290
|
133 } |
|
|
134 } |
|
|
135 } |
|
|
136 |
|
|
137 // Computing the ??? |
|
|
138 |
|
|
139 EIG eigH (H); |
|
|
140 ColumnVector eigenvalH = real (eigH.eigenvalues ()); |
|
|
141 Matrix eigenvecH = real (eigH.eigenvectors ()); |
|
|
142 double minReal = eigenvalH.min (); |
|
5295
|
143 octave_idx_type indminR = 0; |
|
|
144 for (octave_idx_type i = 0; i < n; i++) |
|
5290
|
145 { |
|
|
146 if (minReal == eigenvalH(i)) |
|
|
147 { |
|
|
148 indminR = i; |
|
|
149 break; |
|
|
150 } |
|
|
151 } |
|
|
152 |
|
|
153 bool done = false; |
|
|
154 |
|
|
155 double alpha = 0.0; |
|
|
156 |
|
|
157 Matrix R; |
|
|
158 Matrix Y (n, 0, 0.0); |
|
|
159 |
|
|
160 ColumnVector g (n, 0.0); |
|
|
161 ColumnVector p (n, 0.0); |
|
|
162 |
|
|
163 ColumnVector lambda_tmp (n_in, 0.0); |
|
|
164 |
|
|
165 while (! done) |
|
|
166 { |
|
|
167 iter++; |
|
|
168 |
|
|
169 // Current Gradient |
|
|
170 // g = q + H * x; |
|
|
171 |
|
|
172 g = q + H * x; |
|
|
173 |
|
|
174 if (n_act == 0) |
|
|
175 { |
|
|
176 // There are no active constraints. |
|
|
177 |
|
|
178 if (minReal > 0.0) |
|
|
179 { |
|
|
180 // Inverting the Hessian. Using the Cholesky |
|
|
181 // factorization since the Hessian is positive |
|
|
182 // definite. |
|
|
183 |
|
|
184 CHOL cholH (H); |
|
|
185 |
|
|
186 R = cholH.chol_matrix (); |
|
|
187 |
|
5341
|
188 Matrix Hinv = chol2inv (R); |
|
5290
|
189 |
|
|
190 // Computing the unconstrained step. |
|
|
191 // p = -Hinv * g; |
|
|
192 |
|
|
193 p = -Hinv * g; |
|
|
194 |
|
|
195 info = 0; |
|
|
196 } |
|
|
197 else |
|
|
198 { |
|
|
199 // Finding the negative curvature of H. |
|
|
200 |
|
|
201 p = eigenvecH.column (indminR); |
|
|
202 |
|
|
203 // Following the negative curvature of H. |
|
|
204 |
|
|
205 if (p.transpose () * g > DBL_EPSILON) |
|
|
206 p = -p; |
|
|
207 |
|
|
208 info = 1; |
|
|
209 } |
|
|
210 |
|
|
211 // Multipliers are zero. |
|
|
212 lambda_tmp.fill (0.0); |
|
|
213 } |
|
|
214 else |
|
|
215 { |
|
|
216 // There are active constraints. |
|
|
217 |
|
|
218 // Computing the null space. |
|
|
219 |
|
5295
|
220 octave_idx_type rank; |
|
5290
|
221 |
|
|
222 Matrix Z = null (Aact, rank); |
|
|
223 |
|
5295
|
224 octave_idx_type dimZ = n - rank; |
|
5290
|
225 |
|
5775
|
226 // FIXME -- still remain to handle the case of |
|
5290
|
227 // non-full rank active set matrix. |
|
|
228 |
|
7361
|
229 // Computing the Y matrix (orthogonal to Z) |
|
|
230 Y = Aact.pseudo_inverse (); |
|
|
231 |
|
5290
|
232 // Reduced Hessian |
|
|
233 Matrix Zt = Z.transpose (); |
|
|
234 Matrix rH = Zt * H * Z; |
|
|
235 |
|
5295
|
236 octave_idx_type pR = 0; |
|
5290
|
237 |
|
|
238 if (dimZ > 0) |
|
|
239 { |
|
|
240 // Computing the Cholesky factorization (pR = 0 means |
|
|
241 // that the reduced Hessian was positive definite). |
|
|
242 |
|
|
243 CHOL cholrH (rH, pR); |
|
|
244 Matrix tR = cholrH.chol_matrix (); |
|
|
245 if (pR == 0) |
|
|
246 R = tR; |
|
|
247 } |
|
|
248 |
|
|
249 if (pR == 0) |
|
|
250 { |
|
|
251 info = 0; |
|
|
252 |
|
|
253 // Computing the step pz. |
|
|
254 if (dimZ > 0) |
|
|
255 { |
|
|
256 // Using the Cholesky factorization to invert rH |
|
|
257 |
|
5341
|
258 Matrix rHinv = chol2inv (R); |
|
5290
|
259 |
|
|
260 ColumnVector pz = -rHinv * Zt * g; |
|
|
261 |
|
|
262 // Global step. |
|
|
263 p = Z * pz; |
|
|
264 } |
|
|
265 else |
|
|
266 { |
|
|
267 // Global step. |
|
|
268 p.fill (0.0); |
|
|
269 } |
|
|
270 } |
|
|
271 else |
|
|
272 { |
|
|
273 info = 1; |
|
|
274 |
|
|
275 // Searching for the most negative curvature. |
|
|
276 |
|
|
277 EIG eigrH (rH); |
|
|
278 ColumnVector eigenvalrH = real (eigrH.eigenvalues ()); |
|
|
279 Matrix eigenvecrH = real (eigrH.eigenvectors ()); |
|
|
280 double mRrH = eigenvalrH.min (); |
|
|
281 indminR = 0; |
|
5295
|
282 for (octave_idx_type i = 0; i < n; i++) |
|
5290
|
283 { |
|
|
284 if (mRrH == eigenvalH(i)) |
|
|
285 { |
|
|
286 indminR = i; |
|
|
287 break; |
|
|
288 } |
|
|
289 } |
|
|
290 |
|
|
291 ColumnVector eVrH = eigenvecrH.column (indminR); |
|
|
292 |
|
|
293 // Computing the step pz. |
|
|
294 p = Z * eVrH; |
|
|
295 |
|
|
296 if (p.transpose () * g > DBL_EPSILON) |
|
|
297 p = -p; |
|
|
298 } |
|
|
299 } |
|
|
300 |
|
|
301 // Checking the step-size. |
|
|
302 ColumnVector abs_p (n); |
|
5295
|
303 for (octave_idx_type i = 0; i < n; i++) |
|
6431
|
304 abs_p(i) = std::abs (p(i)); |
|
5290
|
305 double max_p = abs_p.max (); |
|
|
306 |
|
|
307 if (max_p < rtol) |
|
|
308 { |
|
|
309 // The step is null. Checking constraints. |
|
|
310 if (n_act - n_eq == 0) |
|
|
311 // Solution is found because no inequality |
|
|
312 // constraints are active. |
|
|
313 done = true; |
|
|
314 else |
|
|
315 { |
|
|
316 // Computing the multipliers only for the inequality |
|
|
317 // constraints that are active. We do NOT compute |
|
|
318 // multipliers for the equality constraints. |
|
|
319 Matrix Yt = Y.transpose (); |
|
|
320 Yt = Yt.extract_n (n_eq, 0, n_act-n_eq, n); |
|
|
321 lambda_tmp = Yt * (g + H * p); |
|
|
322 |
|
|
323 // Checking the multipliers. We remove the most |
|
|
324 // negative from the set (if any). |
|
|
325 double min_lambda = lambda_tmp.min (); |
|
|
326 if (min_lambda >= 0) |
|
|
327 { |
|
|
328 // Solution is found. |
|
|
329 done = true; |
|
|
330 } |
|
|
331 else |
|
|
332 { |
|
5295
|
333 octave_idx_type which_eig = 0; |
|
|
334 for (octave_idx_type i = 0; i < n_act; i++) |
|
5290
|
335 { |
|
|
336 if (lambda_tmp(i) == min_lambda) |
|
|
337 { |
|
|
338 which_eig = i; |
|
|
339 break; |
|
|
340 } |
|
|
341 } |
|
|
342 |
|
|
343 // At least one multiplier is negative, we |
|
|
344 // remove it from the set. |
|
|
345 |
|
5295
|
346 for (octave_idx_type i = which_eig; i < n_act - n_eq; i++) |
|
5290
|
347 { |
|
|
348 Wact(i) = Wact(i+1); |
|
5295
|
349 for (octave_idx_type j = 0; j < n; j++) |
|
5290
|
350 Aact(n_eq+i,j) = Aact(n_eq+i+1,j); |
|
|
351 bact(n_eq+i) = bact(n_eq+i+1); |
|
|
352 } |
|
|
353 n_act--; |
|
|
354 |
|
|
355 // Resizing the active set. |
|
|
356 Wact.resize (n_act-n_eq); |
|
|
357 bact.resize (n_act); |
|
|
358 Aact.resize (n_act, n); |
|
|
359 } |
|
|
360 } |
|
|
361 } |
|
|
362 else |
|
|
363 { |
|
|
364 // The step is not null. |
|
|
365 if (n_act - n_eq == n_in) |
|
|
366 { |
|
|
367 // All inequality constraints were active. We can |
|
|
368 // add the whole step. |
|
|
369 x += p; |
|
|
370 } |
|
|
371 else |
|
|
372 { |
|
|
373 // Some constraints were not active. Checking if |
|
|
374 // there is a blocking constraint. |
|
|
375 alpha = 1.0; |
|
5295
|
376 octave_idx_type is_block = -1; |
|
5290
|
377 |
|
5295
|
378 for (octave_idx_type i = 0; i < n_in; i++) |
|
5290
|
379 { |
|
|
380 bool found = false; |
|
|
381 |
|
5295
|
382 for (octave_idx_type j = 0; j < n_act-n_eq; j++) |
|
5290
|
383 { |
|
7035
|
384 if (Wact(j) == i) |
|
5290
|
385 { |
|
|
386 found = true; |
|
|
387 break; |
|
|
388 } |
|
|
389 } |
|
|
390 |
|
|
391 if (! found) |
|
|
392 { |
|
|
393 // The i-th constraint was not in the set. Is it a |
|
|
394 // blocking constraint? |
|
|
395 |
|
|
396 RowVector tmp_row = Ain.row (i); |
|
|
397 double tmp = tmp_row * p; |
|
|
398 double res = tmp_row * x; |
|
|
399 |
|
|
400 if (tmp < 0.0) |
|
|
401 { |
|
|
402 double alpha_tmp = (bin(i) - res) / tmp; |
|
|
403 |
|
|
404 if (alpha_tmp < alpha) |
|
|
405 { |
|
|
406 alpha = alpha_tmp; |
|
|
407 is_block = i; |
|
|
408 } |
|
|
409 } |
|
|
410 } |
|
|
411 } |
|
|
412 |
|
|
413 // In is_block there is the index of the blocking |
|
|
414 // constraint (if any). |
|
|
415 if (is_block >= 0) |
|
|
416 { |
|
|
417 // There is a blocking constraint (index in |
|
|
418 // is_block) which is added to the active set. |
|
|
419 n_act++; |
|
|
420 Aact = Aact.stack (Ain.row (is_block)); |
|
|
421 bact.resize (n_act, bin(is_block)); |
|
6848
|
422 Wact.resize (n_act-n_eq, is_block); |
|
5290
|
423 |
|
|
424 // Adding the reduced step |
|
|
425 x += alpha * p; |
|
|
426 } |
|
|
427 else |
|
|
428 { |
|
|
429 // There are no blocking constraints. Adding the |
|
|
430 // whole step. |
|
|
431 x += alpha * p; |
|
|
432 } |
|
|
433 } |
|
|
434 } |
|
|
435 |
|
|
436 if (iter == maxit) |
|
|
437 { |
|
|
438 done = true; |
|
|
439 // warning ("qp_main: maximum number of iteration reached"); |
|
|
440 info = 3; |
|
|
441 } |
|
|
442 } |
|
|
443 |
|
|
444 lambda_tmp = Y.transpose () * (g + H * p); |
|
|
445 |
|
|
446 // Reordering the Lagrange multipliers. |
|
|
447 |
|
|
448 lambda.resize (n_tot); |
|
|
449 lambda.fill (0.0); |
|
5295
|
450 for (octave_idx_type i = 0; i < n_eq; i++) |
|
5290
|
451 lambda(i) = lambda_tmp(i); |
|
|
452 |
|
5295
|
453 for (octave_idx_type i = n_eq; i < n_tot; i++) |
|
5290
|
454 { |
|
5295
|
455 for (octave_idx_type j = 0; j < n_act-n_eq; j++) |
|
5290
|
456 { |
|
7035
|
457 if (Wact(j) == i - n_eq) |
|
5290
|
458 { |
|
|
459 lambda(i) = lambda_tmp(n_eq+j); |
|
|
460 break; |
|
|
461 } |
|
|
462 } |
|
|
463 } |
|
|
464 |
|
|
465 return info; |
|
|
466 } |
|
|
467 |
|
|
468 DEFUN_DLD (__qp__, args, , |
|
6945
|
469 "-*- texinfo -*-\n\ |
|
|
470 @deftypefn {Loadable Function} {[@var{x}, @var{lambda}, @var{info}, @var{iter}] =} __qp__ (@var{x0}, @var{H}, @var{q}, @var{Aeq}, @var{beq}, @var{Ain}, @var{bin}, @var{maxit})\n\ |
|
|
471 Undocumented internal function.\n\ |
|
|
472 @end deftypefn") |
|
5290
|
473 { |
|
|
474 octave_value_list retval; |
|
|
475 |
|
|
476 if (args.length () == 8) |
|
|
477 { |
|
|
478 const ColumnVector x0 (args(0) . vector_value ()); |
|
|
479 const Matrix H (args(1) . matrix_value ()); |
|
|
480 const ColumnVector q (args(2) . vector_value ()); |
|
|
481 const Matrix Aeq (args(3) . matrix_value ()); |
|
|
482 const ColumnVector beq (args(4) . vector_value ()); |
|
|
483 const Matrix Ain (args(5) . matrix_value ()); |
|
|
484 const ColumnVector bin (args(6) . vector_value ()); |
|
|
485 const int maxit (args(7) . int_value ()); |
|
|
486 |
|
|
487 if (! error_state) |
|
|
488 { |
|
|
489 int iter = 0; |
|
|
490 |
|
|
491 // Copying the initial guess in the working variable |
|
|
492 ColumnVector x = x0; |
|
|
493 |
|
|
494 // Reordering the Lagrange multipliers |
|
|
495 ColumnVector lambda; |
|
|
496 |
|
|
497 int info = qp (H, q, Aeq, beq, Ain, bin, maxit, x, lambda, iter); |
|
|
498 |
|
|
499 retval(3) = iter; |
|
|
500 retval(2) = info; |
|
|
501 retval(1) = lambda; |
|
|
502 retval(0) = x; |
|
|
503 } |
|
|
504 else |
|
|
505 error ("__qp__: invalid arguments"); |
|
|
506 } |
|
|
507 else |
|
5823
|
508 print_usage (); |
|
5290
|
509 |
|
|
510 return retval; |
|
|
511 } |
