Kernel Quantum Probability Library
The KQP library aims at providing tools for working with quantums probabilities
coneprog.inc.hpp
1 
7 #include <iostream>
8 #include <vector>
9 #include <cmath>
10 #include <boost/scoped_ptr.hpp>
11 #include <boost/shared_ptr.hpp>
12 #include <boost/format.hpp>
13 #include <algorithm>
14 #include <numeric>
15 
16 
17 
18 #include <kqp/kqp.hpp>
19 #include <kqp/coneprog.hpp>
20 
21 #include "Eigen/Core"
22 
23 #include <kqp/define_header_logger.hpp>
24 DEFINE_KQP_HLOGGER("kqp.coneqp")
25 
26 #define KQP_NOT_IMPLEMENTED BOOST_THROW_EXCEPTION(kqp::not_implemented_exception())
27 
28 typedef boost::error_info<struct qp_error_,std::string> qp_error_info;
29 
30 namespace kqp { namespace cvxopt {
31 
32  typedef std::vector<int>::iterator IntIterator;
33 
34  template<typename Scalar>
35  ConeQPOptions<Scalar>::ConeQPOptions() :
36  useCorrection(true),
37  DEBUG(false),
38  correction(true),
39  show_progress(true),
40  maxiters(100),
41  abstol(1e-5),
42  reltol(1e-3),
43  feastol(1e-5),
44  refinement(-1)
45  {
46 
47  }
48 
49  template<typename Scalar>
50  ConeQPReturn<Scalar>::ConeQPReturn() :
51  gap(0), relative_gap(0), primal_objective(0), dual_objective(0), primal_infeasibility(0), dual_infeasibility(0), primal_slack(0), dual_slack(0), iterations(0) {}
52 
53 
54 
56  template<typename Scalar>
57  Scalar sdot(const KQP_VECTOR(Scalar) &x, const KQP_VECTOR(Scalar) &y, const Dimensions &dims, size_t mnl = 0) {
58  size_t ind = mnl + dims.l;
59  for(std::vector<int>::const_iterator k = dims.q.begin(); k != dims.q.end(); k++)
60  ind += *k;
61 
62 
63  Scalar a = x.head(ind).dot(y.head(ind));
64 
65  for(size_t i = 0; i < dims.s.size(); i++) {
66  int m = dims.s[i];
67 
68  (void)m; KQP_NOT_IMPLEMENTED;
69  /*
70  a +=
71  blas.dot(x, y, offsetx = ind, offsety = ind, incx = m+1,
72  incy = m+1, n = m);
73 
74  for(int i = 1; i < m; i++)
75  a += 2.0 * blas.dot(x, y, incx = m+1, incy = m+1,
76  offsetx = ind+j, offsety = ind+j, n = m-j);
77  ind += m * m;
78 
79  */
80  }
81  return a;
82  }
83 
85  template<typename Scalar>
86  Scalar snrm2(const KQP_VECTOR(Scalar) &x, const Dimensions &dims, size_t mnl = 0) {
87  return std::sqrt(sdot(x, x, dims, mnl));
88  }
89 
100  template<typename Scalar>
101  Scalar max_step(KQP_VECTOR(Scalar) &x, const Dimensions &dims, int mnl = 0, const KQP_VECTOR(Scalar) * sigma = NULL) {
102  // Get rid of unused warning
103  (void)sigma;
104 
105  std::vector<Scalar> t;
106  int ind = mnl + dims.l;
107  if (ind > 0)
108  t.push_back(- x.topRows(ind).minCoeff() );
109 
110  if (!dims.q.empty()) {
111  KQP_NOT_IMPLEMENTED;
112  // for(size_t i = 0; i < dims.q.size(); i++) { int m = dims.q[i];
113  // if (m > 0)
114  // t += [ blas.nrm2(x, offset = ind + 1, n = m-1) - x[ind] ];
115  // ind += m;
116  // }
117  //
118  }
119 
120  if (!dims.s.empty()) {
121  KQP_NOT_IMPLEMENTED;
122  // if (sigma == NULL) {
123  // Q = matrix(0.0, (max(dims['s']), max(dims['s'])));
124  // w = matrix(0.0, (max(dims['s']),1));
125  // }
126 
127  // int ind2 = 0;
128  // for(int m : dims.s) {
129  // if sigma is None:
130  // blas.copy(x, Q, offsetx = ind, n = m**2)
131  // lapack.syevr(Q, w, range = 'I', il = 1, iu = 1, n = m, ldA = m)
132  // if m: t += [ -w[0] ]
133  // else:
134  // lapack.syevd(x, sigma, jobz = 'V', n = m, ldA = m, offsetA =
135  // ind, offsetW = ind2)
136  // if m: t += [ -sigma[ind2] ]
137  // ind += m*m
138  // ind2 += m
139  // }
140 
141  }
142 
143  if (!t.empty()) return *std::max_element(t.begin(), t.end());
144  return 0.0;
145  }
146 
147  /*
148  Applies Nesterov-Todd scaling or its inverse.
149 
150  Computes
151 
152  x := W*x (trans is 'N', inverse = 'N')
153  x := W^T*x (trans is 'T', inverse = 'N')
154  x := W^{-1}*x (trans is 'N', inverse = 'I')
155  x := W^{-T}*x (trans is 'T', inverse = 'I').
156 
157  x is a dense 'd' matrix.
158 
159 
160  The 'dnl' and 'dnli' entries are optional, and only present when the
161  function is called from the nonlinear solver.
162 
163  */
164  template <typename Scalar, int ColsAtCompileTime>
165  void scale(Eigen::Matrix<Scalar, Dynamic, ColsAtCompileTime> &x, const ScalingMatrix<Scalar> &W, bool trans = false, bool inverse = false) {
166  (void)trans;
167 
168  size_t ind = 0;
169 
170  // Scaling for nonlinear component xk is xk := dnl .* xk; inverse
171  // scaling is xk ./ dnl = dnli .* xk, where dnl = W['dnl'],
172  // dnli = W['dnli'].
173 
174  if (W.dnl.rows() > 0) {
175  const KQP_VECTOR(Scalar) &w = inverse ? W.dnli : W.dnl;
176  x.block(0, 0, w.rows(), x.cols()) = w.asDiagonal() * x.block(0, 0, w.rows(), x.cols());
177  ind += w.rows();
178  }
179 
180  // Scaling for linear 'l' component xk is xk := d .* xk; inverse
181  // scaling is xk ./ d = di .* xk, where d = W['d'], di = W['di'].
182 
183  const KQP_VECTOR(Scalar) &w = inverse ? W.di : W.d;
184  x.block(ind, 0, w.rows(), x.cols()).noalias() = w.asDiagonal() * x.block(ind, 0, w.rows(), x.cols());
185  ind += w.rows();
186 
187 
188  // Scaling for 'q' component is
189  //
190  // xk := beta * (2*v*v' - J) * xk
191  // = beta * (2*v*(xk'*v)' - J*xk)
192  //
193  // where beta = W['beta'][k], v = W['v'][k], J = [1, 0; 0, -I].
194  //
195  // Inverse scaling is
196  //
197  // xk := 1/beta * (2*J*v*v'*J - J) * xk
198  // = 1/beta * (-J) * (2*v*((-J*xk)'*v)' + xk).
199 
200  if (!W.beta.empty()) {
201  KQP_NOT_IMPLEMENTED;
202 
203  /*
204 
205  w = matrix(0.0, (x.size[1], 1));
206 
207  for k in xrange(len(W['v'])) {
208  v = W['v'][k]
209  m = v.size[0]
210  if inverse == 'I':
211  blas.scal(-1.0, x, offset = ind, inc = x.size[0])
212  blas.gemv(x, v, w, trans = 'T', m = m, n = x.size[1], offsetA =
213  ind, ldA = x.size[0])
214  blas.scal(-1.0, x, offset = ind, inc = x.size[0])
215  blas.ger(v, w, x, alpha = 2.0, m = m, n = x.size[1], ldA =
216  x.size[0], offsetA = ind)
217  if inverse == 'I':
218  blas.scal(-1.0, x, offset = ind, inc = x.size[0])
219  a = 1.0 / W['beta'][k]
220  else:
221  a = W['beta'][k]
222  for i in xrange(x.size[1]):
223  blas.scal(a, x, n = m, offset = ind + i*x.size[0])
224  ind += m
225  }
226  */
227  }
228 
229  // Scaling for 's' component xk is
230  //
231  // xk := vec( r' * mat(xk) * r ) if trans = 'N'
232  // xk := vec( r * mat(xk) * r' ) if trans = 'T'.
233  //
234  // r is kth element of W['r'].
235  //
236  // Inverse scaling is
237  //
238  // xk := vec( rti * mat(xk) * rti' ) if trans = 'N'
239  // xk := vec( rti' * mat(xk) * rti ) if trans = 'T'.
240  //
241  // rti is kth element of W['rti'].
242 
243  if (!W.r.empty()) {
244  KQP_NOT_IMPLEMENTED;
245  /*
246  maxn = max( [0] + [ r.size[0] for r in W['r'] ] )
247  a = matrix(0.0, (maxn, maxn))
248  for k in xrange(len(W['r'])) {
249  bool t;
250 
251  if (inverse == 'N') {
252  r = W['r'][k];
253  t = !trans;
254  }else {
255  r = W['rti'][k];
256  t = trans;
257  }
258 
259  n = r.size[0]
260  for i in xrange(x.size[1]) {
261 
262  // scale diagonal of xk by 0.5
263  blas.scal(0.5, x, offset = ind + i*x.size[0], inc = n+1, n = n)
264 
265  // a = r*tril(x) (t is 'N') or a = tril(x)*r (t is 'T')
266  blas.copy(r, a)
267  if t == 'N':
268  blas.trmm(x, a, side = 'R', m = n, n = n, ldA = n, ldB = n,
269  offsetA = ind + i*x.size[0])
270  else:
271  blas.trmm(x, a, side = 'L', m = n, n = n, ldA = n, ldB = n,
272  offsetA = ind + i*x.size[0])
273 
274  // x := (r*a' + a*r') if t is 'N'
275  // x := (r'*a + a'*r) if t is 'T'
276  blas.syr2k(r, a, x, trans = t, n = n, k = n, ldB = n, ldC = n,
277  offsetC = ind + i*x.size[0])
278  }
279  ind += n**2
280 
281  }
282  */
283  }
284 
285  }
286 
287 
288 
289  template<typename Scalar>
290  void res(const QPMatrix<Scalar> &P, const KQP_MATRIX(Scalar) &A, const QPMatrix<Scalar> &G, const KQP_VECTOR(Scalar) & ux, const KQP_VECTOR(Scalar) & uy, const KQP_VECTOR(Scalar) & uz,const KQP_VECTOR(Scalar) & us, KQP_VECTOR(Scalar) & vx, KQP_VECTOR(Scalar) & vy, KQP_VECTOR(Scalar) & vz, KQP_VECTOR(Scalar) & vs, const ScalingMatrix<Scalar> &W, const KQP_VECTOR(Scalar) & lmbda) {
291 
292  // Evaluates residual in Newton equations:
293  //
294  // [ vx ] [ vx ] [ 0 ] [ P A' G' ] [ ux ]
295  // [ vy ] := [ vy ] - [ 0 ] - [ A 0 0 ] * [ uy ]
296  // [ vz ] [ vz ] [ W'*us ] [ G 0 0 ] [ W^{-1}*uz ]
297  //
298  // vs := vs - lmbda o (uz + us).
299 
300  // vx := vx - P*ux - A'*uy - G'*W^{-1}*uz
301  KQP_MATRIX(Scalar) wz3 = uz;
302  scale(wz3, W, false, true);
303 
304  KQP_VECTOR(Scalar) Pux, Gtuz;
305  P.mult(ux, Pux);
306  G.mult(uz, Gtuz);
307  vx -= Pux - A.adjoint() * uy - Gtuz;
308 
309 
310  // vy := vy - A*ux
311  vy -= A * ux;
312 
313  // vz := vz - G*ux - W'*us
314  KQP_MATRIX(Scalar) ws3 = ux;
315  scale(ws3, W, true, false);
316  KQP_VECTOR(Scalar) Gux;
317  G.mult(ux, Gux);
318  vz -= Gux - ws3;
319 
320  // vs := vs - lmbda o (uz + us)
321  vs.array() -= lmbda.array() * (uz + us).array();
322  }
323 
329  //KQP_MATRIX(Scalar) symm(const KQP_MATRIX(Scalar) &x) {
330  // if (n <= 1) return x;
331  // return x.selfadjointView<Eigen::Lower>();
332  // for i in xrange(n-1):
333  // blas.copy(x, x, offsetx = offset + i*(n+1) + 1, offsety =
334  // offset + (i+1)*(n+1) - 1, incy = n, n = n-i-1)
335  //}
336 
341  template<typename Scalar>
342  void sinv(KQP_VECTOR(Scalar) &x, const KQP_VECTOR(Scalar) &y, const Dimensions &dims, int mnl = 0) {
343 
344  // For the nonlinear and 'l' blocks:
345  //
346  // yk o\ xk = yk .\ xk.
347 
348  x.topRows(mnl + dims.l).array() /= y.topRows(mnl + dims.l).array();
349 
350  // blas.tbsv(y, x, n = mnl + dims.l, k = 0, ldA = 1)
351 
352  // For the 'q' blocks:
353  //
354  // [ l0 -l1' ]
355  // yk o\ xk = 1/a^2 * [ ] * xk
356  // [ -l1 (a*I + l1*l1')/l0 ]
357  //
358  // where yk = (l0, l1) and a = l0^2 - l1'*l1.
359 
360 // int ind = mnl + dims.l;
361  for(size_t i = 0; i < dims.q.size(); i++) { int m = dims.q[i];
362  KQP_NOT_IMPLEMENTED; (void)m;
363  // aa = jnrm2(y, n = m, offset = ind)**2
364  // cc = x[ind]
365  // dd = blas.dot(y, x, offsetx = ind+1, offsety = ind+1, n = m-1)
366  // x[ind] = cc * y[ind] - dd
367  // blas.scal(aa / y[ind], x, n = m-1, offset = ind+1)
368  // blas.axpy(y, x, alpha = dd/y[ind] - cc, n = m-1, offsetx = ind+1,
369  // offsety = ind+1)
370  // blas.scal(1.0/aa, x, n = m, offset = ind)
371  // ind += m
372  }
373 
374  // For the 's' blocks:
375  //
376  // yk o\ xk = xk ./ gamma
377  //
378  // where gammaij = .5 * (yk_i + yk_j).
379 
380 // int ind2 = ind;
381  for(size_t i = 0; i < dims.s.size(); i++) { int m = dims.s[i];
382  KQP_NOT_IMPLEMENTED; (void)m;
383  // for j in xrange(m):
384  // u = 0.5 * ( y[ind2+j:ind2+m] + y[ind2+j] )
385  // blas.tbsv(u, x, n = m-j, k = 0, ldA = 1, offsetx = ind +
386  // j*(m+1))
387  // ind += m*m
388  // ind2 += m
389  }
390  }
391 
392 
393  template<typename Scalar>
395  const ScalingMatrix<Scalar> &W;
396  const KKTSolver<Scalar> &solver;
397  const Dimensions &dims;
398  const KQP_VECTOR(Scalar) &lmbda;
399  public:
400  f4_no_ir_class(const ScalingMatrix<Scalar> &W, const KKTSolver<Scalar> &solver, const Dimensions &dims, const KQP_VECTOR(Scalar) &lmbda):
401  W(W), solver(solver), dims(dims), lmbda(lmbda) {}
402 
403  // f4_no_ir(x, y, z, s) solves
404  //
405  // [ 0 ] [ P A' G' ] [ ux ] [ bx ]
406  // [ 0 ] + [ A 0 0 ] * [ uy ] = [ by ]
407  // [ W'*us ] [ G 0 0 ] [ W^{-1}*uz ] [ bz ]
408  //
409  // lmbda o (uz + us) = bs.
410  //
411  // On entry, x, y, z, s contain bx, by, bz, bs.
412  // On exit, they contain ux, uy, uz, us.
413  void operator()(KQP_VECTOR(Scalar) &x, KQP_VECTOR(Scalar) &y, KQP_VECTOR(Scalar) &z, KQP_VECTOR(Scalar) &s) const {
414  // Solve
415  //
416  // [ P A' G' ] [ ux ] [ bx ]
417  // [ A 0 0 ] [ uy ] = [ by ]
418  // [ G 0 -W'*W ] [ W^{-1}*uz ] [ bz - W'*(lmbda o\ bs) ]
419  //
420  // us = lmbda o\ bs - uz.
421  //
422  // On entry, x, y, z, s contains bx, by, bz, bs.
423  // On exit they contain x, y, z, s.
424 
425  // s := lmbda o\ s
426  // = lmbda o\ bs
427  sinv(s, lmbda, dims);
428 
429  // z := z - W'*s
430  // = bz - W'*(lambda o\ bs)
431  KQP_MATRIX(Scalar) ws3 = s;
432  scale(ws3, W, true, false);
433  z = z - ws3;
434 
435  // Solve for ux, uy, uz
436  solver.solve(x, y, z);
437 
438  // s := s - z
439  // = lambda o\ bs - uz.
440  s = s - z;
441  }
442  };
443 
444  template<typename Scalar>
445  class f4_class {
446  const int refinement;
447  const bool DEBUG;
448  const f4_no_ir_class<Scalar> &f4_no_ir;
449  const ScalingMatrix<Scalar>& W;
450  const KQP_VECTOR(Scalar) &lmbda;
451  const Dimensions &dims;
452  const KQP_MATRIX(Scalar) &A;
453  const QPMatrix<Scalar> &P, &G;
454 
455  public:
456  f4_class(int refinement, bool DEBUG, const f4_no_ir_class<Scalar> &f4_no_ir, const ScalingMatrix<Scalar> &W, const KQP_VECTOR(Scalar) &lmbda, const Dimensions &dims,
457  const QPMatrix<Scalar> &P, const KQP_MATRIX(Scalar) &A, const QPMatrix<Scalar> &G)
458  : refinement(refinement), DEBUG(DEBUG), f4_no_ir(f4_no_ir), W(W), lmbda(lmbda), dims(dims), A(A), P(P), G(G)
459  {
460 
461  }
462 
463  void operator()(KQP_VECTOR(Scalar) &x, KQP_VECTOR(Scalar) &y, KQP_VECTOR(Scalar) &z, KQP_VECTOR(Scalar) &s) {
464 
465  KQP_VECTOR(Scalar) wx, wy, wz, ws;
466  KQP_VECTOR(Scalar) wx2, wy2, wz2, ws2;
467 
468  if (refinement > 0|| DEBUG) {
469  wx = x;
470  wy = y;
471  wz = z;
472  ws = s;
473  }
474 
475  f4_no_ir(x, y, z, s);
476 
477  for(int i = 0; i < refinement; i++) {
478  wx2 = wx;
479  wy2 = wy;
480  wz2 = wz;
481  ws2 = ws;
482  res(P, A, G, x, y, z, s, wx2, wy2, wz2, ws2, W, lmbda);
483  f4_no_ir(wx2, wy2, wz2, ws2);
484  x += wx2;
485  y += wy2;
486  z += wz2;
487  s += ws2;
488  }
489 
490  if (DEBUG) {
491  res(P, A, G, x, y, z, s, wx, wy, wz, ws, W, lmbda);
492  std::cerr << "KKT residuals:" << std::endl;
493  std::cerr << boost::format(" 'x': %e") % wx.norm() << std::endl;
494  std::cerr << boost::format(" 'y': %e") % wy.norm() << std::endl;
495  std::cerr << boost::format(" 'z': %e") % snrm2(wz, dims) << std::endl;
496  std::cerr << boost::format(" 's': %e") % snrm2(ws, dims) << std::endl;
497  }
498  }
499  };
500 
501 
508  template<typename Scalar>
509  void compute_scaling(ScalingMatrix<Scalar> &W, const KQP_VECTOR(Scalar) &s, const KQP_VECTOR(Scalar) &z, KQP_VECTOR(Scalar) &lmbda, const Dimensions &dims, int mnl = -1) {
510 
511 
512  // For the nonlinear block:
513  //
514  // W['dnl'] = sqrt( s[:mnl] ./ z[:mnl] )
515  // W['dnli'] = sqrt( z[:mnl] ./ s[:mnl] )
516  // lambda[:mnl] = sqrt( s[:mnl] .* z[:mnl] )
517 
518  if (mnl < 0) {
519  mnl = 0;
520  } else {
521  W.dnl = (s.segment(0, mnl).array() / z.segment(0, mnl).array()).sqrt();
522  W.dnli = W.dnl.cwiseInverse();
523  lmbda.segment(0,mnl) = (s.segment(0, mnl).array() * z.segment(0, mnl).array()).sqrt();
524  }
525 
526  // For the 'l' block:
527  //
528  // W['d'] = sqrt( sk ./ zk )
529  // W['di'] = sqrt( zk ./ sk )
530  // lambdak = sqrt( sk .* zk )
531  //
532  // where sk and zk are the firstdims.l entries of s and z.
533  // lambda_k is stored in the firstdims.l positions of lmbda.
534 
535  long m = dims.l;
536  W.d = (s.segment(mnl, m).array() / z.segment(mnl, m).array()).sqrt();
537  W.di = W.d.cwiseInverse();
538 
539  lmbda.segment(mnl, m) = (s.segment(mnl, m).array() * z.segment(mnl, m).array()).sqrt();
540 
541 
542  if (!dims.q.empty() || !dims.s.empty())
543  // see below
544  KQP_NOT_IMPLEMENTED;
545  /*
546  // For the 'q' blocks, compute lists 'v', 'beta'.
547  //
548  // The vector v[k] has unit hyperbolic norm:
549  //
550  // (sqrt( v[k]' * J * v[k] ) = 1 with J = [1, 0; 0, -I]).
551  //
552  // beta[k] is a positive scalar.
553  //
554  // The hyperbolic Householder matrix H = 2*v[k]*v[k]' - J
555  // defined by v[k] satisfies
556  //
557  // (beta[k] * H) * zk = (beta[k] * H) \ sk = lambda_k
558  //
559  // where sk = s[indq[k]:indq[k+1]], zk = z[indq[k]:indq[k+1]].
560  //
561  // lambda_k is stored in lmbda[indq[k]:indq[k+1]].
562 
563  ind = mnl +dims.l
564  W['v'] = [ matrix(0.0, (k,1)) for k in dims['q'] ]
565  W['beta'] = len(dims['q']) * [ 0.0 ]
566 
567  for k in xrange(len(dims['q'])):
568  m = dims['q'][k]
569  v = W['v'][k]
570 
571  // a = sqrt( sk' * J * sk ) where J = [1, 0; 0, -I]
572  aa = jnrm2(s, offset = ind, n = m)
573 
574  // b = sqrt( zk' * J * zk )
575  bb = jnrm2(z, offset = ind, n = m)
576 
577  // beta[k] = ( a / b )**1/2
578  W['beta'][k] = math.sqrt( aa / bb )
579 
580  // c = sqrt( (sk/a)' * (zk/b) + 1 ) / sqrt(2)
581  cc = math.sqrt( ( blas.dot(s, z, n = m, offsetx = ind, offsety =
582  ind) / aa / bb + 1.0 ) / 2.0 )
583 
584  // vk = 1/(2*c) * ( (sk/a) + J * (zk/b) )
585  blas.copy(z, v, offsetx = ind, n = m)
586  blas.scal(-1.0/bb, v)
587  v[0] *= -1.0
588  blas.axpy(s, v, 1.0/aa, offsetx = ind, n = m)
589  blas.scal(1.0/2.0/cc, v)
590 
591  // v[k] = 1/sqrt(2*(vk0 + 1)) * ( vk + e ), e = [1; 0]
592  v[0] += 1.0
593  blas.scal(1.0/math.sqrt(2.0 * v[0]), v)
594 
595  // To get the scaled variable lambda_k
596  //
597  // d = sk0/a + zk0/b + 2*c
598  // lambda_k = [ c;
599  // (c + zk0/b)/d * sk1/a + (c + sk0/a)/d * zk1/b ]
600  // lambda_k *= sqrt(a * b)
601 
602  lmbda[ind] = cc
603  dd = 2*cc + s[ind]/aa + z[ind]/bb
604  blas.copy(s, lmbda, offsetx = ind+1, offsety = ind+1, n = m-1)
605  blas.scal((cc + z[ind]/bb)/dd/aa, lmbda, n = m-1, offset = ind+1)
606  blas.axpy(z, lmbda, (cc + s[ind]/aa)/dd/bb, n = m-1, offsetx =
607  ind+1, offsety = ind+1)
608  blas.scal(math.sqrt(aa*bb), lmbda, offset = ind, n = m)
609 
610  ind += m
611 
612 
613  // For the 's' blocks: compute two lists 'r' and 'rti'.
614  //
615  // r[k]' * sk^{-1} * r[k] = diag(lambda_k)^{-1}
616  // r[k]' * zk * r[k] = diag(lambda_k)
617  //
618  // where sk and zk are the entries inds[k] : inds[k+1] of
619  // s and z, reshaped into symmetric matrices.
620  //
621  // rti[k] is the inverse of r[k]', so
622  //
623  // rti[k]' * sk * rti[k] = diag(lambda_k)^{-1}
624  // rti[k]' * zk^{-1} * rti[k] = diag(lambda_k).
625  //
626  // The vectors lambda_k are stored in
627  //
628  // lmbda[dims.l + dimsq : -1 ]
629 
630  W['r'] = [ matrix(0.0, (m,m)) for m in dims['s'] ]
631  W['rti'] = [ matrix(0.0, (m,m)) for m in dims['s'] ]
632  work = matrix(0.0, (max( [0] + dims['s'] )**2, 1))
633  Ls = matrix(0.0, (max( [0] + dims['s'] )**2, 1))
634  Lz = matrix(0.0, (max( [0] + dims['s'] )**2, 1))
635 
636  ind2 = ind
637  for k in xrange(len(dims['s'])):
638  m = dims['s'][k]
639  r, rti = W['r'][k], W['rti'][k]
640 
641  // Factor sk = Ls*Ls'; store Ls in ds[inds[k]:inds[k+1]].
642  blas.copy(s, Ls, offsetx = ind2, n = m**2)
643  lapack.potrf(Ls, n = m, ldA = m)
644 
645  // Factor zs[k] = Lz*Lz'; store Lz in dz[inds[k]:inds[k+1]].
646  blas.copy(z, Lz, offsetx = ind2, n = m**2)
647  lapack.potrf(Lz, n = m, ldA = m)
648 
649  // SVD Lz'*Ls = U*diag(lambda_k)*V'. Keep U in work.
650  for i in xrange(m):
651  blas.scal(0.0, Ls, offset = i*m, n = i)
652  blas.copy(Ls, work, n = m**2)
653  blas.trmm(Lz, work, transA = 'T', ldA = m, ldB = m, n = m, m = m)
654  lapack.gesvd(work, lmbda, jobu = 'O', ldA = m, m = m, n = m,
655  offsetS = ind)
656 
657  // r = Lz^{-T} * U
658  blas.copy(work, r, n = m*m)
659  blas.trsm(Lz, r, transA = 'T', m = m, n = m, ldA = m)
660 
661  // rti = Lz * U
662  blas.copy(work, rti, n = m*m)
663  blas.trmm(Lz, rti, m = m, n = m, ldA = m)
664 
665  // r := r * diag(sqrt(lambda_k))
666  // rti := rti * diag(1 ./ sqrt(lambda_k))
667  for i in xrange(m):
668  a = math.sqrt( lmbda[ind+i] )
669  blas.scal(a, r, offset = m*i, n = m)
670  blas.scal(1.0/a, rti, offset = m*i, n = m)
671 
672  ind += m
673  ind2 += m*m
674  */
675  }
676 
691  template<typename Scalar>
692  void update_scaling(const Dimensions &dims, ScalingMatrix<Scalar> &W, KQP_VECTOR(Scalar) &lmbda, KQP_VECTOR(Scalar) &s, KQP_VECTOR(Scalar) &z) {
693  // Nonlinear and 'l' blocks
694  //
695  // d := d .* sqrt( s ./ z )
696  // lmbda := lmbda .* sqrt(s) .* sqrt(z)
697 
698  int mnl = W.dnl.rows();
699 
700  int ml = W.d.rows();
701  int m = mnl + ml;
702  s.segment(0,m) = s.segment(0,m).cwiseSqrt();
703  z.segment(0,m) = z.segment(0,m).cwiseSqrt();
704 
705  // d := d .* s .* z
706  if (mnl > 0) {
707  W.dnl = W.dnl.segment(0,mnl).array() * s.segment(0, mnl).array() / z.segment(0, mnl).array();
708  W.dnli = W.dnl.cwiseInverse();
709  }
710 
711  W.d = W.d.array() * s.segment(mnl, ml).array() / z.segment(0, ml).array();
712  W.di = W.d.cwiseInverse();
713 
714  // lmbda := s .* z
715 
716  lmbda = s.topRows(m).array() * z.topRows(m).array();
717 
718  if (!dims.q.empty() || !dims.s.empty())
719  // see below
720  KQP_NOT_IMPLEMENTED;
721 
722  /*
723  // 'q' blocks.
724  //
725  // Let st and zt be the new variables in the old scaling:
726  //
727  // st = s_k, zt = z_k
728  //
729  // and a = sqrt(st' * J * st), b = sqrt(zt' * J * zt).
730  //
731  // 1. Compute the hyperbolic Householder transformation 2*q*q' - J
732  // that maps st/a to zt/b.
733  //
734  // c = sqrt( (1 + st'*zt/(a*b)) / 2 )
735  // q = (st/a + J*zt/b) / (2*c).
736  //
737  // The new scaling point is
738  //
739  // wk := betak * sqrt(a/b) * (2*v[k]*v[k]' - J) * q
740  //
741  // with betak = W['beta'][k].
742  //
743  // 3. The scaled variable:
744  //
745  // lambda_k0 = sqrt(a*b) * c
746  // lambda_k1 = sqrt(a*b) * ( (2vk*vk' - J) * (-d*q + u/2) )_1
747  //
748  // where
749  //
750  // u = st/a - J*zt/b
751  // d = ( vk0 * (vk'*u) + u0/2 ) / (2*vk0 *(vk'*q) - q0 + 1).
752  //
753  // 4. Update scaling
754  //
755  // v[k] := wk^1/2
756  // = 1 / sqrt(2*(wk0 + 1)) * (wk + e).
757  // beta[k] *= sqrt(a/b)
758 
759  ind = m
760  for k in xrange(len(W['v'])):
761 
762  v = W['v'][k]
763  m = len(v)
764 
765  // ln = sqrt( lambda_k' * J * lambda_k )
766  ln = jnrm2(lmbda, n = m, offset = ind)
767 
768  // a = sqrt( sk' * J * sk ) = sqrt( st' * J * st )
769  // s := s / a = st / a
770  aa = jnrm2(s, offset = ind, n = m)
771  blas.scal(1.0/aa, s, offset = ind, n = m)
772 
773  // b = sqrt( zk' * J * zk ) = sqrt( zt' * J * zt )
774  // z := z / a = zt / b
775  bb = jnrm2(z, offset = ind, n = m)
776  blas.scal(1.0/bb, z, offset = ind, n = m)
777 
778  // c = sqrt( ( 1 + (st'*zt) / (a*b) ) / 2 )
779  cc = math.sqrt( ( 1.0 + blas.dot(s, z, offsetx = ind, offsety =
780  ind, n = m) ) / 2.0 )
781 
782  // vs = v' * st / a
783  vs = blas.dot(v, s, offsety = ind, n = m)
784 
785  // vz = v' * J *zt / b
786  vz = jdot(v, z, offsety = ind, n = m)
787 
788  // vq = v' * q where q = (st/a + J * zt/b) / (2 * c)
789  vq = (vs + vz ) / 2.0 / cc
790 
791  // vu = v' * u where u = st/a - J * zt/b
792  vu = vs - vz
793 
794  // lambda_k0 = c
795  lmbda[ind] = cc
796 
797  // wk0 = 2 * vk0 * (vk' * q) - q0
798  wk0 = 2 * v[0] * vq - ( s[ind] + z[ind] ) / 2.0 / cc
799 
800  // d = (v[0] * (vk' * u) - u0/2) / (wk0 + 1)
801  dd = (v[0] * vu - s[ind]/2.0 + z[ind]/2.0) / (wk0 + 1.0)
802 
803  // lambda_k1 = 2 * v_k1 * vk' * (-d*q + u/2) - d*q1 + u1/2
804  blas.copy(v, lmbda, offsetx = 1, offsety = ind+1, n = m-1)
805  blas.scal(2.0 * (-dd * vq + 0.5 * vu), lmbda, offset = ind+1,
806  n = m-1)
807  blas.axpy(s, lmbda, 0.5 * (1.0 - dd/cc), offsetx = ind+1, offsety
808  = ind+1, n = m-1)
809  blas.axpy(z, lmbda, 0.5 * (1.0 + dd/cc), offsetx = ind+1, offsety
810  = ind+1, n = m-1)
811 
812  // Scale so that sqrt(lambda_k' * J * lambda_k) = sqrt(aa*bb).
813  blas.scal(math.sqrt(aa*bb), lmbda, offset = ind, n = m)
814 
815  // v := (2*v*v' - J) * q
816  // = 2 * (v'*q) * v' - (J* st/a + zt/b) / (2*c)
817  blas.scal(2.0 * vq, v)
818  v[0] -= s[ind] / 2.0 / cc
819  blas.axpy(s, v, 0.5/cc, offsetx = ind+1, offsety = 1, n = m-1)
820  blas.axpy(z, v, -0.5/cc, offsetx = ind, n = m)
821 
822  // v := v^{1/2} = 1/sqrt(2 * (v0 + 1)) * (v + e)
823  v[0] += 1.0
824  blas.scal(1.0 / math.sqrt(2.0 * v[0]), v)
825 
826  // beta[k] *= ( aa / bb )**1/2
827  W['beta'][k] *= math.sqrt( aa / bb )
828 
829  ind += m
830 
831 
832  // 's' blocks
833  //
834  // Let st, zt be the updated variables in the old scaling:
835  //
836  // st = Ls * Ls', zt = Lz * Lz'.
837  //
838  // where Ls and Lz are the 's' components of s, z.
839  //
840  // 1. SVD Lz'*Ls = Uk * lambda_k^+ * Vk'.
841  //
842  // 2. New scaling is
843  //
844  // r[k] := r[k] * Ls * Vk * diag(lambda_k^+)^{-1/2}
845  // rti[k] := r[k] * Lz * Uk * diag(lambda_k^+)^{-1/2}.
846  //
847 
848  work = matrix(0.0, (max( [0] + [r.size[0] for r in W['r']])**2, 1))
849  ind = mnl + ml + sum([ len(v) for v in W['v'] ])
850  ind2, ind3 = ind, 0
851  for k in xrange(len(W['r'])):
852  r, rti = W['r'][k], W['rti'][k]
853  m = r.size[0]
854 
855  // r := r*sk = r*Ls
856  blas.gemm(r, s, work, m = m, n = m, k = m, ldB = m, ldC = m,
857  offsetB = ind2)
858  blas.copy(work, r, n = m**2)
859 
860  // rti := rti*zk = rti*Lz
861  blas.gemm(rti, z, work, m = m, n = m, k = m, ldB = m, ldC = m,
862  offsetB = ind2)
863  blas.copy(work, rti, n = m**2)
864 
865  // SVD Lz'*Ls = U * lmbds^+ * V'; store U in sk and V' in zk.
866  blas.gemm(z, s, work, transA = 'T', m = m, n = m, k = m, ldA = m,
867  ldB = m, ldC = m, offsetA = ind2, offsetB = ind2)
868  lapack.gesvd(work, lmbda, jobu = 'A', jobvt = 'A', m = m, n = m,
869  ldA = m, U = s, Vt = z, ldU = m, ldVt = m, offsetS = ind,
870  offsetU = ind2, offsetVt = ind2)
871 
872  // r := r*V
873  blas.gemm(r, z, work, transB = 'T', m = m, n = m, k = m, ldB = m,
874  ldC = m, offsetB = ind2)
875  blas.copy(work, r, n = m**2)
876 
877  // rti := rti*U
878  blas.gemm(rti, s, work, n = m, m = m, k = m, ldB = m, ldC = m,
879  offsetB = ind2)
880  blas.copy(work, rti, n = m**2)
881 
882  // r := r*lambda^{-1/2}; rti := rti*lambda^{-1/2}
883  for i in xrange(m):
884  a = 1.0 / math.sqrt(lmbda[ind+i])
885  blas.scal(a, r, offset = m*i, n = m)
886  blas.scal(a, rti, offset = m*i, n = m)
887 
888  ind += m
889  ind2 += m*m
890  ind3 += m
891  */
892  }
893 
898  template<typename Scalar>
899  void ssqr(KQP_VECTOR(Scalar) &x, const KQP_VECTOR(Scalar) &y, const Dimensions &dims, int /*mnl*/ = 0) {
900  x = y.array() * y.array();
901 // int ind = mnl + dims.l;
902 
903  if (!dims.q.empty() || !dims.s.empty())
904  // see below
905  KQP_NOT_IMPLEMENTED;
906 
907  /*
908  for m in dims['q']:
909  x[ind] = blas.nrm2(y, offset = ind, n = m)**2
910  blas.scal(2.0*y[ind], x, n = m-1, offset = ind+1)
911  ind += m
912  blas.tbmv(y, x, n = sum(dims['s']), k = 0, ldA = 1, offsetA = ind,
913  offsetx = ind)
914  */
915  }
916 
917 
922  template<typename Scalar>
923  void sprod(KQP_VECTOR(Scalar) &x, const KQP_VECTOR(Scalar) &y, const Dimensions &dims, int mnl = 0, bool /*diag*/ = false) {
924 
925  // For the nonlinear and 'l' blocks:
926  //
927  // yk o xk = yk .* xk.
928 
929  int n = mnl +dims.l;
930  x.topRows(n).array() *= y.topRows(n).array();
931 
932  if (!dims.q.empty() || !dims.s.empty())
933  // see below
934  KQP_NOT_IMPLEMENTED;
935 
936  /*
937  // For 'q' blocks:
938  //
939  // [ l0 l1' ]
940  // yk o xk = [ ] * xk
941  // [ l1 l0*I ]
942  //
943  // where yk = (l0, l1).
944 
945  ind = mnl +dims.l
946  for m in dims['q']:
947  dd = blas.dot(x, y, offsetx = ind, offsety = ind, n = m)
948  blas.scal(y[ind], x, offset = ind+1, n = m-1)
949  blas.axpy(y, x, alpha = x[ind], n = m-1, offsetx = ind+1, offsety
950  = ind+1)
951  x[ind] = dd
952  ind += m
953 
954 
955  // For the 's' blocks:
956  //
957  // yk o sk = .5 * ( Yk * mat(xk) + mat(xk) * Yk )
958  //
959  // where Yk = mat(yk) if diag is 'N' and Yk = diag(yk) if diag is 'D'.
960 
961  if diag is 'N':
962  maxm = max([0] + dims['s'])
963  A = matrix(0.0, (maxm, maxm))
964 
965  for m in dims['s']:
966  blas.copy(x, A, offsetx = ind, n = m*m)
967 
968  // Write upper triangular part of A and yk.
969  for i in xrange(m-1):
970  symm(A, m)
971  symm(y, m, offset = ind)
972 
973  // xk = 0.5 * (A*yk + yk*A)
974  blas.syr2k(A, y, x, alpha = 0.5, n = m, k = m, ldA = m, ldB =
975  m, ldC = m, offsetB = ind, offsetC = ind)
976 
977  ind += m*m
978 
979  else:
980  ind2 = ind
981  for m in dims['s']:
982  for j in xrange(m):
983  u = 0.5 * ( y[ind2+j:ind2+m] + y[ind2+j] )
984  blas.tbmv(u, x, n = m-j, k = 0, ldA = 1, offsetx =
985  ind + j*(m+1))
986  ind += m*m
987  ind2 += m
988  */
989 
990  }
991 
992  /*
993  Evaluates
994 
995  x := H(lambda^{1/2}) * x (inverse is 'N')
996  x := H(lambda^{-1/2}) * x (inverse is 'I').
997 
998  H is the Hessian of the logarithmic barrier.
999  */
1000  template<typename Scalar>
1001  void scale2(const KQP_VECTOR(Scalar) &lmbda, KQP_VECTOR(Scalar) &x, const Dimensions &dims, int mnl = 0, bool inverse = false) {
1002 
1003  // For the nonlinear and 'l' blocks,
1004  //
1005  // xk := xk ./ l (inverse is 'N')
1006  // xk := xk .* l (inverse is 'I')
1007  //
1008  // where l is lmbda[:mnl+dims.l].
1009 
1010  if (!inverse)
1011  x.topRows(mnl + dims.l).array() /= lmbda.topRows(mnl + dims.l).array();
1012  else
1013  x.topRows(mnl + dims.l).array() *= lmbda.topRows(mnl + dims.l).array();
1014 
1015 
1016  if (!dims.q.empty() || !dims.s.empty())
1017  // see below
1018  KQP_NOT_IMPLEMENTED;
1019  /*
1020  // For 'q' blocks, if inverse is 'N',
1021  //
1022  // xk := 1/a * [ l'*J*xk;
1023  // xk[1:] - (xk[0] + l'*J*xk) / (l[0] + 1) * l[1:] ].
1024  //
1025  // If inverse is 'I',
1026  //
1027  // xk := a * [ l'*xk;
1028  // xk[1:] + (xk[0] + l'*xk) / (l[0] + 1) * l[1:] ].
1029  //
1030  // a = sqrt(lambda_k' * J * lambda_k), l = lambda_k / a.
1031 
1032  ind = mnl +dims.l
1033  for m in dims['q']:
1034  a = jnrm2(lmbda, n = m, offset = ind)
1035  if inverse == 'N':
1036  lx = jdot(lmbda, x, n = m, offsetx = ind, offsety = ind)/a
1037  else:
1038  lx = blas.dot(lmbda, x, n = m, offsetx = ind, offsety = ind)/a
1039  x0 = x[ind]
1040  x[ind] = lx
1041  c = (lx + x0) / (lmbda[ind]/a + 1) / a
1042  if inverse == 'N': c *= -1.0
1043  blas.axpy(lmbda, x, alpha = c, n = m-1, offsetx = ind+1, offsety =
1044  ind+1)
1045  if inverse == 'N': a = 1.0/a
1046  blas.scal(a, x, offset = ind, n = m)
1047  ind += m
1048 
1049 
1050  // For the 's' blocks, if inverse is 'N',
1051  //
1052  // xk := vec( diag(l)^{-1/2} * mat(xk) * diag(k)^{-1/2}).
1053  //
1054  // If inverse is 'I',
1055  //
1056  // xk := vec( diag(l)^{1/2} * mat(xk) * diag(k)^{1/2}).
1057  //
1058  // where l is kth block of lambda.
1059  //
1060  // We scale upper and lower triangular part of mat(xk) because the
1061  // inverse operation will be applied to nonsymmetric matrices.
1062 
1063  ind2 = ind
1064  for k in xrange(len(dims['s'])):
1065  m = dims['s'][k]
1066  for j in xrange(m):
1067  c = math.sqrt(lmbda[ind2+j]) * base.sqrt(lmbda[ind2:ind2+m])
1068  if inverse == 'N':
1069  blas.tbsv(c, x, n = m, k = 0, ldA = 1, offsetx = ind + j*m)
1070  else:
1071  blas.tbmv(c, x, n = m, k = 0, ldA = 1, offsetx = ind + j*m)
1072  ind += m*m
1073  ind2 += m
1074  */
1075 
1076  }
1077 
1078  template<typename Scalar>
1079  void coneqp(const QPMatrix<Scalar> &P, KQP_VECTOR(Scalar) &q,
1080  ConeQPReturn<Scalar> &result,
1081  bool initVals,
1082  Dimensions dims,
1083  const QPMatrix<Scalar> *_G, KQP_VECTOR(Scalar)* _h,
1084  KQP_MATRIX(Scalar) *_A, KQP_VECTOR(Scalar) *_b,
1085  KKTPreSolver<Scalar>* kktpresolver,
1086  ConeQPOptions<Scalar> options) {
1087 
1088  KQP_HLOG_DEBUG( "Starting QP optimization");
1089 
1090  typedef KQP_VECTOR(Scalar) Vector;
1091  typedef KQP_MATRIX(Scalar) Matrix;
1092 
1093  const Scalar STEP = 0.99;
1094  const Scalar EXPON = 3;
1095 
1096  if (options.maxiters < 1)
1097  KQP_THROW_EXCEPTION(illegal_argument_exception, "Option maxiters must be a positive integer");
1098 
1099 
1100  if (options.reltol <= 0.0 && options.abstol <= 0.0)
1101  BOOST_THROW_EXCEPTION(illegal_argument_exception() << errinfo_message("at least one of options['reltol'] and options['abstol'] must be positive"));
1102 
1103  if (options.feastol <= 0.0)
1104  BOOST_THROW_EXCEPTION(illegal_argument_exception() << errinfo_message("options['feastol'] must be a positive scalar"));
1105 
1106 
1107  // -- Choose the KKT Solver if not set
1108  if (kktpresolver == NULL) {
1109  if (dims.q.size() > 0 || dims.s.size() > 0 )
1110  //kktsolver = 'chol';
1111  BOOST_THROW_EXCEPTION(illegal_argument_exception() << errinfo_message("chol solver not avaible"));
1112  else
1113  //kktsolver = 'chol2';
1114  BOOST_THROW_EXCEPTION(illegal_argument_exception() << errinfo_message("chol2 solver not avaible"));
1115  }
1116 
1117  if (q.cols() != 1)
1118  BOOST_THROW_EXCEPTION(illegal_argument_exception() << errinfo_message("'q' must be a matrix with one column"));
1119 
1120  // if (P.rows() != q.rows() || P.cols() != q.rows())
1121  // BOOST_THROW_EXCEPTION(illegal_argument_exception() << errinfo_message("'P' must be a 'd' matrix of size (%d, %d)"));
1122 
1123  /*
1124  def fP(x, y, alpha = 1.0, beta = 0.0):
1125  base.symv(P, x, y, alpha = alpha, beta = beta)
1126  else:
1127  fP = P
1128 
1129  */
1130 
1131  if (_h && _h->cols() != 1)
1132  BOOST_THROW_EXCEPTION(illegal_argument_exception() << errinfo_message("'h' must be a 'd' matrix with one column"));
1133 
1134  if (dims.l == -1) {
1135  dims.l = _h ? _h->rows() : (_G ? _G->rows() : 0);
1136  }
1137 
1138  /*
1139  if (dims.l < 0)
1140  raise TypeError("'dims.l' must be a nonnegative integer")
1141  if [ k for k in dims['q'] if type(k) is not int or k < 1 ]:
1142  raise TypeError("'dims['q']' must be a list of positive integers")
1143  if [ k for k in dims['s'] if type(k) is not int or k < 0 ]:
1144  raise TypeError("'dims['s']' must be a list of nonnegative " \
1145  "integers")
1146  */
1147 
1148  if (options.refinement == -1) {
1149  options.refinement = (dims.q.size() > 0 || dims.s.size() > 0) ? 1 : 0;
1150  }
1151 
1152 
1153  int cdim = dims.l;
1154  int dimsq = 0, dimss = 0;
1155  for(IntIterator i = dims.q.begin(); i != dims.q.end(); i++) { cdim += *i; dimsq += *i; }
1156  for(IntIterator i = dims.s.begin(); i != dims.q.end(); i++) { cdim += *i * *i; dimss += *i; }
1157 
1158  boost::shared_ptr<KQP_VECTOR(Scalar)> __h;
1159  KQP_VECTOR(Scalar) &h = _h ? *_h : *(__h = boost::shared_ptr<KQP_VECTOR(Scalar)>(new KQP_VECTOR(Scalar)(cdim)));
1160  if (!_h) h.setZero(cdim);
1161  if (h.rows() != cdim)
1162  BOOST_THROW_EXCEPTION(illegal_argument_exception() << errinfo_message((boost::format("'h' must be a 'd' matrix of size (%d,1)") %cdim).str()));
1163 
1164  // Data for kth 'q' constraint are found in rows indq[k]:indq[k+1] of G.
1165  std::vector<int> indq;
1166 
1167  indq.push_back(dims.l);
1168 
1169  for(IntIterator k = dims.q.begin(); k != dims.q.end(); k++)
1170  indq.push_back(indq.back() + *k);
1171 
1172 
1173  // Data for kth 's' constraint are found in rows inds[k]:inds[k+1] of G.
1174  std::vector<int> inds;
1175  if (!indq.empty()) inds.push_back(indq.back());
1176 
1177  for(IntIterator k = dims.s.begin(); k != dims.s.end(); k++)
1178  inds.push_back(inds.back() + *k * *k);
1179 
1180  boost::shared_ptr<KQP_MATRIX(Scalar)> __G;
1181  const QPMatrix<Scalar> &G = *_G; // _G ? *_G : *(__G = boost::shared_ptr<Matrix>(new *(cdim, q.rows())));
1182  // if (G.rows() != cdim || G.cols() != q.rows())
1183  // BOOST_THROW_EXCEPTION(illegal_argument_exception() << errinfo_message((boost::format("'G' must be a 'd' matrix of size (%d, %d)") % cdim % q.rows()).str()));
1184 
1185 
1186  boost::shared_ptr<KQP_MATRIX(Scalar)> __A;
1187 
1188  KQP_MATRIX(Scalar) &A = _A ? *_A : *(__A = boost::shared_ptr<KQP_MATRIX(Scalar)>(new KQP_MATRIX(Scalar)(0, q.rows())));
1189 
1190  if (A.cols() != q.rows())
1191  BOOST_THROW_EXCEPTION(illegal_argument_exception() << errinfo_message((boost::format("'A' must be a 'd' matrix with %d columns") % q.rows()).str()));
1192 
1193  boost::shared_ptr<KQP_VECTOR(Scalar)> __b;
1194  KQP_VECTOR(Scalar) &b = _b ? *_b : *(__b = boost::shared_ptr<KQP_VECTOR(Scalar)>(new KQP_VECTOR(Scalar)(A.rows())));
1195  if (!_b) b.setZero(b.rows());
1196 
1197  if (b.rows() != A.rows())
1198  BOOST_THROW_EXCEPTION(illegal_argument_exception() << errinfo_message((boost::format("'b' must have length %d") % A.rows()).str()));
1199 
1200  KQP_VECTOR(Scalar) ws3(cdim);
1201  KQP_VECTOR(Scalar) wz3(cdim);
1202 
1203  // HERE was res (moved to cvxopt_res)
1204 
1205 
1206  Scalar resx0 = std::max((Scalar)1, q.norm());
1207  Scalar resy0 = std::max((Scalar)1, b.norm());
1208 
1209  Scalar resz0 = std::max((Scalar)1, snrm2(h, dims));
1210 
1211  ConeQPReturn<Scalar> &r = result;
1212  KQP_VECTOR(Scalar) &x = r.x, &y = r.y, &z = r.z, &s = r.s;
1213 
1214  // --- No inequalities in QP
1215  if (cdim == 0) {
1216 
1217  // Solve
1218  //
1219  // [ P A' ] [ x ] [ -q ]
1220  // [ ] [ ] = [ ].
1221  // [ A 0 ] [ y ] [ b ]
1222 
1223  boost::shared_ptr<KKTSolver<Scalar> > f3;
1224  try {
1225  ScalingMatrix<Scalar> w;
1226  KQP_HLOG_DEBUG( "Constructing a KKT solver");
1227  f3 = boost::shared_ptr<KKTSolver<Scalar> >(kktpresolver->get(w));
1228  } catch(exception &e) {
1229  e << qp_error_info("Rank(A) < p or Rank([P; A; G]) < n");
1230  throw;
1231  }
1232 
1233  x = -q;
1234  y = b;
1235 
1236  f3->solve(x, y, z);
1237 
1238 
1239  // dres = || P*x + q + A'*y || / resx0
1240  KQP_VECTOR(Scalar) rx;
1241  P.mult(x, rx);
1242  Scalar pcost = 0.5 * (x.dot(rx) + x.dot(q));
1243 
1244  rx += A.adjoint() * y;
1245 
1246  //Scalar dres = rx.norm() / resx0;
1247 
1248  // pres = || A*x - b || / resy0
1249 
1250  // KQP_VECTOR(Scalar) ry = A * x - b;
1251  //Scalar pres = ry.norm() / resy0;
1252 
1253  if (pcost == 0.0) r.relative_gap = NAN;
1254  else r.relative_gap = 0.0;
1255 
1256  r.status = OPTIMAL;
1257  r.primal_objective = r.dual_objective = pcost;
1258  return;
1259  }
1260 
1261 
1262  // --- We have inequalities in the QP
1263 
1264  x = q;
1265  y = b;
1266  s = KQP_VECTOR(Scalar)(cdim);
1267  z = KQP_VECTOR(Scalar)(cdim);
1268 
1269  if (!initVals) {
1270 
1271  // Factor
1272  //
1273  // [ P A' G' ]
1274  // [ A 0 0 ].
1275  // [ G 0 -I ]
1276 
1277  ScalingMatrix<Scalar> W;
1278  W.d = Vector::Ones(dims.l);
1279  W.di = W.d;
1280 
1281  for(IntIterator m = dims.q.begin(); m != dims.q.end(); m++)
1282  W.v.push_back(KQP_MATRIX(Scalar)(*m,1));
1283 
1284  W.beta.push_back(dims.q.size());
1285  for(typename std::vector<KQP_MATRIX(Scalar)>::iterator v = W.v.begin(); v != W.v.end(); v++)
1286  (*v)(0,0) = 1.0;
1287 
1288  for(size_t i = 0; i < dims.s.size(); i++) { int m = dims.s[i];
1289  W.r.push_back(KQP_MATRIX(Scalar)(m,m));
1290  W.r.back().setIdentity();
1291  W.rti.push_back(KQP_MATRIX(Scalar)(m,m));
1292  W.rti.back().setIdentity();
1293  }
1294 
1295  boost::shared_ptr<KKTSolver<Scalar> > f;
1296 
1297  try {
1298  f = boost::shared_ptr<KKTSolver<Scalar> >(kktpresolver->get(W));
1299  } catch(arithmetic_exception &e) {
1300  e << qp_error_info("Rank(A) < p or Rank([P; A; G]) < n");
1301  throw;
1302  }
1303 
1304 
1305  // Solve
1306  //
1307  // [ P A' G' ] [ x ] [ -q ]
1308  // [ A 0 0 ] * [ y ] = [ b ].
1309  // [ G 0 -I ] [ z ] [ h ]
1310 
1311  x = - q;
1312  y = b;
1313  z = h;
1314 
1315  try {
1316  KQP_HLOG_DEBUG( "x=" << x.adjoint());
1317  KQP_HLOG_DEBUG( "z=" << z.adjoint());
1318 
1319  f->solve(x, y, z);
1320  KQP_HLOG_DEBUG( "x=" << x.adjoint());
1321  KQP_HLOG_DEBUG( "z=" << z.adjoint());
1322  } catch(arithmetic_exception &e) {
1323  // Add some context information
1324  e << qp_error_info("Rank(A) < p or Rank([P; A; G]) < n");
1325  throw;
1326  }
1327 
1328  s = -z;
1329 
1330 
1331  // Ensures s is positive
1332  Scalar nrms = snrm2(s, dims);
1333  Scalar ts = max_step(s, dims);
1334  if (ts >= -1e-8 * std::max(nrms, (Scalar)1)) {
1335  Scalar a = 1.0 + ts;
1336  s.segment(0, dims.l).array() += a;
1337 
1338  for(size_t i = 0; i < indq.size() - 1; i++)
1339  s[indq[i]] += a;
1340 
1341  int ind = dims.l + dimsq;
1342  for(size_t i = 0; i < dims.s.size(); i++) {
1343  int m = dims.s[i];
1344  for(int i = ind; i < ind + m * m; i += m+1) s[i] += a;
1345  ind += m * m;
1346  }
1347 
1348  }
1349 
1350  // ensures z is positive
1351  Scalar nrmz = snrm2(z, dims);
1352  Scalar tz = max_step(z, dims);
1353  if (tz >= -1e-8 * std::max(nrmz, (Scalar)1)) {
1354  Scalar a = 1.0 + tz;
1355  z.segment(0, dims.l).array() += a;
1356 
1357  for(size_t i = 0; i < indq.size() - 1; i++) z[indq[i]] += a;
1358  int ind = dims.l + dimsq;
1359  for(size_t i = 0; i < dims.s.size(); i++) {
1360  int m = dims.s[i];
1361  for(int i = ind; i < ind + m * m; i += m+1)
1362  z[i] += a;
1363  ind += m * m;
1364  }
1365 
1366  }
1367 
1368 
1369  } else {
1370  // We have initialised values
1371 
1372  if (result.x.rows() > 0) x = result.x;
1373  else x.setZero();
1374 
1375  if (result.s.rows()) {
1376  s = result.s;
1377  // ts = min{ t | s + t*e >= 0 }
1378  if (max_step(s, dims) >= 0.)
1379  BOOST_THROW_EXCEPTION(illegal_argument_exception() << errinfo_message("initial s is not positive"));
1380 
1381  } else {
1382  for(int i = 0; i < dims.l; i++) s[i] = 1.;
1383 
1384  int ind = dims.l;
1385  for(size_t i = 0; i < dims.q.size(); i++) {
1386  int m = dims.q[i];
1387  s[ind] = 1.0;
1388  ind += m;
1389  }
1390 
1391  for(size_t i = 0; i < dims.s.size(); i++) {
1392  int m = dims.s[i];
1393  for(int i = ind; i < ind+m*m; i += m+1)
1394  s[i] = 1.;
1395  ind += m * m;
1396  }
1397  }
1398 
1399  if (result.y.rows()) y = result.y; else y.setZero();
1400 
1401  if (result.z.rows()) {
1402  z = result.z;
1403  // tz = min{ t | z + t*e >= 0 }
1404  if (max_step(z, dims) >= 0.)
1405  BOOST_THROW_EXCEPTION(illegal_argument_exception() << errinfo_message("initial z is not positive"));
1406  } else {
1407  for(int i = 0; i < dims.l; i++) z[i] = 1.;
1408  int ind = dims.l;
1409  for(size_t i = 0; i < dims.q.size(); i++) { int m = dims.q[i];
1410  z[ind] = 1.0;
1411  ind += m;
1412  }
1413 
1414  for(size_t i = 0; i < dims.s.size(); i++) { int m = dims.s[i];
1415  for(int i = ind; i < ind+m*m; i += m+1)
1416  z[i] = 1.;
1417  ind += m * m;
1418  }
1419  }
1420  }
1421 
1422 
1423  // --- Start the optimisation
1424 
1425  KQP_VECTOR(Scalar) rx = q, ry = b, rz = KQP_VECTOR(Scalar)(cdim);
1426  KQP_VECTOR(Scalar) dx = x, dy = y;
1427  KQP_VECTOR(Scalar) dz = KQP_VECTOR(Scalar)(cdim), ds = KQP_VECTOR(Scalar)(cdim);
1428 
1429  KQP_VECTOR(Scalar) lmbda = KQP_VECTOR(Scalar)(dims.l + dimsq + dimss);
1430  KQP_VECTOR(Scalar) lmbdasq = lmbda;
1431  KQP_VECTOR(Scalar) sigs(dimss);
1432  KQP_VECTOR(Scalar) sigz(dimss);
1433 
1434  // Makes some references for fast access
1435  Status &status = r.status;
1436  Scalar &pcost = r.primal_objective, &dcost = r.dual_objective, &dres = r.dual_infeasibility, &pres = r.primal_infeasibility;
1437  Scalar &gap = r.gap, &relgap = r.relative_gap;
1438 
1439 
1440  KQP_HLOG_DEBUG( boost::format("% 10s% 12s% 10s% 8s% 7s") % pcost % dcost % gap % pres % dres);
1441 
1442  gap = sdot(s, z, dims);
1443 
1444  int &iters = r.iterations;
1445  ScalingMatrix<Scalar> W;
1446  for(iters = 0; iters <= options.maxiters; iters++) {
1447  KQP_HLOG_DEBUG( "========= ITERATION " << convert(iters));
1448  KQP_HLOG_DEBUG( "x=" << convert(x.adjoint()));
1449  KQP_HLOG_DEBUG( "z=" << convert(z.adjoint()));
1450  KQP_HLOG_DEBUG( "s=" << convert(s.adjoint()));
1451  KQP_HLOG_DEBUG( "h=" << convert(h.adjoint()));
1452 
1453  // f0 = (1/2)*x'*P*x + q'*x + r and rx = P*x + q + A'*y + G'*z.
1454  KQP_VECTOR(Scalar) rx;
1455  P.mult(x, rx);
1456  rx += q;
1457  Scalar f0 = 0.5 * ( x.dot(rx) + x.dot(q));
1458 
1459  KQP_VECTOR(Scalar) Gz;
1460  G.mult(z, Gz, true);
1461 
1462  rx += A.adjoint() * y + Gz;
1463  Scalar resx = rx.norm();
1464 
1465  // ry = A*x - b
1466  KQP_VECTOR(Scalar) ry = A * x - b;
1467  Scalar resy = ry.norm();
1468 
1469  // rz = s + G*x - h
1470  KQP_VECTOR(Scalar) rz;
1471  G.mult(x, rz);
1472  rz += s - h;
1473  Scalar resz = snrm2(rz, dims);
1474 
1475 
1476 
1477  // Statistics for stopping criteria.
1478 
1479  // pcost = (1/2)*x'*P*x + q'*x
1480  // dcost = (1/2)*x'*P*x + q'*x + y'*(A*x-b) + z'*(G*x-h)
1481  // = (1/2)*x'*P*x + q'*x + y'*(A*x-b) + z'*(G*x-h+s) - z'*s
1482  // = (1/2)*x'*P*x + q'*x + y'*ry + z'*rz - gap
1483  pcost = f0;
1484  dcost = f0 + y.dot(ry) + sdot(z, rz, dims) - gap;
1485  if (pcost < 0.0)
1486  relgap = gap / -pcost;
1487  else if (dcost > 0.0)
1488  relgap = gap / dcost;
1489  else
1490  relgap = NAN;
1491  pres = std::max(resy/resy0, resz/resz0);
1492  dres = resx/resx0;
1493 
1494 
1495  KQP_HLOG_DEBUG( boost::format("%2d: % 8.4e % 8.4e % 4.0e% 7.0e% 7.0e") % iters % pcost % dcost % gap % pres % dres);
1496 
1497  if (( pres <= options.feastol && dres <= options.feastol && ( gap <= options.abstol || (!std::isnan(relgap) && relgap <= options.reltol) )) || iters == options.maxiters) {
1498  int ind = dims.l + dimsq;
1499  for(size_t i = 0; i < dims.s.size(); i++) { int m = dims.s[i];
1500  KQP_NOT_IMPLEMENTED;
1501  // misc.symm(s, m, ind);
1502  // misc.symm(z, m, ind);
1503  ind += m * m;
1504  }
1505  Scalar ts = max_step(s, dims);
1506  Scalar tz = max_step(z, dims);
1507  if (iters == options.maxiters) {
1508  KQP_HLOG_DEBUG( "Terminated (maximum number of iterations reached).");
1509  status = NOT_CONVERGED;
1510  }
1511  else {
1512  KQP_HLOG_DEBUG( "Optimal solution found.");
1513  status = OPTIMAL;
1514  }
1515  r.dual_slack = -tz;
1516  r.primal_slack = -ts;
1517  return;
1518  }
1519 
1520  // Compute initial scaling W and scaled iterates:
1521  //
1522  // W * z = W^{-T} * s = lambda.
1523  //
1524  // lmbdasq = lambda o lambda.
1525 
1526  if (iters == 0) {
1527  compute_scaling(W, s, z, lmbda, dims);
1528  KQP_HLOG_DEBUG( "lmbda=" << convert(lmbda.adjoint()));
1529  }
1530 
1531  KQP_HLOG_DEBUG( "scaling(d)=" << convert(W.d));
1532 
1533  ssqr(lmbdasq, lmbda, dims);
1534  KQP_HLOG_DEBUG( "lmbdasq=" << convert(lmbdasq.adjoint()));
1535 
1536 
1537  // f3(x, y, z) solves
1538  //
1539  // [ P A' G' ] [ ux ] [ bx ]
1540  // [ A 0 0 ] [ uy ] = [ by ].
1541  // [ G 0 -W'*W ] [ W^{-1}*uz ] [ bz ]
1542  //
1543  // On entry, x, y, z containg bx, by, bz.
1544  // On exit, they contain ux, uy, uz.
1545 
1546  boost::shared_ptr<KKTSolver<Scalar> > f3;
1547  try {
1548  f3 = boost::shared_ptr<KKTSolver<Scalar> >(kktpresolver->get(W));
1549  } catch(arithmetic_exception &e) {
1550  if (iters == 0)
1551  BOOST_THROW_EXCEPTION(arithmetic_exception() << errinfo_message("Rank(A) < p or Rank([P; A; G]) < n"));
1552  else {
1553 // int ind = dims.l + dimsq;
1554  for(size_t i = 0; i < dims.s.size(); i++) {
1555 // int m = dims.s[i];
1556  KQP_NOT_IMPLEMENTED;
1557  // misc.symm(s, m, ind)
1558  // misc.symm(z, m, ind)
1559  // ind += m**2
1560  }
1561  r.primal_slack = -max_step(s, dims);
1562  r.dual_slack = -max_step(z, dims);
1563 
1564  status = SINGULAR_KKT_MATRIX;
1565  e << qp_error_info("Terminated [A] (singular KKT matrix).");
1566 
1567  throw;
1568  }
1569  }
1570 
1571  f4_no_ir_class<Scalar> f4_no_ir(W, *f3, dims, lmbda);
1572 
1573 
1574  KQP_VECTOR(Scalar) wx, wy, wz, ws;
1575  KQP_VECTOR(Scalar) wx2, wy2, wz2, ws2;
1576 
1577  if (iters == 0) {
1578  if (options.refinement > 0 || options.DEBUG) {
1579  wx = q;
1580  wy = b;
1581  wz = KQP_VECTOR(Scalar)(cdim);
1582  ws = KQP_VECTOR(Scalar)(cdim);
1583  }
1584  if (options.refinement > 0) {
1585  wx2 = q;
1586  wy2 = b;
1587  wz2 = KQP_VECTOR(Scalar)(cdim);
1588  ws2 = KQP_VECTOR(Scalar)(cdim);
1589  }
1590  }
1591 
1592  f4_class<Scalar> f4(options.refinement, options.DEBUG, f4_no_ir, W, lmbda, dims, P, A, G);
1593 
1594  Scalar mu = gap / (dims.l + dims.q.size() + dimss);
1595  Scalar sigma = 0.;
1596  Scalar eta = 0.0;
1597 
1598  Scalar step = 0.0;
1599 
1600  for(size_t i = 0; i <= 1; i++) {
1601 
1602  // Solve
1603  //
1604  // [ 0 ] [ P A' G' ] [ dx ]
1605  // [ 0 ] + [ A 0 0 ] * [ dy ] = -(1 - eta) * r
1606  // [ W'*ds ] [ G 0 0 ] [ W^{-1}*dz ]
1607  //
1608  // lmbda o (dz + ds) = -lmbda o lmbda + sigma*mu*e (i=0)
1609  // lmbda o (dz + ds) = -lmbda o lmbda - dsa o dza
1610  // + sigma*mu*e (i=1) where dsa, dza
1611  // are the solution for i=0.
1612 
1613  // ds = -lmbdasq + sigma * mu * e (if i is 0)
1614  // = -lmbdasq - dsa o dza + sigma * mu * e (if i is 1),
1615  // where ds, dz are solution for i is 0.
1616  ds.setZero();
1617  if (options.correction && i == 1)
1618  ds += -ws3;
1619 
1620  ds.topRows(dims.l + dimsq) += -lmbdasq.topRows(dims.l+dimsq);
1621  ds.topRows(dims.l).array() += sigma*mu;
1622  int ind =dims.l;
1623  for(size_t j = 0; j < dims.q.size(); j++) { int m = dims.q[j];
1624  ds[ind] += sigma*mu;
1625  ind += m;
1626  }
1627 // int ind2 = ind;
1628  for(size_t j = 0; j < dims.s.size(); j++) {
1629 // int m = dims.s[j];
1630  KQP_NOT_IMPLEMENTED;
1631  // blas.axpy(lmbdasq, ds, n = m, offsetx = ind2, offsety =
1632  // ind, incy = m + 1, alpha = -1.0)
1633  // ds[ind : ind + m*m : m+1] += sigma*mu
1634  // ind += m*m
1635  // ind2 += m
1636  }
1637 
1638 
1639  // (dx, dy, dz) := -(1 - eta) * (rx, ry, rz)
1640  dx = -(1-eta) * rx;
1641  dy = -(1-eta) * ry;
1642  dz = -(1-eta) * rz;
1643 
1644  try {
1645  KQP_HLOG_DEBUG( "[dx]=" << convert(dx.adjoint()));
1646  KQP_HLOG_DEBUG( "[dz]=" << convert(dz.adjoint()));
1647  KQP_HLOG_DEBUG( "[ds]=" << convert(ds.adjoint()));
1648  f4(dx, dy, dz, ds);
1649  }
1650  catch(arithmetic_exception &e) {
1651  if (iters == 0)
1652  BOOST_THROW_EXCEPTION(arithmetic_exception() << errinfo_message("Rank(A) < p or Rank([P; A; G]) < n"));
1653 
1654  ind =dims.l + dimsq;
1655  for(size_t j = 0; j < dims.s.size(); j++) {
1656 // int m = dims.s[j];
1657  KQP_NOT_IMPLEMENTED;
1658  // misc.symm(s, m, ind)
1659  // misc.symm(z, m, ind)
1660  // ind += m**2
1661  }
1662  r.primal_slack = -max_step(s, dims);
1663  r.dual_slack = -max_step(z, dims);
1664  status = SINGULAR_KKT_MATRIX;
1665  e << qp_error_info("Terminated [B] (singular KKT matrix).");
1666  throw;
1667 
1668  }
1669 
1670  Scalar dsdz = sdot(ds, dz, dims);
1671 
1672  // Save ds o dz for Mehrotra NULL
1673  if (options.correction && i == 0) {
1674  ws3 = ds;
1675  sprod(ws3, dz, dims);
1676  }
1677 
1678  // Maximum steps to boundary.
1679  //
1680  // If i is 1, also compute eigenvalue decomposition of the
1681  // 's' blocks in ds,dz. The eigenvectors Qs, Qz are stored in
1682  // dsk, dzk. The eigenvalues are stored in sigs, sigz.
1683 
1684  scale2(lmbda, ds, dims);
1685  scale2(lmbda, dz, dims);
1686  Scalar ts, tz;
1687  if (i == 0) {
1688  ts = max_step(ds, dims);
1689  tz = max_step(dz, dims);
1690  } else {
1691  ts = max_step(ds, dims, 0, &sigs);
1692  tz = max_step(dz, dims, 0, &sigz);
1693  }
1694 
1695 
1696  Scalar t = std::max(std::max((Scalar)0, ts), tz);
1697  if (t == 0)
1698  step = 1.0;
1699  else
1700  if (i == 0)
1701  step = std::min(1.0, 1.0 / t);
1702  else
1703  step = std::min((Scalar)1, STEP / t);
1704  if (i == 0) {
1705  sigma = std::pow(std::min((Scalar)1, std::max((Scalar)0,
1706  (Scalar)1 - step + dsdz/gap * step*step)), EXPON);
1707  eta = 0.0;
1708  }
1709  }
1710 
1711 
1712  KQP_HLOG_DEBUG( " STEP " << convert(step));
1713  KQP_HLOG_DEBUG( " dx=" << convert(dx.adjoint()));
1714  KQP_HLOG_DEBUG( " dy=" << convert(dx.adjoint()));
1715 
1716  x += step * dx;
1717  y += step * dy;
1718 
1719 
1720  // We will now replace the 'l' and 'q' blocks of ds and dz with
1721  // the updated iterates in the current scaling.
1722  // We also replace the 's' blocks of ds and dz with the factors
1723  // Ls, Lz in a factorization Ls*Ls', Lz*Lz' of the updated variables
1724  // in the current scaling.
1725 
1726  // ds := e + step*ds for nonlinear, 'l' and 'q' blocks.
1727  // dz := e + step*dz for nonlinear, 'l' and 'q' blocks.
1728  ds.topRows(dims.l + dimsq) *= step;
1729  dz.topRows(dims.l + dimsq) *= step;
1730  int ind =dims.l;
1731  ds.topRows(ind).array() += 1.0;
1732  dz.topRows(ind).array() += 1.0;
1733  for(IntIterator m = dims.q.begin(); m != dims.q.end(); m++) {
1734  ds[ind] += 1.0;
1735  dz[ind] += 1.0;
1736  ind += *m;
1737  }
1738 
1739  // ds := H(lambda)^{-1/2} * ds and dz := H(lambda)^{-1/2} * dz.
1740  //
1741  // This replaced the 'l' and 'q' components of ds and dz with the
1742  // updated iterates in the current scaling.
1743  // The 's' components of ds and dz are replaced with
1744  //
1745  // diag(lmbda_k)^{1/2} * Qs * diag(lmbda_k)^{1/2}
1746  // diag(lmbda_k)^{1/2} * Qz * diag(lmbda_k)^{1/2}
1747  //
1748  scale2(lmbda, ds, dims, false, true);
1749  scale2(lmbda, dz, dims, false, true);
1750 
1751  // sigs := ( e + step*sigs ) ./ lambda for 's' blocks.
1752  // sigz := ( e + step*sigz ) ./ lmabda for 's' blocks.
1753  sigs = (1.0 + step * sigs.array()) / lmbda.segment(dims.l + dimsq, dimss).array();
1754  sigz = (1.0 + step * sigz.array()) / lmbda.segment(dims.l + dimsq, dimss).array();
1755 
1756 
1757  // dsk := Ls = dsk * sqrt(sigs).
1758  // dzk := Lz = dzk * sqrt(sigz).
1759  // int ind2 = dims.l + dimsq;
1760 // int ind3 = 0;
1761  for(size_t k = 0; k < dims.s.size(); k++) {
1762  KQP_NOT_IMPLEMENTED;
1763  // m = dims['s'][k]
1764  // for i in xrange(m):
1765  // blas.scal(math.sqrt(sigs[ind3+i]), ds, offset = ind2 + m*i,
1766  // n = m)
1767  // blas.scal(math.sqrt(sigz[ind3+i]), dz, offset = ind2 + m*i,
1768  // n = m)
1769  // ind2 += m*m
1770  // ind3 += m
1771  }
1772 
1773 
1774  // Update lambda and scaling.
1775  update_scaling(dims, W, lmbda, ds, dz);
1776 
1777 
1778  // Unscale s, z (unscaled variables are used only to compute
1779  // feasibility residuals).
1780 
1781  s.topRows(dims.l + dimsq) = lmbda.topRows(dims.l + dimsq);
1782  ind =dims.l + dimsq;
1783  //ind2 = ind;
1784  for(IntIterator m = dims.s.begin(); m != dims.s.end(); m++) {
1785  KQP_NOT_IMPLEMENTED;
1786  // blas.scal(0.0, s, offset = ind2)
1787  // blas.copy(lmbda, s, offsetx = ind, offsety = ind2, n = m,
1788  // incy = m+1)
1789  // ind += m
1790  // ind2 += m*m
1791  }
1792 
1793  scale(s, W, true, false);
1794 
1795  z.topRows(dims.l + dimsq) = lmbda.topRows(dims.l + dimsq);
1796  ind =dims.l + dimsq;
1797  //ind2 = ind;
1798  for(IntIterator m = dims.s.begin(); m != dims.s.end(); m++) {
1799  KQP_NOT_IMPLEMENTED;
1800  // blas.scal(0.0, z, offset = ind2)
1801  // blas.copy(lmbda, z, offsetx = ind, offsety = ind2, n = m,
1802  // incy = m+1)
1803  // ind += m
1804  // ind2 += m*m
1805  }
1806  scale(z, W, false, true);
1807 
1808  gap = lmbda.squaredNorm();
1809 
1810  }
1811  }
1812 
1813 
1814 
1815 
1816  /*
1817 
1818 
1819 
1820 
1821  if use_C:
1822  pack = misc_solvers.pack
1823  else:
1824  def pack(x, y, dims, mnl = 0, offsetx = 0, offsety = 0):
1825  """
1826  Copy x to y using packed storage.
1827 
1828  The vector x is an element of S, with the 's' components stored in
1829  unpacked storage. On return, x is copied to y with the 's' components
1830  stored in packed storage and the off-diagonal entries scaled by
1831  sqrt(2).
1832  """
1833 
1834  nlq = mnl +dims.l + dimsq
1835  blas.copy(x, y, n = nlq, offsetx = offsetx, offsety = offsety)
1836  iu, ip = offsetx + nlq, offsety + nlq
1837  for n in dims['s']:
1838  for k in xrange(n):
1839  blas.copy(x, y, n = n-k, offsetx = iu + k*(n+1), offsety = ip)
1840  y[ip] /= math.sqrt(2)
1841  ip += n-k
1842  iu += n**2
1843  np = sum([ n*(n+1)/2 for n in dims['s'] ])
1844  blas.scal(math.sqrt(2.0), y, n = np, offset = offsety+nlq)
1845 
1846 
1847  if use_C:
1848  pack2 = misc_solvers.pack2
1849  else:
1850  def pack2(x, dims, mnl = 0):
1851  """
1852  In-place version of pack(), which also accepts matrix arguments x.
1853  The columns of x are elements of S, with the 's' components stored in
1854  unpacked storage. On return, the 's' components are stored in packed
1855  storage and the off-diagonal entries are scaled by sqrt(2).
1856  """
1857 
1858  if not dims['s']: return
1859  iu = mnl +dims.l + dimsq
1860  ip = iu
1861  for n in dims['s']:
1862  for k in xrange(n):
1863  x[ip, :] = x[iu + (n+1)*k, :]
1864  x[ip + 1 : ip+n-k, :] = x[iu + (n+1)*k + 1: iu + n*(k+1), :] \
1865  * math.sqrt(2.0)
1866  ip += n - k
1867  iu += n**2
1868  np = sum([ n*(n+1)/2 for n in dims['s'] ])
1869 
1870 
1871  if use_C:
1872  unpack = misc_solvers.unpack
1873  else:
1874  def unpack(x, y, dims, mnl = 0, offsetx = 0, offsety = 0):
1875  """
1876  The vector x is an element of S, with the 's' components stored
1877  in unpacked storage and off-diagonal entries scaled by sqrt(2).
1878  On return, x is copied to y with the 's' components stored in
1879  unpacked storage.
1880  """
1881 
1882  nlq = mnl +dims.l + dimsq
1883  blas.copy(x, y, n = nlq, offsetx = offsetx, offsety = offsety)
1884  iu, ip = offsety+nlq, offsetx+nlq
1885  for n in dims['s']:
1886  for k in xrange(n):
1887  blas.copy(x, y, n = n-k, offsetx = ip, offsety = iu+k*(n+1))
1888  y[iu+k*(n+1)] *= math.sqrt(2)
1889  ip += n-k
1890  iu += n**2
1891  nu = sum([ n**2 for n in dims['s'] ])
1892  blas.scal(1.0/math.sqrt(2.0), y, n = nu, offset = offsety+nlq)
1893 
1894 
1895 
1896 
1897  def sdot2(x, y):
1898  """
1899  Inner product of two block-diagonal symmetric dense 'd' matrices.
1900 
1901  x and y are square dense 'd' matrices, or lists of N square dense 'd'
1902  matrices.
1903  """
1904 
1905  a = 0.0
1906  if type(x) is matrix:
1907  n = x.size[0]
1908  a += blas.dot(x, y, incx=n+1, incy=n+1, n=n)
1909  for j in xrange(1,n):
1910  a += 2.0 * blas.dot(x, y, incx=n+1, incy=n+1, offsetx=j,
1911  offsety=j, n=n-j)
1912 
1913  else:
1914  for k in xrange(len(x)):
1915  n = x[k].size[0]
1916  a += blas.dot(x[k], y[k], incx=n+1, incy=n+1, n=n)
1917  for j in xrange(1,n):
1918  a += 2.0 * blas.dot(x[k], y[k], incx=n+1, incy=n+1,
1919  offsetx=j, offsety=j, n=n-j)
1920  return a
1921 
1922 
1923 
1924 
1925 
1926  if use_C:
1927  trisc = misc_solvers.trisc
1928  else:
1929  def trisc(x, dims, offset = 0):
1930  """
1931  Sets upper triangular part of the 's' components of x equal to zero
1932  and scales the strictly lower triangular part by 2.0.
1933  """
1934 
1935  m =dims.l + dimsq + sum([ k**2 for k in dims['s'] ])
1936  ind = offset +dims.l + dimsq
1937  for mk in dims['s']:
1938  for j in xrange(1, mk):
1939  blas.scal(0.0, x, n = mk-j, inc = mk, offset =
1940  ind + j*(mk + 1) - 1)
1941  blas.scal(2.0, x, offset = ind + mk*(j-1) + j, n = mk-j)
1942  ind += mk**2
1943 
1944 
1945  if use_C:
1946  triusc = misc_solvers.triusc
1947  else:
1948  def triusc(x, dims, offset = 0):
1949  """
1950  Scales the strictly lower triangular part of the 's' components of x
1951  by 0.5.
1952  """
1953 
1954  m =dims.l + dimsq + sum([ k**2 for k in dims['s'] ])
1955  ind = offset +dims.l + dimsq
1956  for mk in dims['s']:
1957  for j in xrange(1, mk):
1958  blas.scal(0.5, x, offset = ind + mk*(j-1) + j, n = mk-j)
1959  ind += mk**2
1960 
1961 
1962  def sgemv(A, x, y, dims, trans = 'N', alpha = 1.0, beta = 0.0, n = None,
1963  offsetA = 0, offsetx = 0, offsety = 0):
1964  """
1965  KQP_MATRIX(Scalar)-vector multiplication.
1966 
1967  A is a matrix or spmatrix of size (m, n) where
1968 
1969  N =dims.l + dimsq + sum( k**2 for k in dims['s'] )
1970 
1971  representing a mapping from R^n to S.
1972 
1973  If trans is 'N':
1974 
1975  y := alpha*A*x + beta * y (trans = 'N').
1976 
1977  x is a vector of length n. y is a vector of length N.
1978 
1979  If trans is 'T':
1980 
1981  y := alpha*A'*x + beta * y (trans = 'T').
1982 
1983  x is a vector of length N. y is a vector of length n.
1984 
1985  The 's' components in S are stored in unpacked 'L' storage.
1986  """
1987 
1988  m =dims.l + dimsq + sum([ k**2 for k in dims['s'] ])
1989  if n is None: n = A.size[1]
1990  if trans == 'T' and alpha: trisc(x, dims, offsetx)
1991  base.gemv(A, x, y, trans = trans, alpha = alpha, beta = beta, m = m,
1992  n = n, offsetA = offsetA, offsetx = offsetx, offsety = offsety)
1993  if trans == 'T' and alpha: triusc(x, dims, offsetx)
1994 
1995 
1996  def jdot(x, y, n = None, offsetx = 0, offsety = 0):
1997  """
1998  Returns x' * J * y, where J = [1, 0; 0, -I].
1999  """
2000 
2001  if n is None:
2002  if len(x) != len(y): raise ValueError("x and y must have the "\
2003  "same length")
2004  n = len(x)
2005  return x[offsetx] * y[offsety] - blas.dot(x, y, n = n-1,
2006  offsetx = offsetx + 1, offsety = offsety + 1)
2007 
2008 
2009  def jnrm2(x, n = None, offset = 0):
2010  """
2011  Returns sqrt(x' * J * x) where J = [1, 0; 0, -I], for a vector
2012  x in a second order cone.
2013  """
2014 
2015  if n is None: n = len(x)
2016  a = blas.nrm2(x, n = n-1, offset = offset+1)
2017  return math.sqrt(x[offset] - a) * math.sqrt(x[offset] + a)
2018 
2019 
2020 
2021 
2022 
2023 
2024 
2025 
2026  def kkt_ldl(G, dims, A, mnl = 0):
2027  """
2028  Solution of KKT equations by a dense LDL factorization of the
2029  3 x 3 system.
2030 
2031  Returns a function that (1) computes the LDL factorization of
2032 
2033  [ H A' GG'*W^{-1} ]
2034  [ A 0 0 ],
2035  [ W^{-T}*GG 0 -I ]
2036 
2037  given H, Df, W, where GG = [Df; G], and (2) returns a function for
2038  solving
2039 
2040  [ H A' GG' ] [ ux ] [ bx ]
2041  [ A 0 0 ] * [ uy ] = [ by ].
2042  [ GG 0 -W'*W ] [ uz ] [ bz ]
2043 
2044  H is n x n, A is p x n, Df is mnl x n, G is N x n where
2045  N =dims.l + dimsq + sum( k**2 for k in dims['s'] ).
2046  """
2047 
2048  p, n = A.size
2049  ldK = n + p + mnl +dims.l + dimsq + sum([ k*(k+1)/2 for k
2050  in dims['s'] ])
2051  K = matrix(0.0, (ldK, ldK))
2052  ipiv = matrix(0, (ldK, 1))
2053  u = matrix(0.0, (ldK, 1))
2054  g = matrix(0.0, (mnl + G.size[0], 1))
2055 
2056  def factor(W, H = None, Df = None):
2057 
2058  blas.scal(0.0, K)
2059  if H is not None: K[:n, :n] = H
2060  K[n:n+p, :n] = A
2061  for k in xrange(n):
2062  if mnl: g[:mnl] = Df[:,k]
2063  g[mnl:] = G[:,k]
2064  scale(g, W, trans = 'T', inverse = 'I')
2065  pack(g, K, dims, mnl, offsety = k*ldK + n + p)
2066  K[(ldK+1)*(p+n) :: ldK+1] = -1.0
2067  lapack.sytrf(K, ipiv)
2068 
2069  def solve(x, y, z):
2070 
2071  // Solve
2072  //
2073  // [ H A' GG'*W^{-1} ] [ ux ] [ bx ]
2074  // [ A 0 0 ] * [ uy [ = [ by ]
2075  // [ W^{-T}*GG 0 -I ] [ W*uz ] [ W^{-T}*bz ]
2076  //
2077  // and return ux, uy, W*uz.
2078  //
2079  // On entry, x, y, z contain bx, by, bz. On exit, they contain
2080  // the solution ux, uy, W*uz.
2081 
2082  blas.copy(x, u)
2083  blas.copy(y, u, offsety = n)
2084  scale(z, W, trans = 'T', inverse = 'I')
2085  pack(z, u, dims, mnl, offsety = n + p)
2086  lapack.sytrs(K, ipiv, u)
2087  blas.copy(u, x, n = n)
2088  blas.copy(u, y, offsetx = n, n = p)
2089  unpack(u, z, dims, mnl, offsetx = n + p)
2090 
2091  return solve
2092 
2093  return factor
2094 
2095 
2096  def kkt_ldl2(G, dims, A, mnl = 0):
2097  """
2098  Solution of KKT equations by a dense LDL factorization of the 2 x 2
2099  system.
2100 
2101  Returns a function that (1) computes the LDL factorization of
2102 
2103  [ H + GG' * W^{-1} * W^{-T} * GG A' ]
2104  [ ]
2105  [ A 0 ]
2106 
2107  given H, Df, W, where GG = [Df; G], and (2) returns a function for
2108  solving
2109 
2110  [ H A' GG' ] [ ux ] [ bx ]
2111  [ A 0 0 ] * [ uy ] = [ by ].
2112  [ GG 0 -W'*W ] [ uz ] [ bz ]
2113 
2114  H is n x n, A is p x n, Df is mnl x n, G is N x n where
2115  N =dims.l + dimsq + sum( k**2 for k in dims['s'] ).
2116  """
2117 
2118  p, n = A.size
2119  ldK = n + p
2120  K = matrix(0.0, (ldK, ldK))
2121  if p: ipiv = matrix(0, (ldK, 1))
2122  g = matrix(0.0, (mnl + G.size[0], 1))
2123  u = matrix(0.0, (ldK, 1))
2124 
2125  def factor(W, H = None, Df = None):
2126 
2127  blas.scal(0.0, K)
2128  if H is not None: K[:n, :n] = H
2129  K[n:,:n] = A
2130  for k in xrange(n):
2131  if mnl: g[:mnl] = Df[:,k]
2132  g[mnl:] = G[:,k]
2133  scale(g, W, trans = 'T', inverse = 'I')
2134  scale(g, W, inverse = 'I')
2135  if mnl: base.gemv(Df, g, K, trans = 'T', beta = 1.0, n = n-k,
2136  offsetA = mnl*k, offsety = (ldK + 1)*k)
2137  sgemv(G, g, K, dims, trans = 'T', beta = 1.0, n = n-k,
2138  offsetA = G.size[0]*k, offsetx = mnl, offsety =
2139  (ldK + 1)*k)
2140  if p: lapack.sytrf(K, ipiv)
2141  else: lapack.potrf(K)
2142 
2143  def solve(x, y, z):
2144 
2145  // Solve
2146  //
2147  // [ H + GG' * W^{-1} * W^{-T} * GG A' ] [ ux ]
2148  // [ ] * [ ]
2149  // [ A 0 ] [ uy ]
2150  //
2151  // [ bx + GG' * W^{-1} * W^{-T} * bz ]
2152  // = [ ]
2153  // [ by ]
2154  //
2155  // and return x, y, W*z = W^{-T} * (GG*x - bz).
2156 
2157  blas.copy(z, g)
2158  scale(g, W, trans = 'T', inverse = 'I')
2159  scale(g, W, inverse = 'I')
2160  if mnl:
2161  base.gemv(Df, g, u, trans = 'T')
2162  beta = 1.0
2163  else:
2164  beta = 0.0
2165  sgemv(G, g, u, dims, trans = 'T', offsetx = mnl, beta = beta)
2166  blas.axpy(x, u)
2167  blas.copy(y, u, offsety = n)
2168  if p: lapack.sytrs(K, ipiv, u)
2169  else: lapack.potrs(K, u)
2170  blas.copy(u, x, n = n)
2171  blas.copy(u, y, offsetx = n, n = p)
2172  if mnl: base.gemv(Df, x, z, alpha = 1.0, beta = -1.0)
2173  sgemv(G, x, z, dims, alpha = 1.0, beta = -1.0, offsety = mnl)
2174  scale(z, W, trans = 'T', inverse = 'I')
2175 
2176  return solve
2177 
2178  return factor
2179 
2180 
2181  def kkt_chol(G, dims, A, mnl = 0):
2182  """
2183  Solution of KKT equations by reduction to a 2 x 2 system, a QR
2184  factorization to eliminate the equality constraints, and a dense
2185  Cholesky factorization of order n-p.
2186 
2187  Computes the QR factorization
2188 
2189  A' = [Q1, Q2] * [R; 0]
2190 
2191  and returns a function that (1) computes the Cholesky factorization
2192 
2193  Q_2^T * (H + GG^T * W^{-1} * W^{-T} * GG) * Q2 = L * L^T,
2194 
2195  given H, Df, W, where GG = [Df; G], and (2) returns a function for
2196  solving
2197 
2198  [ H A' GG' ] [ ux ] [ bx ]
2199  [ A 0 0 ] * [ uy ] = [ by ].
2200  [ GG 0 -W'*W ] [ uz ] [ bz ]
2201 
2202  H is n x n, A is p x n, Df is mnl x n, G is N x n where
2203  N =dims.l + dimsq + sum( k**2 for k in dims['s'] ).
2204  """
2205 
2206  p, n = A.size
2207  cdim = mnl +dims.l + dimsq + sum([ k**2 for k in
2208  dims['s'] ])
2209  cdim_pckd = mnl +dims.l + dimsq + sum([ k*(k+1)/2 for k
2210  in dims['s'] ])
2211 
2212  // A' = [Q1, Q2] * [R; 0] (Q1 is n x p, Q2 is n x n-p).
2213  if type(A) is matrix:
2214  QA = A.T
2215  else:
2216  QA = matrix(A.T)
2217  tauA = matrix(0.0, (p,1))
2218  lapack.geqrf(QA, tauA)
2219 
2220  Gs = matrix(0.0, (cdim, n))
2221  K = matrix(0.0, (n,n))
2222  bzp = matrix(0.0, (cdim_pckd, 1))
2223  yy = matrix(0.0, (p,1))
2224 
2225  def factor(W, H = None, Df = None):
2226 
2227  // Compute
2228  //
2229  // K = [Q1, Q2]' * (H + GG' * W^{-1} * W^{-T} * GG) * [Q1, Q2]
2230  //
2231  // and take the Cholesky factorization of the 2,2 block
2232  //
2233  // Q_2' * (H + GG^T * W^{-1} * W^{-T} * GG) * Q2.
2234 
2235  // Gs = W^{-T} * GG in packed storage.
2236  if mnl:
2237  Gs[:mnl, :] = Df
2238  Gs[mnl:, :] = G
2239  scale(Gs, W, trans = 'T', inverse = 'I')
2240  pack2(Gs, dims, mnl)
2241 
2242  // K = [Q1, Q2]' * (H + Gs' * Gs) * [Q1, Q2].
2243  blas.syrk(Gs, K, k = cdim_pckd, trans = 'T')
2244  if H is not None: K[:,:] += H
2245  symm(K, n)
2246  lapack.ormqr(QA, tauA, K, side = 'L', trans = 'T')
2247  lapack.ormqr(QA, tauA, K, side = 'R')
2248 
2249  // Cholesky factorization of 2,2 block of K.
2250  lapack.potrf(K, n = n-p, offsetA = p*(n+1))
2251 
2252  def solve(x, y, z):
2253 
2254  // Solve
2255  //
2256  // [ 0 A' GG'*W^{-1} ] [ ux ] [ bx ]
2257  // [ A 0 0 ] * [ uy ] = [ by ]
2258  // [ W^{-T}*GG 0 -I ] [ W*uz ] [ W^{-T}*bz ]
2259  //
2260  // and return ux, uy, W*uz.
2261  //
2262  // On entry, x, y, z contain bx, by, bz. On exit, they contain
2263  // the solution ux, uy, W*uz.
2264  //
2265  // If we change variables ux = Q1*v + Q2*w, the system becomes
2266  //
2267  // [ K11 K12 R ] [ v ] [Q1'*(bx+GG'*W^{-1}*W^{-T}*bz)]
2268  // [ K21 K22 0 ] * [ w ] = [Q2'*(bx+GG'*W^{-1}*W^{-T}*bz)]
2269  // [ R^T 0 0 ] [ uy ] [by ]
2270  //
2271  // W*uz = W^{-T} * ( GG*ux - bz ).
2272 
2273  // bzp := W^{-T} * bz in packed storage
2274  scale(z, W, trans = 'T', inverse = 'I')
2275  pack(z, bzp, dims, mnl)
2276 
2277  // x := [Q1, Q2]' * (x + Gs' * bzp)
2278  // = [Q1, Q2]' * (bx + Gs' * W^{-T} * bz)
2279  blas.gemv(Gs, bzp, x, beta = 1.0, trans = 'T', m = cdim_pckd)
2280  lapack.ormqr(QA, tauA, x, side = 'L', trans = 'T')
2281 
2282  // y := x[:p]
2283  // = Q1' * (bx + Gs' * W^{-T} * bz)
2284  blas.copy(y, yy)
2285  blas.copy(x, y, n = p)
2286 
2287  // x[:p] := v = R^{-T} * by
2288  blas.copy(yy, x)
2289  lapack.trtrs(QA, x, uplo = 'U', trans = 'T', n = p)
2290 
2291  // x[p:] := K22^{-1} * (x[p:] - K21*x[:p])
2292  // = K22^{-1} * (Q2' * (bx + Gs' * W^{-T} * bz) - K21*v)
2293  blas.gemv(K, x, x, alpha = -1.0, beta = 1.0, m = n-p, n = p,
2294  offsetA = p, offsety = p)
2295  lapack.potrs(K, x, n = n-p, offsetA = p*(n+1), offsetB = p)
2296 
2297  // y := y - [K11, K12] * x
2298  // = Q1' * (bx + Gs' * W^{-T} * bz) - K11*v - K12*w
2299  blas.gemv(K, x, y, alpha = -1.0, beta = 1.0, m = p, n = n)
2300 
2301  // y := R^{-1}*y
2302  // = R^{-1} * (Q1' * (bx + Gs' * W^{-T} * bz) - K11*v
2303  // - K12*w)
2304  lapack.trtrs(QA, y, uplo = 'U', n = p)
2305 
2306  // x := [Q1, Q2] * x
2307  lapack.ormqr(QA, tauA, x, side = 'L')
2308 
2309  // bzp := Gs * x - bzp.
2310  // = W^{-T} * ( GG*ux - bz ) in packed storage.
2311  // Unpack and copy to z.
2312  blas.gemv(Gs, x, bzp, alpha = 1.0, beta = -1.0, m = cdim_pckd)
2313  unpack(bzp, z, dims, mnl)
2314 
2315  return solve
2316 
2317  return factor
2318 
2319 
2320  def kkt_chol2(G, dims, A, mnl = 0):
2321  """
2322  Solution of KKT equations by reduction to a 2 x 2 system, a sparse
2323  or dense Cholesky factorization of order n to eliminate the 1,1
2324  block, and a sparse or dense Cholesky factorization of order p.
2325  Implemented only for problems with no second-order or semidefinite
2326  cone constraints.
2327 
2328  Returns a function that (1) computes Cholesky factorizations of
2329  the matrices
2330 
2331  S = H + GG' * W^{-1} * W^{-T} * GG,
2332  K = A * S^{-1} *A'
2333 
2334  or (if K is singular in the first call to the function), the matrices
2335 
2336  S = H + GG' * W^{-1} * W^{-T} * GG + A' * A,
2337  K = A * S^{-1} * A',
2338 
2339  given H, Df, W, where GG = [Df; G], and (2) returns a function for
2340  solving
2341 
2342  [ H A' GG' ] [ ux ] [ bx ]
2343  [ A 0 0 ] * [ uy ] = [ by ].
2344  [ GG 0 -W'*W ] [ uz ] [ bz ]
2345 
2346  H is n x n, A is p x n, Df is mnl x n, G isdims.l x n.
2347  """
2348 
2349  if dims['q'] or dims['s']:
2350  raise ValueError("kktsolver option 'kkt_chol2' is implemented "\
2351  "only for problems with no second-order or semidefinite cone "\
2352  "constraints")
2353  p, n = A.size
2354  ml =dims.l
2355  F = {'firstcall': True, 'singular': False}
2356 
2357  def factor(W, H = None, Df = None):
2358 
2359  if F['firstcall']:
2360  if type(G) is matrix:
2361  F['Gs'] = matrix(0.0, G.size)
2362  else:
2363  F['Gs'] = spmatrix(0.0, G.I, G.J, G.size)
2364  if mnl:
2365  if type(Df) is matrix:
2366  F['Dfs'] = matrix(0.0, Df.size)
2367  else:
2368  F['Dfs'] = spmatrix(0.0, Df.I, Df.J, Df.size)
2369  if (mnl and type(Df) is matrix) or type(G) is matrix or \
2370  type(H) is matrix:
2371  F['S'] = matrix(0.0, (n,n))
2372  F['K'] = matrix(0.0, (p,p))
2373  else:
2374  F['S'] = spmatrix([], [], [], (n,n), 'd')
2375  F['Sf'] = None
2376  if type(A) is matrix:
2377  F['K'] = matrix(0.0, (p,p))
2378  else:
2379  F['K'] = spmatrix([], [], [], (p,p), 'd')
2380 
2381  // Dfs = Wnl^{-1} * Df
2382  if mnl: base.gemm(spmatrix(W['dnli'], range(mnl), range(mnl)), Df,
2383  F['Dfs'], partial = True)
2384 
2385  // Gs = Wl^{-1} * G.
2386  base.gemm(spmatrix(W['di'], range(ml), range(ml)), G, F['Gs'],
2387  partial = True)
2388 
2389  if F['firstcall']:
2390  base.syrk(F['Gs'], F['S'], trans = 'T')
2391  if mnl:
2392  base.syrk(F['Dfs'], F['S'], trans = 'T', beta = 1.0)
2393  if H is not None:
2394  F['S'] += H
2395  try:
2396  if type(F['S']) is matrix:
2397  lapack.potrf(F['S'])
2398  else:
2399  F['Sf'] = cholmod.symbolic(F['S'])
2400  cholmod.numeric(F['S'], F['Sf'])
2401  except ArithmeticError:
2402  F['singular'] = True
2403  if type(A) is matrix and type(F['S']) is spmatrix:
2404  F['S'] = matrix(0.0, (n,n))
2405  base.syrk(F['Gs'], F['S'], trans = 'T')
2406  if mnl:
2407  base.syrk(F['Dfs'], F['S'], trans = 'T', beta = 1.0)
2408  base.syrk(A, F['S'], trans = 'T', beta = 1.0)
2409  if H is not None:
2410  F['S'] += H
2411  if type(F['S']) is matrix:
2412  lapack.potrf(F['S'])
2413  else:
2414  F['Sf'] = cholmod.symbolic(F['S'])
2415  cholmod.numeric(F['S'], F['Sf'])
2416  F['firstcall'] = False
2417 
2418  else:
2419  base.syrk(F['Gs'], F['S'], trans = 'T', partial = True)
2420  if mnl: base.syrk(F['Dfs'], F['S'], trans = 'T', beta = 1.0,
2421  partial = True)
2422  if H is not None:
2423  F['S'] += H
2424  if F['singular']:
2425  base.syrk(A, F['S'], trans = 'T', beta = 1.0, partial =
2426  True)
2427  if type(F['S']) is matrix:
2428  lapack.potrf(F['S'])
2429  else:
2430  cholmod.numeric(F['S'], F['Sf'])
2431 
2432  if type(F['S']) is matrix:
2433  // Asct := L^{-1}*A'. Factor K = Asct'*Asct.
2434  if type(A) is matrix:
2435  Asct = A.T
2436  else:
2437  Asct = matrix(A.T)
2438  blas.trsm(F['S'], Asct)
2439  blas.syrk(Asct, F['K'], trans = 'T')
2440  lapack.potrf(F['K'])
2441 
2442  else:
2443  // Asct := L^{-1}*P*A'. Factor K = Asct'*Asct.
2444  if type(A) is matrix:
2445  Asct = A.T
2446  cholmod.solve(F['Sf'], Asct, sys = 7)
2447  cholmod.solve(F['Sf'], Asct, sys = 4)
2448  blas.syrk(Asct, F['K'], trans = 'T')
2449  lapack.potrf(F['K'])
2450  else:
2451  Asct = cholmod.spsolve(F['Sf'], A.T, sys = 7)
2452  Asct = cholmod.spsolve(F['Sf'], Asct, sys = 4)
2453  base.syrk(Asct, F['K'], trans = 'T')
2454  Kf = cholmod.symbolic(F['K'])
2455  cholmod.numeric(F['K'], Kf)
2456 
2457  def solve(x, y, z):
2458 
2459  // Solve
2460  //
2461  // [ H A' GG'*W^{-1} ] [ ux ] [ bx ]
2462  // [ A 0 0 ] * [ uy ] = [ by ]
2463  // [ W^{-T}*GG 0 -I ] [ W*uz ] [ W^{-T}*bz ]
2464  //
2465  // and return ux, uy, W*uz.
2466  //
2467  // If not F['singular']:
2468  //
2469  // K*uy = A * S^{-1} * ( bx + GG'*W^{-1}*W^{-T}*bz ) - by
2470  // S*ux = bx + GG'*W^{-1}*W^{-T}*bz - A'*uy
2471  // W*uz = W^{-T} * ( GG*ux - bz ).
2472  //
2473  // If F['singular']:
2474  //
2475  // K*uy = A * S^{-1} * ( bx + GG'*W^{-1}*W^{-T}*bz + A'*by )
2476  // - by
2477  // S*ux = bx + GG'*W^{-1}*W^{-T}*bz + A'*by - A'*y.
2478  // W*uz = W^{-T} * ( GG*ux - bz ).
2479 
2480  // z := W^{-1} * z = W^{-1} * bz
2481  scale(z, W, trans = 'T', inverse = 'I')
2482 
2483  // If not F['singular']:
2484  // x := L^{-1} * P * (x + GGs'*z)
2485  // = L^{-1} * P * (x + GG'*W^{-1}*W^{-T}*bz)
2486  //
2487  // If F['singular']:
2488  // x := L^{-1} * P * (x + GGs'*z + A'*y))
2489  // = L^{-1} * P * (x + GG'*W^{-1}*W^{-T}*bz + A'*y)
2490 
2491  if mnl: base.gemv(F['Dfs'], z, x, trans = 'T', beta = 1.0)
2492  base.gemv(F['Gs'], z, x, offsetx = mnl, trans = 'T',
2493  beta = 1.0)
2494  if F['singular']:
2495  base.gemv(A, y, x, trans = 'T', beta = 1.0)
2496  if type(F['S']) is matrix:
2497  blas.trsv(F['S'], x)
2498  else:
2499  cholmod.solve(F['Sf'], x, sys = 7)
2500  cholmod.solve(F['Sf'], x, sys = 4)
2501 
2502  // y := K^{-1} * (Asc*x - y)
2503  // = K^{-1} * (A * S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz) - by)
2504  // (if not F['singular'])
2505  // = K^{-1} * (A * S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz +
2506  // A'*by) - by)
2507  // (if F['singular']).
2508 
2509  base.gemv(Asct, x, y, trans = 'T', beta = -1.0)
2510  if type(F['K']) is matrix:
2511  lapack.potrs(F['K'], y)
2512  else:
2513  cholmod.solve(Kf, y)
2514 
2515  // x := P' * L^{-T} * (x - Asc'*y)
2516  // = S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz - A'*y)
2517  // (if not F['singular'])
2518  // = S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz + A'*by - A'*y)
2519  // (if F['singular'])
2520 
2521  base.gemv(Asct, y, x, alpha = -1.0, beta = 1.0)
2522  if type(F['S']) is matrix:
2523  blas.trsv(F['S'], x, trans='T')
2524  else:
2525  cholmod.solve(F['Sf'], x, sys = 5)
2526  cholmod.solve(F['Sf'], x, sys = 8)
2527 
2528  // W*z := GGs*x - z = W^{-T} * (GG*x - bz)
2529  if mnl:
2530  base.gemv(F['Dfs'], x, z, beta = -1.0)
2531  base.gemv(F['Gs'], x, z, beta = -1.0, offsety = mnl)
2532 
2533  return solve
2534 
2535  return factor
2536 
2537 
2538  / **
2539  Solution of KKT equations with zero 1,1 block, by eliminating the
2540  equality constraints via a QR factorization, and solving the
2541  reduced KKT system by another QR factorization.
2542 
2543  Computes the QR factorization
2544 
2545  A' = [Q1, Q2] * [R1; 0]
2546 
2547  and returns a function that (1) computes the QR factorization
2548 
2549  W^{-T} * G * Q2 = Q3 * R3
2550 
2551  (with columns of W^{-T}*G in packed storage), and (2) returns a
2552  function for solving
2553 
2554  [ 0 A' G' ] [ ux ] [ bx ]
2555  [ A 0 0 ] * [ uy ] = [ by ].
2556  [ G 0 -W'*W ] [ uz ] [ bz ]
2557 
2558  A is p x n and G is N x n where N =dims.l + dimsq +
2559  sum( k**2 for k in dims['s'] ).
2560 
2561  * /
2562  void kkt_qr(G, dims, A) {
2563 
2564  p, n = A.size
2565  cdim =dims.l + dimsq + sum([ k**2 for k in dims['s'] ])
2566  cdim_pckd =dims.l + dimsq + sum([ k*(k+1)/2 for k in
2567  dims['s'] ])
2568 
2569  // A' = [Q1, Q2] * [R1; 0]
2570  if type(A) is matrix:
2571  QA = +A.T
2572  else:
2573  QA = matrix(A.T)
2574  tauA = matrix(0.0, (p,1))
2575  lapack.geqrf(QA, tauA)
2576 
2577  Gs = matrix(0.0, (cdim, n))
2578  tauG = matrix(0.0, (n-p,1))
2579  u = matrix(0.0, (cdim_pckd, 1))
2580  vv = matrix(0.0, (n,1))
2581  w = matrix(0.0, (cdim_pckd, 1))
2582 
2583  def factor(W):
2584 
2585  // Gs = W^{-T}*G, in packed storage.
2586  Gs[:,:] = G
2587  scale(Gs, W, trans = 'T', inverse = 'I')
2588  pack2(Gs, dims)
2589 
2590  // Gs := [ Gs1, Gs2 ]
2591  // = Gs * [ Q1, Q2 ]
2592  lapack.ormqr(QA, tauA, Gs, side = 'R', m = cdim_pckd)
2593 
2594  // QR factorization Gs2 := [ Q3, Q4 ] * [ R3; 0 ]
2595  lapack.geqrf(Gs, tauG, n = n-p, m = cdim_pckd, offsetA =
2596  Gs.size[0]*p)
2597 
2598  def solve(x, y, z):
2599 
2600  // On entry, x, y, z contain bx, by, bz. On exit, they
2601  // contain the solution x, y, W*z of
2602  //
2603  // [ 0 A' G'*W^{-1} ] [ x ] [bx ]
2604  // [ A 0 0 ] * [ y ] = [by ].
2605  // [ W^{-T}*G 0 -I ] [ W*z ] [W^{-T}*bz]
2606  //
2607  // The system is solved in five steps:
2608  //
2609  // w := W^{-T}*bz - Gs1*R1^{-T}*by
2610  // u := R3^{-T}*Q2'*bx + Q3'*w
2611  // W*z := Q3*u - w
2612  // y := R1^{-1} * (Q1'*bx - Gs1'*(W*z))
2613  // x := [ Q1, Q2 ] * [ R1^{-T}*by; R3^{-1}*u ]
2614 
2615  // w := W^{-T} * bz in packed storage
2616  scale(z, W, trans = 'T', inverse = 'I')
2617  pack(z, w, dims)
2618 
2619  // vv := [ Q1'*bx; R3^{-T}*Q2'*bx ]
2620  blas.copy(x, vv)
2621  lapack.ormqr(QA, tauA, vv, trans='T')
2622  lapack.trtrs(Gs, vv, uplo = 'U', trans = 'T', n = n-p, offsetA
2623  = Gs.size[0]*p, offsetB = p)
2624 
2625  // x[:p] := R1^{-T} * by
2626  blas.copy(y, x)
2627  lapack.trtrs(QA, x, uplo = 'U', trans = 'T', n = p)
2628 
2629  // w := w - Gs1 * x[:p]
2630  // = W^{-T}*bz - Gs1*by
2631  blas.gemv(Gs, x, w, alpha = -1.0, beta = 1.0, n = p, m =
2632  cdim_pckd)
2633 
2634  // u := [ Q3'*w + v[p:]; 0 ]
2635  // = [ Q3'*w + R3^{-T}*Q2'*bx; 0 ]
2636  blas.copy(w, u)
2637  lapack.ormqr(Gs, tauG, u, trans = 'T', k = n-p, offsetA =
2638  Gs.size[0]*p, m = cdim_pckd)
2639  blas.axpy(vv, u, offsetx = p, n = n-p)
2640  blas.scal(0.0, u, offset = n-p)
2641 
2642  // x[p:] := R3^{-1} * u[:n-p]
2643  blas.copy(u, x, offsety = p, n = n-p)
2644  lapack.trtrs(Gs, x, uplo='U', n = n-p, offsetA = Gs.size[0]*p,
2645  offsetB = p)
2646 
2647  // x is now [ R1^{-T}*by; R3^{-1}*u[:n-p] ]
2648  // x := [Q1 Q2]*x
2649  lapack.ormqr(QA, tauA, x)
2650 
2651  // u := [Q3, Q4] * u - w
2652  // = Q3 * u[:n-p] - w
2653  lapack.ormqr(Gs, tauG, u, k = n-p, m = cdim_pckd, offsetA =
2654  Gs.size[0]*p)
2655  blas.axpy(w, u, alpha = -1.0)
2656 
2657  // y := R1^{-1} * ( v[:p] - Gs1'*u )
2658  // = R1^{-1} * ( Q1'*bx - Gs1'*u )
2659  blas.copy(vv, y, n = p)
2660  blas.gemv(Gs, u, y, m = cdim_pckd, n = p, trans = 'T', alpha =
2661  -1.0, beta = 1.0)
2662  lapack.trtrs(QA, y, uplo = 'U', n=p)
2663 
2664  unpack(u, z, dims)
2665 
2666  return solve
2667 
2668  return factor
2669 
2670  }
2671  */
2672 
2673 #define IMPL_CONEQP(Scalar) template \
2674 void coneqp<Scalar>(const QPMatrix<Scalar> &P, KQP_VECTOR(Scalar) &q, \
2675  ConeQPReturn<Scalar> &result, \
2676  bool initVals, \
2677  Dimensions dims, \
2678  const QPMatrix<Scalar> *_G, KQP_VECTOR(Scalar)* _h, \
2679  KQP_MATRIX(Scalar) *_A, KQP_VECTOR(Scalar) *_b, \
2680  KKTPreSolver<Scalar>* kktpresolver, \
2681  ConeQPOptions<Scalar> options); \
2682  \
2683  template struct ConeQPOptions<Scalar>;\
2684  template struct ConeQPReturn<Scalar>;
2685 
2686  IMPL_CONEQP(double);
2687  IMPL_CONEQP(float);
2688 } // ns
2689 } // ns
2690