GNU Octave  4.0.0
A high-level interpreted language, primarily intended for numerical computations, mostly compatible with Matlab
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Pages
__qp__.cc
Go to the documentation of this file.
1 /*
2 
3 Copyright (C) 2000-2015 Gabriele Pannocchia
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
9 Free Software Foundation; either version 3 of the License, or (at your
10 option) any later version.
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
18 along with Octave; see the file COPYING. If not, see
19 <http://www.gnu.org/licenses/>.
20 
21 */
22 
23 #ifdef HAVE_CONFIG_H
24 #include <config.h>
25 #endif
26 
27 #include <cfloat>
28 
29 #include "dbleCHOL.h"
30 #include "dbleSVD.h"
31 #include "mx-m-dm.h"
32 #include "EIG.h"
33 
34 #include "defun.h"
35 #include "error.h"
36 #include "gripes.h"
37 #include "oct-obj.h"
38 #include "pr-output.h"
39 #include "utils.h"
40 
41 static Matrix
42 null (const Matrix& A, octave_idx_type& rank)
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.extract_diag ();
55 
56  Matrix V = A_svd.right_singular_matrix ();
57 
58  octave_idx_type A_nr = A.rows ();
59  octave_idx_type A_nc = A.cols ();
60 
61  octave_idx_type tmp = A_nr > A_nc ? A_nr : A_nc;
62 
63  double tol = tmp * s(0) * std::numeric_limits<double>::epsilon ();
64 
65  octave_idx_type n = s.length ();
66 
67  for (octave_idx_type i = 0; i < n; i++)
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);
77 
78  for (octave_idx_type i = 0; i < retval.numel (); i++)
79  if (std::abs (retval(i)) < std::numeric_limits<double>::epsilon ())
80  retval(i) = 0;
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 (std::numeric_limits<double>::epsilon ());
98 
99  // Problem dimension.
100  octave_idx_type n = x.length ();
101 
102  // Dimension of constraints.
103  octave_idx_type n_eq = beq.length ();
104  octave_idx_type n_in = bin.length ();
105 
106  // Filling the current active set.
107 
108  octave_idx_type n_act = n_eq;
109 
110  octave_idx_type n_tot = n_eq + n_in;
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 
123  for (octave_idx_type i = 0; i < n_in; i++)
124  {
125  res(i) /= (1.0 + std::abs (bin(i)));
126 
127  if (res(i) < rtol)
128  {
129  n_act++;
130  Aact = Aact.stack (Ain.row (i));
131  bact.resize (n_act, bin(i));
132  Wact.resize (n_act-n_eq, i);
133  }
134  }
135  }
136 
137  // Computing the ???
138 
139  EIG eigH (H);
140 
141  if (error_state)
142  {
143  error ("qp: failed to compute eigenvalues of H");
144  return -1;
145  }
146 
147  ColumnVector eigenvalH = real (eigH.eigenvalues ());
148  Matrix eigenvecH = real (eigH.eigenvectors ());
149  double minReal = eigenvalH.min ();
150  octave_idx_type indminR = 0;
151  for (octave_idx_type i = 0; i < n; i++)
152  {
153  if (minReal == eigenvalH(i))
154  {
155  indminR = i;
156  break;
157  }
158  }
159 
160  bool done = false;
161 
162  double alpha = 0.0;
163 
164  Matrix R;
165  Matrix Y (n, 0, 0.0);
166 
167  ColumnVector g (n, 0.0);
168  ColumnVector p (n, 0.0);
169 
170  ColumnVector lambda_tmp (n_in, 0.0);
171 
172  while (! done)
173  {
174  iter++;
175 
176  // Current Gradient
177  // g = q + H * x;
178 
179  g = q + H * x;
180 
181  if (n_act == 0)
182  {
183  // There are no active constraints.
184 
185  if (minReal > 0.0)
186  {
187  // Inverting the Hessian. Using the Cholesky
188  // factorization since the Hessian is positive
189  // definite.
190 
191  CHOL cholH (H);
192 
193  R = cholH.chol_matrix ();
194 
195  Matrix Hinv = chol2inv (R);
196 
197  // Computing the unconstrained step.
198  // p = -Hinv * g;
199 
200  p = -Hinv * g;
201 
202  info = 0;
203  }
204  else
205  {
206  // Finding the negative curvature of H.
207 
208  p = eigenvecH.column (indminR);
209 
210  // Following the negative curvature of H.
211 
212  if (p.transpose () * g > std::numeric_limits<double>::epsilon ())
213  p = -p;
214 
215  info = 1;
216  }
217 
218  // Multipliers are zero.
219  lambda_tmp.fill (0.0);
220  }
221  else
222  {
223  // There are active constraints.
224 
225  // Computing the null space.
226 
227  octave_idx_type rank;
228 
229  Matrix Z = null (Aact, rank);
230 
231  octave_idx_type dimZ = n - rank;
232 
233  // FIXME: still remain to handle the case of
234  // non-full rank active set matrix.
235 
236  // Computing the Y matrix (orthogonal to Z)
237  Y = Aact.pseudo_inverse ();
238 
239  // Reduced Hessian
240  Matrix Zt = Z.transpose ();
241  Matrix rH = Zt * H * Z;
242 
243  octave_idx_type pR = 0;
244 
245  if (dimZ > 0)
246  {
247  // Computing the Cholesky factorization (pR = 0 means
248  // that the reduced Hessian was positive definite).
249 
250  CHOL cholrH (rH, pR);
251  Matrix tR = cholrH.chol_matrix ();
252  if (pR == 0)
253  R = tR;
254  }
255 
256  if (pR == 0)
257  {
258  info = 0;
259 
260  // Computing the step pz.
261  if (dimZ > 0)
262  {
263  // Using the Cholesky factorization to invert rH
264 
265  Matrix rHinv = chol2inv (R);
266 
267  ColumnVector pz = -rHinv * Zt * g;
268 
269  // Global step.
270  p = Z * pz;
271  }
272  else
273  {
274  // Global step.
275  p.fill (0.0);
276  }
277  }
278  else
279  {
280  info = 1;
281 
282  // Searching for the most negative curvature.
283 
284  EIG eigrH (rH);
285 
286  if (error_state)
287  {
288  error ("qp: failed to compute eigenvalues of rH");
289  return -1;
290  }
291 
292  ColumnVector eigenvalrH = real (eigrH.eigenvalues ());
293  Matrix eigenvecrH = real (eigrH.eigenvectors ());
294  double mRrH = eigenvalrH.min ();
295  indminR = 0;
296  for (octave_idx_type i = 0; i < n; i++)
297  {
298  if (mRrH == eigenvalH(i))
299  {
300  indminR = i;
301  break;
302  }
303  }
304 
305  ColumnVector eVrH = eigenvecrH.column (indminR);
306 
307  // Computing the step pz.
308  p = Z * eVrH;
309 
310  if (p.transpose () * g > std::numeric_limits<double>::epsilon ())
311  p = -p;
312  }
313  }
314 
315  // Checking the step-size.
316  ColumnVector abs_p (n);
317  for (octave_idx_type i = 0; i < n; i++)
318  abs_p(i) = std::abs (p(i));
319  double max_p = abs_p.max ();
320 
321  if (max_p < rtol)
322  {
323  // The step is null. Checking constraints.
324  if (n_act - n_eq == 0)
325  // Solution is found because no inequality
326  // constraints are active.
327  done = true;
328  else
329  {
330  // Computing the multipliers only for the inequality
331  // constraints that are active. We do NOT compute
332  // multipliers for the equality constraints.
333  Matrix Yt = Y.transpose ();
334  Yt = Yt.extract_n (n_eq, 0, n_act-n_eq, n);
335  lambda_tmp = Yt * (g + H * p);
336 
337  // Checking the multipliers. We remove the most
338  // negative from the set (if any).
339  double min_lambda = lambda_tmp.min ();
340  if (min_lambda >= 0)
341  {
342  // Solution is found.
343  done = true;
344  }
345  else
346  {
347  octave_idx_type which_eig = 0;
348  for (octave_idx_type i = 0; i < n_act; i++)
349  {
350  if (lambda_tmp(i) == min_lambda)
351  {
352  which_eig = i;
353  break;
354  }
355  }
356 
357  // At least one multiplier is negative, we
358  // remove it from the set.
359 
360  n_act--;
361  for (octave_idx_type i = which_eig; i < n_act - n_eq; i++)
362  {
363  Wact(i) = Wact(i+1);
364  for (octave_idx_type j = 0; j < n; j++)
365  Aact(n_eq+i,j) = Aact(n_eq+i+1,j);
366  bact(n_eq+i) = bact(n_eq+i+1);
367  }
368 
369  // Resizing the active set.
370  Wact.resize (n_act-n_eq);
371  bact.resize (n_act);
372  Aact.resize (n_act, n);
373  }
374  }
375  }
376  else
377  {
378  // The step is not null.
379  if (n_act - n_eq == n_in)
380  {
381  // All inequality constraints were active. We can
382  // add the whole step.
383  x += p;
384  }
385  else
386  {
387  // Some constraints were not active. Checking if
388  // there is a blocking constraint.
389  alpha = 1.0;
390  octave_idx_type is_block = -1;
391 
392  for (octave_idx_type i = 0; i < n_in; i++)
393  {
394  bool found = false;
395 
396  for (octave_idx_type j = 0; j < n_act-n_eq; j++)
397  {
398  if (Wact(j) == i)
399  {
400  found = true;
401  break;
402  }
403  }
404 
405  if (! found)
406  {
407  // The i-th constraint was not in the set. Is it a
408  // blocking constraint?
409 
410  RowVector tmp_row = Ain.row (i);
411  double tmp = tmp_row * p;
412  double res = tmp_row * x;
413 
414  if (tmp < 0.0)
415  {
416  double alpha_tmp = (bin(i) - res) / tmp;
417 
418  if (alpha_tmp < alpha)
419  {
420  alpha = alpha_tmp;
421  is_block = i;
422  }
423  }
424  }
425  }
426 
427  // In is_block there is the index of the blocking
428  // constraint (if any).
429  if (is_block >= 0)
430  {
431  // There is a blocking constraint (index in
432  // is_block) which is added to the active set.
433  n_act++;
434  Aact = Aact.stack (Ain.row (is_block));
435  bact.resize (n_act, bin(is_block));
436  Wact.resize (n_act-n_eq, is_block);
437 
438  // Adding the reduced step
439  x += alpha * p;
440  }
441  else
442  {
443  // There are no blocking constraints. Adding the
444  // whole step.
445  x += alpha * p;
446  }
447  }
448  }
449 
450  if (iter == maxit)
451  {
452  done = true;
453  // warning ("qp_main: maximum number of iteration reached");
454  info = 3;
455  }
456  }
457 
458  lambda_tmp = Y.transpose () * (g + H * p);
459 
460  // Reordering the Lagrange multipliers.
461 
462  lambda.resize (n_tot);
463  lambda.fill (0.0);
464  for (octave_idx_type i = 0; i < n_eq; i++)
465  lambda(i) = lambda_tmp(i);
466 
467  for (octave_idx_type i = n_eq; i < n_tot; i++)
468  {
469  for (octave_idx_type j = 0; j < n_act-n_eq; j++)
470  {
471  if (Wact(j) == i - n_eq)
472  {
473  lambda(i) = lambda_tmp(n_eq+j);
474  break;
475  }
476  }
477  }
478 
479  return info;
480 }
481 
482 DEFUN (__qp__, args, ,
483  "-*- texinfo -*-\n\
484 @deftypefn {Built-in 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\
485 Undocumented internal function.\n\
486 @end deftypefn")
487 {
488  octave_value_list retval;
489 
490  if (args.length () == 8)
491  {
492  const ColumnVector x0 (args(0) . vector_value ());
493  const Matrix H (args(1) . matrix_value ());
494  const ColumnVector q (args(2) . vector_value ());
495  const Matrix Aeq (args(3) . matrix_value ());
496  const ColumnVector beq (args(4) . vector_value ());
497  const Matrix Ain (args(5) . matrix_value ());
498  const ColumnVector bin (args(6) . vector_value ());
499  const int maxit (args(7) . int_value ());
500 
501  if (! error_state)
502  {
503  int iter = 0;
504 
505  // Copying the initial guess in the working variable
506  ColumnVector x = x0;
507 
508  // Reordering the Lagrange multipliers
509  ColumnVector lambda;
510 
511  int info = qp (H, q, Aeq, beq, Ain, bin, maxit, x, lambda, iter);
512 
513  if (! error_state)
514  {
515  retval(3) = iter;
516  retval(2) = info;
517  retval(1) = lambda;
518  retval(0) = x;
519  }
520  else
521  error ("qp: internal error");
522  }
523  else
524  error ("__qp__: invalid arguments");
525  }
526  else
527  print_usage ();
528 
529  return retval;
530 }
531 
532 /*
533 ## No test needed for internal helper function.
534 %!assert (1)
535 */
DiagMatrix singular_values(void) const
Definition: dbleSVD.h:86
ColumnVector & fill(double val)
Definition: dColVector.cc:93
static int qp(const Matrix &H, const ColumnVector &q, const Matrix &Aeq, const ColumnVector &beq, const Matrix &Ain, const ColumnVector &bin, int maxit, ColumnVector &x, ColumnVector &lambda, int &iter)
Definition: __qp__.cc:87
bool is_empty(void) const
Definition: Array.h:472
Matrix right_singular_matrix(void) const
Definition: dbleSVD.cc:71
void resize(octave_idx_type nr, octave_idx_type nc, double rfv=0)
Definition: dMatrix.h:130
Matrix extract(octave_idx_type r1, octave_idx_type c1, octave_idx_type r2, octave_idx_type c2) const
Definition: dMatrix.cc:620
OCTINTERP_API void print_usage(void)
Definition: defun.cc:51
octave_idx_type numel(void) const
Number of elements in the array.
Definition: Array.h:275
Definition: EIG.h:32
F77_RET_T const octave_idx_type Complex * A
Definition: CmplxGEPBAL.cc:39
Matrix extract_n(octave_idx_type r1, octave_idx_type c1, octave_idx_type nr, octave_idx_type nc) const
Definition: dMatrix.cc:630
#define DEFUN(name, args_name, nargout_name, doc)
Definition: defun.h:44
ComplexMatrix eigenvectors(void) const
Definition: EIG.h:112
void error(const char *fmt,...)
Definition: error.cc:476
RowVector row(octave_idx_type i) const
Definition: dMatrix.cc:639
F77_RET_T const octave_idx_type const octave_idx_type const octave_idx_type double const octave_idx_type double const octave_idx_type double const octave_idx_type double * Z
Definition: qz.cc:114
ColumnVector extract_diag(octave_idx_type k=0) const
Definition: dDiagMatrix.h:105
Matrix chol_matrix(void) const
Definition: dbleCHOL.h:64
octave_idx_type rows(void) const
Definition: Array.h:313
RowVector transpose(void) const
Definition: dColVector.cc:144
ComplexColumnVector eigenvalues(void) const
Definition: EIG.h:110
static Matrix null(const Matrix &A, octave_idx_type &rank)
Definition: __qp__.cc:42
ComplexMatrix chol2inv(const ComplexMatrix &r)
Definition: CmplxCHOL.cc:443
int error_state
Definition: error.cc:101
Matrix transpose(void) const
Definition: dMatrix.h:114
Array< T > column(octave_idx_type k) const
Extract column: A(:,k+1).
Definition: Array.cc:258
Definition: dMatrix.h:35
Definition: dbleSVD.h:31
octave_idx_type length(void) const
Number of elements in the array.
Definition: Array.h:267
double min(void) const
Definition: dColVector.cc:268
Matrix stack(const Matrix &a) const
Definition: dMatrix.cc:532
double max(void) const
Definition: dColVector.cc:284
F77_RET_T const octave_idx_type const octave_idx_type const octave_idx_type const double const double octave_idx_type double * V
Definition: CmplxGEPBAL.cc:49
Definition: dbleCHOL.h:32
ColumnVector real(const ComplexColumnVector &a)
Definition: dColVector.cc:156
octave_idx_type cols(void) const
Definition: Array.h:321
T abs(T x)
Definition: pr-output.cc:3062
Matrix pseudo_inverse(double tol=0.0) const
Definition: dMatrix.cc:870
F77_RET_T const double * x
void resize(octave_idx_type n, const double &rfv=0)
Definition: dColVector.h:106