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