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