nikcleju@2: 
nikcleju@2: import numpy as np
nikcleju@2: import scipy.linalg
nikcleju@2: import math
nikcleju@2: 
nikcleju@2: 
nikcleju@2: #function [x, res, iter] = cgsolve(A, b, tol, maxiter, verbose)
nikcleju@2: def cgsolve(A, b, tol, maxiter, verbose=1):
nikcleju@2:     # Solve a symmetric positive definite system Ax = b via conjugate gradients.
nikcleju@2:     #
nikcleju@2:     # Usage: [x, res, iter] = cgsolve(A, b, tol, maxiter, verbose)
nikcleju@2:     #
nikcleju@2:     # A - Either an NxN matrix, or a function handle.
nikcleju@2:     #
nikcleju@2:     # b - N vector
nikcleju@2:     #
nikcleju@2:     # tol - Desired precision.  Algorithm terminates when 
nikcleju@2:     #    norm(Ax-b)/norm(b) < tol .
nikcleju@2:     #
nikcleju@2:     # maxiter - Maximum number of iterations.
nikcleju@2:     #
nikcleju@2:     # verbose - If 0, do not print out progress messages.
nikcleju@2:     #    If and integer greater than 0, print out progress every 'verbose' iters.
nikcleju@2:     #
nikcleju@2:     # Written by: Justin Romberg, Caltech
nikcleju@2:     # Email: jrom@acm.caltech.edu
nikcleju@2:     # Created: October 2005
nikcleju@2:     #
nikcleju@2: 
nikcleju@2:     #---------------------
nikcleju@2:     # Original Matab code:
nikcleju@2:     #        
nikcleju@2:     #if (nargin < 5), verbose = 1; end
nikcleju@2:     #
nikcleju@2:     #implicit = isa(A,'function_handle');
nikcleju@2:     #
nikcleju@2:     #x = zeros(length(b),1);
nikcleju@2:     #r = b;
nikcleju@2:     #d = r;
nikcleju@2:     #delta = r'*r;
nikcleju@2:     #delta0 = b'*b;
nikcleju@2:     #numiter = 0;
nikcleju@2:     #bestx = x;
nikcleju@2:     #bestres = sqrt(delta/delta0); 
nikcleju@2:     #while ((numiter < maxiter) && (delta > tol^2*delta0))
nikcleju@2:     #
nikcleju@2:     #  # q = A*d
nikcleju@2:     #  if (implicit), q = A(d);  else  q = A*d;  end
nikcleju@2:     # 
nikcleju@2:     #  alpha = delta/(d'*q);
nikcleju@2:     #  x = x + alpha*d;
nikcleju@2:     #  
nikcleju@2:     #  if (mod(numiter+1,50) == 0)
nikcleju@2:     #    # r = b - Aux*x
nikcleju@2:     #    if (implicit), r = b - A(x);  else  r = b - A*x;  end
nikcleju@2:     #  else
nikcleju@2:     #    r = r - alpha*q;
nikcleju@2:     #  end
nikcleju@2:     #  
nikcleju@2:     #  deltaold = delta;
nikcleju@2:     #  delta = r'*r;
nikcleju@2:     #  beta = delta/deltaold;
nikcleju@2:     #  d = r + beta*d;
nikcleju@2:     #  numiter = numiter + 1;
nikcleju@2:     #  if (sqrt(delta/delta0) < bestres)
nikcleju@2:     #    bestx = x;
nikcleju@2:     #    bestres = sqrt(delta/delta0);
nikcleju@2:     #  end    
nikcleju@2:     #  
nikcleju@2:     #  if ((verbose) && (mod(numiter,verbose)==0))
nikcleju@2:     #    disp(sprintf('cg: Iter = #d, Best residual = #8.3e, Current residual = #8.3e', ...
nikcleju@2:     #      numiter, bestres, sqrt(delta/delta0)));
nikcleju@2:     #  end
nikcleju@2:     #  
nikcleju@2:     #end
nikcleju@2:     #
nikcleju@2:     #if (verbose)
nikcleju@2:     #  disp(sprintf('cg: Iterations = #d, best residual = #14.8e', numiter, bestres));
nikcleju@2:     #end
nikcleju@2:     #x = bestx;
nikcleju@2:     #res = bestres;
nikcleju@2:     #iter = numiter;
nikcleju@2: 
nikcleju@2:     # End of original Matab code
nikcleju@2:     #----------------------------
nikcleju@2:     
nikcleju@2:     #if (nargin < 5), verbose = 1; end
nikcleju@2:     # Optional argument
nikcleju@2:     
nikcleju@2:     #implicit = isa(A,'function_handle');
nikcleju@2:     if hasattr(A, '__call__'):
nikcleju@2:         implicit = True
nikcleju@2:     else:
nikcleju@2:         implicit = False
nikcleju@2:     
nikcleju@2:     x = np.zeros(b.size)
nikcleju@2:     r = b.copy()
nikcleju@2:     d = r.copy()
nikcleju@2:     delta = np.vdot(r,r)
nikcleju@2:     delta0 = np.vdot(b,b)
nikcleju@2:     numiter = 0
nikcleju@2:     bestx = x.copy()
nikcleju@2:     bestres = math.sqrt(delta/delta0)
nikcleju@2:     while (numiter < maxiter) and (delta > tol**2*delta0):
nikcleju@2:     
nikcleju@2:       # q = A*d
nikcleju@2:       #if (implicit), q = A(d);  else  q = A*d;  end
nikcleju@2:       if implicit:
nikcleju@2:           q = A(d)
nikcleju@2:       else:
nikcleju@2:           q = np.dot(A,d)
nikcleju@2:      
nikcleju@2:       alpha = delta/np.vdot(d,q)
nikcleju@2:       x = x + alpha*d
nikcleju@2:       
nikcleju@2:       if divmod(numiter+1,50)[1] == 0:
nikcleju@2:         # r = b - Aux*x
nikcleju@2:         #if (implicit), r = b - A(x);  else  r = b - A*x;  end
nikcleju@2:         if implicit:
nikcleju@2:             r = b - A(x)
nikcleju@2:         else:
nikcleju@2:             r = b - np.dot(A,x)
nikcleju@2:       else:
nikcleju@2:         r = r - alpha*q
nikcleju@2:       #end
nikcleju@2:       
nikcleju@2:       deltaold = delta;
nikcleju@2:       delta = np.vdot(r,r)
nikcleju@2:       beta = delta/deltaold;
nikcleju@2:       d = r + beta*d
nikcleju@2:       numiter = numiter + 1
nikcleju@2:       if (math.sqrt(delta/delta0) < bestres):
nikcleju@2:         bestx = x.copy()
nikcleju@2:         bestres = math.sqrt(delta/delta0)
nikcleju@2:       #end    
nikcleju@2:       
nikcleju@2:       if ((verbose) and (divmod(numiter,verbose)[1]==0)):
nikcleju@2:         #disp(sprintf('cg: Iter = #d, Best residual = #8.3e, Current residual = #8.3e', ...
nikcleju@2:         #  numiter, bestres, sqrt(delta/delta0)));
nikcleju@2:         print 'cg: Iter = ',numiter,', Best residual = ',bestres,', Current residual = ',math.sqrt(delta/delta0)
nikcleju@2:       #end
nikcleju@2:       
nikcleju@2:     #end
nikcleju@2:     
nikcleju@2:     if (verbose):
nikcleju@2:       #disp(sprintf('cg: Iterations = #d, best residual = #14.8e', numiter, bestres));
nikcleju@2:       print 'cg: Iterations = ',numiter,', best residual = ',bestres
nikcleju@2:     #end
nikcleju@2:     x = bestx.copy()
nikcleju@2:     res = bestres
nikcleju@2:     iter = numiter
nikcleju@2:     
nikcleju@2:     return x,res,iter
nikcleju@2: 
nikcleju@2: 
nikcleju@2: 
nikcleju@2: #function [xp, up, niter] = l1qc_newton(x0, u0, A, At, b, epsilon, tau, newtontol, newtonmaxiter, cgtol, cgmaxiter) 
nikcleju@2: def l1qc_newton(x0, u0, A, At, b, epsilon, tau, newtontol, newtonmaxiter, cgtol, cgmaxiter):
nikcleju@2:     # Newton algorithm for log-barrier subproblems for l1 minimization
nikcleju@2:     # with quadratic constraints.
nikcleju@2:     #
nikcleju@2:     # Usage: 
nikcleju@2:     # [xp,up,niter] = l1qc_newton(x0, u0, A, At, b, epsilon, tau, 
nikcleju@2:     #                             newtontol, newtonmaxiter, cgtol, cgmaxiter)
nikcleju@2:     #
nikcleju@2:     # x0,u0 - starting points
nikcleju@2:     #
nikcleju@2:     # A - Either a handle to a function that takes a N vector and returns a K 
nikcleju@2:     #     vector , or a KxN matrix.  If A is a function handle, the algorithm
nikcleju@2:     #     operates in "largescale" mode, solving the Newton systems via the
nikcleju@2:     #     Conjugate Gradients algorithm.
nikcleju@2:     #
nikcleju@2:     # At - Handle to a function that takes a K vector and returns an N vector.
nikcleju@2:     #      If A is a KxN matrix, At is ignored.
nikcleju@2:     #
nikcleju@2:     # b - Kx1 vector of observations.
nikcleju@2:     #
nikcleju@2:     # epsilon - scalar, constraint relaxation parameter
nikcleju@2:     #
nikcleju@2:     # tau - Log barrier parameter.
nikcleju@2:     #
nikcleju@2:     # newtontol - Terminate when the Newton decrement is <= newtontol.
nikcleju@2:     #         Default = 1e-3.
nikcleju@2:     #
nikcleju@2:     # newtonmaxiter - Maximum number of iterations.
nikcleju@2:     #         Default = 50.
nikcleju@2:     #
nikcleju@2:     # cgtol - Tolerance for Conjugate Gradients; ignored if A is a matrix.
nikcleju@2:     #     Default = 1e-8.
nikcleju@2:     #
nikcleju@2:     # cgmaxiter - Maximum number of iterations for Conjugate Gradients; ignored
nikcleju@2:     #     if A is a matrix.
nikcleju@2:     #     Default = 200.
nikcleju@2:     #
nikcleju@2:     # Written by: Justin Romberg, Caltech
nikcleju@2:     # Email: jrom@acm.caltech.edu
nikcleju@2:     # Created: October 2005
nikcleju@2:     #
nikcleju@2:     
nikcleju@2:     #---------------------
nikcleju@2:     # Original Matab code:
nikcleju@2:     
nikcleju@3:     ## check if the mix A is implicit or explicit
nikcleju@2:     #largescale = isa(A,'function_handle');
nikcleju@2:     #
nikcleju@2:     ## line search parameters
nikcleju@2:     #alpha = 0.01;
nikcleju@2:     #beta = 0.5;  
nikcleju@2:     #
nikcleju@2:     #if (~largescale), AtA = A'*A; end
nikcleju@2:     #
nikcleju@2:     ## initial point
nikcleju@2:     #x = x0;
nikcleju@2:     #u = u0;
nikcleju@2:     #if (largescale), r = A(x) - b; else  r = A*x - b; end
nikcleju@2:     #fu1 = x - u;
nikcleju@2:     #fu2 = -x - u;
nikcleju@2:     #fe = 1/2*(r'*r - epsilon^2);
nikcleju@2:     #f = sum(u) - (1/tau)*(sum(log(-fu1)) + sum(log(-fu2)) + log(-fe));
nikcleju@2:     #
nikcleju@2:     #niter = 0;
nikcleju@2:     #done = 0;
nikcleju@2:     #while (~done)
nikcleju@2:     #  
nikcleju@2:     #  if (largescale), atr = At(r); else  atr = A'*r; end
nikcleju@2:     #  
nikcleju@2:     #  ntgz = 1./fu1 - 1./fu2 + 1/fe*atr;
nikcleju@2:     #  ntgu = -tau - 1./fu1 - 1./fu2;
nikcleju@2:     #  gradf = -(1/tau)*[ntgz; ntgu];
nikcleju@2:     #  
nikcleju@2:     #  sig11 = 1./fu1.^2 + 1./fu2.^2;
nikcleju@2:     #  sig12 = -1./fu1.^2 + 1./fu2.^2;
nikcleju@2:     #  sigx = sig11 - sig12.^2./sig11;
nikcleju@2:     #    
nikcleju@2:     #  w1p = ntgz - sig12./sig11.*ntgu;
nikcleju@2:     #  if (largescale)
nikcleju@2:     #    h11pfun = @(z) sigx.*z - (1/fe)*At(A(z)) + 1/fe^2*(atr'*z)*atr;
nikcleju@2:     #    [dx, cgres, cgiter] = cgsolve(h11pfun, w1p, cgtol, cgmaxiter, 0);
nikcleju@2:     #    if (cgres > 1/2)
nikcleju@2:     #      disp('Cannot solve system.  Returning previous iterate.  (See Section 4 of notes for more information.)');
nikcleju@2:     #      xp = x;  up = u;
nikcleju@2:     #      return
nikcleju@2:     #    end
nikcleju@2:     #    Adx = A(dx);
nikcleju@2:     #  else
nikcleju@2:     #    H11p = diag(sigx) - (1/fe)*AtA + (1/fe)^2*atr*atr';
nikcleju@2:     #    opts.POSDEF = true; opts.SYM = true;
nikcleju@2:     #    [dx,hcond] = linsolve(H11p, w1p, opts);
nikcleju@2:     #    if (hcond < 1e-14)
nikcleju@2:     #      disp('Matrix ill-conditioned.  Returning previous iterate.  (See Section 4 of notes for more information.)');
nikcleju@2:     #      xp = x;  up = u;
nikcleju@2:     #      return
nikcleju@2:     #    end
nikcleju@2:     #    Adx = A*dx;
nikcleju@2:     #  end
nikcleju@2:     #  du = (1./sig11).*ntgu - (sig12./sig11).*dx;  
nikcleju@2:     # 
nikcleju@2:     #  # minimum step size that stays in the interior
nikcleju@2:     #  ifu1 = find((dx-du) > 0); ifu2 = find((-dx-du) > 0);
nikcleju@2:     #  aqe = Adx'*Adx;   bqe = 2*r'*Adx;   cqe = r'*r - epsilon^2;
nikcleju@2:     #  smax = min(1,min([...
nikcleju@2:     #    -fu1(ifu1)./(dx(ifu1)-du(ifu1)); -fu2(ifu2)./(-dx(ifu2)-du(ifu2)); ...
nikcleju@2:     #    (-bqe+sqrt(bqe^2-4*aqe*cqe))/(2*aqe)
nikcleju@2:     #    ]));
nikcleju@2:     #  s = (0.99)*smax;
nikcleju@2:     #  
nikcleju@2:     #  # backtracking line search
nikcleju@2:     #  suffdec = 0;
nikcleju@2:     #  backiter = 0;
nikcleju@2:     #  while (~suffdec)
nikcleju@2:     #    xp = x + s*dx;  up = u + s*du;  rp = r + s*Adx;
nikcleju@2:     #    fu1p = xp - up;  fu2p = -xp - up;  fep = 1/2*(rp'*rp - epsilon^2);
nikcleju@2:     #    fp = sum(up) - (1/tau)*(sum(log(-fu1p)) + sum(log(-fu2p)) + log(-fep));
nikcleju@2:     #    flin = f + alpha*s*(gradf'*[dx; du]);
nikcleju@2:     #    suffdec = (fp <= flin);
nikcleju@2:     #    s = beta*s;
nikcleju@2:     #    backiter = backiter + 1;
nikcleju@2:     #    if (backiter > 32)
nikcleju@2:     #      disp('Stuck on backtracking line search, returning previous iterate.  (See Section 4 of notes for more information.)');
nikcleju@2:     #      xp = x;  up = u;
nikcleju@2:     #      return
nikcleju@2:     #    end
nikcleju@2:     #  end
nikcleju@2:     #  
nikcleju@2:     #  # set up for next iteration
nikcleju@2:     #  x = xp; u = up;  r = rp;
nikcleju@2:     #  fu1 = fu1p;  fu2 = fu2p;  fe = fep;  f = fp;
nikcleju@2:     #  
nikcleju@2:     #  lambda2 = -(gradf'*[dx; du]);
nikcleju@2:     #  stepsize = s*norm([dx; du]);
nikcleju@2:     #  niter = niter + 1;
nikcleju@2:     #  done = (lambda2/2 < newtontol) | (niter >= newtonmaxiter);
nikcleju@2:     #  
nikcleju@2:     #  disp(sprintf('Newton iter = #d, Functional = #8.3f, Newton decrement = #8.3f, Stepsize = #8.3e', ...
nikcleju@2:     #    niter, f, lambda2/2, stepsize));
nikcleju@2:     #  if (largescale)
nikcleju@2:     #    disp(sprintf('                CG Res = #8.3e, CG Iter = #d', cgres, cgiter));
nikcleju@2:     #  else
nikcleju@2:     #    disp(sprintf('                  H11p condition number = #8.3e', hcond));
nikcleju@2:     #  end
nikcleju@2:     #      
nikcleju@2:     #end
nikcleju@2:     
nikcleju@2:     # End of original Matab code
nikcleju@2:     #----------------------------
nikcleju@2:     
nikcleju@2:     # check if the matrix A is implicit or explicit
nikcleju@2:     #largescale = isa(A,'function_handle');
nikcleju@2:     if hasattr(A, '__call__'):
nikcleju@2:         largescale = True
nikcleju@2:     else:
nikcleju@2:         largescale = False    
nikcleju@2:     
nikcleju@2:     # line search parameters
nikcleju@2:     alpha = 0.01
nikcleju@2:     beta = 0.5
nikcleju@2:     
nikcleju@2:     #if (~largescale), AtA = A'*A; end
nikcleju@2:     if not largescale:
nikcleju@2:         AtA = np.dot(A.T,A)
nikcleju@2:     
nikcleju@2:     # initial point
nikcleju@2:     x = x0.copy()
nikcleju@2:     u = u0.copy()
nikcleju@2:     #if (largescale), r = A(x) - b; else  r = A*x - b; end
nikcleju@2:     if largescale:
nikcleju@2:         r = A(x) - b
nikcleju@2:     else:
nikcleju@2:         r = np.dot(A,x) - b
nikcleju@2:         
nikcleju@2:     fu1 = x - u
nikcleju@2:     fu2 = -x - u
nikcleju@3:     fe = 1.0/2*(np.vdot(r,r) - epsilon**2)
nikcleju@3:     f = u.sum() - (1.0/tau)*(np.log(-fu1).sum() + np.log(-fu2).sum() + math.log(-fe))
nikcleju@2:     
nikcleju@2:     niter = 0
nikcleju@2:     done = 0
nikcleju@2:     while not done:
nikcleju@2:       
nikcleju@2:       #if (largescale), atr = At(r); else  atr = A'*r; end
nikcleju@2:       if largescale:
nikcleju@2:           atr = At(r)
nikcleju@2:       else:
nikcleju@2:           atr = np.dot(A.T,r)
nikcleju@2:       
nikcleju@2:       #ntgz = 1./fu1 - 1./fu2 + 1/fe*atr;
nikcleju@2:       ntgz = 1.0/fu1 - 1.0/fu2 + 1.0/fe*atr
nikcleju@2:       #ntgu = -tau - 1./fu1 - 1./fu2;
nikcleju@2:       ntgu = -tau - 1.0/fu1 - 1.0/fu2
nikcleju@2:       #gradf = -(1/tau)*[ntgz; ntgu];
nikcleju@2:       gradf = -(1.0/tau)*np.concatenate((ntgz, ntgu),0)
nikcleju@2:       
nikcleju@2:       #sig11 = 1./fu1.^2 + 1./fu2.^2;
nikcleju@2:       sig11 = 1.0/(fu1**2) + 1.0/(fu2**2)
nikcleju@2:       #sig12 = -1./fu1.^2 + 1./fu2.^2;
nikcleju@2:       sig12 = -1.0/(fu1**2) + 1.0/(fu2**2)
nikcleju@2:       #sigx = sig11 - sig12.^2./sig11;
nikcleju@2:       sigx = sig11 - (sig12**2)/sig11
nikcleju@2:         
nikcleju@2:       #w1p = ntgz - sig12./sig11.*ntgu;
nikcleju@2:       w1p = ntgz - sig12/sig11*ntgu
nikcleju@2:       if largescale:
nikcleju@2:         #h11pfun = @(z) sigx.*z - (1/fe)*At(A(z)) + 1/fe^2*(atr'*z)*atr;
nikcleju@2:         h11pfun = lambda z: sigx*z - (1.0/fe)*At(A(z)) + 1.0/(fe**2)*np.dot(np.dot(atr.T,z),atr)
nikcleju@2:         dx,cgres,cgiter = cgsolve(h11pfun, w1p, cgtol, cgmaxiter, 0)
nikcleju@3:         if (cgres > 1.0/2):
nikcleju@2:           print 'Cannot solve system.  Returning previous iterate.  (See Section 4 of notes for more information.)'
nikcleju@2:           xp = x.copy()
nikcleju@2:           up = u.copy()
nikcleju@2:           return xp,up,niter
nikcleju@2:         #end
nikcleju@2:         Adx = A(dx)
nikcleju@2:       else:
nikcleju@2:         #H11p = diag(sigx) - (1/fe)*AtA + (1/fe)^2*atr*atr';
nikcleju@3:         # Attention: atr is column vector, so atr*atr' means outer(atr,atr)
nikcleju@3:         H11p = np.diag(sigx) - (1.0/fe)*AtA + (1.0/fe)**2*np.outer(atr,atr)
nikcleju@2:         #opts.POSDEF = true; opts.SYM = true;
nikcleju@2:         #[dx,hcond] = linsolve(H11p, w1p, opts);
nikcleju@2:         #if (hcond < 1e-14)
nikcleju@2:         #  disp('Matrix ill-conditioned.  Returning previous iterate.  (See Section 4 of notes for more information.)');
nikcleju@2:         #  xp = x;  up = u;
nikcleju@2:         #  return
nikcleju@2:         #end
nikcleju@2:         try:
nikcleju@2:             dx = scipy.linalg.solve(H11p, w1p, sym_pos=True)
nikcleju@3:             #dx = np.linalg.solve(H11p, w1p)
nikcleju@3:             hcond = 1.0/np.linalg.cond(H11p)
nikcleju@2:         except scipy.linalg.LinAlgError:
nikcleju@2:             print 'Matrix ill-conditioned.  Returning previous iterate.  (See Section 4 of notes for more information.)'
nikcleju@2:             xp = x.copy()
nikcleju@2:             up = u.copy()
nikcleju@2:             return xp,up,niter
nikcleju@2:         if hcond < 1e-14:
nikcleju@2:             print 'Matrix ill-conditioned.  Returning previous iterate.  (See Section 4 of notes for more information.)'
nikcleju@2:             xp = x.copy()
nikcleju@2:             up = u.copy()
nikcleju@2:             return xp,up,niter
nikcleju@2:         
nikcleju@2:         #Adx = A*dx;
nikcleju@2:         Adx = np.dot(A,dx)
nikcleju@2:       #end
nikcleju@2:       #du = (1./sig11).*ntgu - (sig12./sig11).*dx;  
nikcleju@2:       du = (1.0/sig11)*ntgu - (sig12/sig11)*dx;
nikcleju@2:      
nikcleju@2:       # minimum step size that stays in the interior
nikcleju@2:       #ifu1 = find((dx-du) > 0); ifu2 = find((-dx-du) > 0);
nikcleju@2:       ifu1 = np.nonzero((dx-du)>0)
nikcleju@2:       ifu2 = np.nonzero((-dx-du)>0)
nikcleju@2:       #aqe = Adx'*Adx;   bqe = 2*r'*Adx;   cqe = r'*r - epsilon^2;
nikcleju@2:       aqe = np.dot(Adx.T,Adx)
nikcleju@2:       bqe = 2*np.dot(r.T,Adx)
nikcleju@2:       cqe = np.vdot(r,r) - epsilon**2
nikcleju@2:       #smax = min(1,min([...
nikcleju@2:       #  -fu1(ifu1)./(dx(ifu1)-du(ifu1)); -fu2(ifu2)./(-dx(ifu2)-du(ifu2)); ...
nikcleju@2:       #  (-bqe+sqrt(bqe^2-4*aqe*cqe))/(2*aqe)
nikcleju@2:       #  ]));
nikcleju@3:       smax = min(1,np.concatenate( (-fu1[ifu1]/(dx[ifu1]-du[ifu1]) , -fu2[ifu2]/(-dx[ifu2]-du[ifu2]) , np.array([ (-bqe + math.sqrt(bqe**2-4*aqe*cqe))/(2*aqe) ]) ) , 0).min())
nikcleju@3:       
nikcleju@3:       s = 0.99 * smax
nikcleju@2:       
nikcleju@2:       # backtracking line search
nikcleju@2:       suffdec = 0
nikcleju@2:       backiter = 0
nikcleju@2:       while not suffdec:
nikcleju@2:         #xp = x + s*dx;  up = u + s*du;  rp = r + s*Adx;
nikcleju@2:         xp = x + s*dx
nikcleju@2:         up = u + s*du
nikcleju@2:         rp = r + s*Adx
nikcleju@2:         #fu1p = xp - up;  fu2p = -xp - up;  fep = 1/2*(rp'*rp - epsilon^2);
nikcleju@2:         fu1p = xp - up
nikcleju@2:         fu2p = -xp - up
nikcleju@3:         fep = 0.5*(np.vdot(rp,rp) - epsilon**2)
nikcleju@2:         #fp = sum(up) - (1/tau)*(sum(log(-fu1p)) + sum(log(-fu2p)) + log(-fep));
nikcleju@3:         fp = up.sum() - (1.0/tau)*(np.log(-fu1p).sum() + np.log(-fu2p).sum() + math.log(-fep))
nikcleju@2:         #flin = f + alpha*s*(gradf'*[dx; du]);
nikcleju@2:         flin = f + alpha*s*np.dot(gradf.T , np.concatenate((dx,du),0))
nikcleju@2:         #suffdec = (fp <= flin);
nikcleju@2:         if fp <= flin:
nikcleju@2:             suffdec = True
nikcleju@2:         else:
nikcleju@2:             suffdec = False
nikcleju@2:         
nikcleju@2:         s = beta*s
nikcleju@2:         backiter = backiter + 1
nikcleju@2:         if (backiter > 32):
nikcleju@2:           print 'Stuck on backtracking line search, returning previous iterate.  (See Section 4 of notes for more information.)'
nikcleju@2:           xp = x.copy()
nikcleju@2:           up = u.copy()
nikcleju@2:           return xp,up,niter
nikcleju@2:         #end
nikcleju@2:       #end
nikcleju@2:       
nikcleju@2:       # set up for next iteration
nikcleju@2:       #x = xp; u = up;  r = rp;
nikcleju@2:       x = xp.copy()
nikcleju@2:       u = up.copy()
nikcleju@2:       r = rp.copy()
nikcleju@2:       #fu1 = fu1p;  fu2 = fu2p;  fe = fep;  f = fp;
nikcleju@2:       fu1 = fu1p.copy()
nikcleju@2:       fu2 = fu2p.copy()
nikcleju@2:       fe = fep
nikcleju@2:       f = fp
nikcleju@2:       
nikcleju@2:       #lambda2 = -(gradf'*[dx; du]);
nikcleju@3:       lambda2 = -np.dot(gradf.T , np.concatenate((dx,du),0))
nikcleju@2:       #stepsize = s*norm([dx; du]);
nikcleju@2:       stepsize = s * np.linalg.norm(np.concatenate((dx,du),0))
nikcleju@2:       niter = niter + 1
nikcleju@2:       #done = (lambda2/2 < newtontol) | (niter >= newtonmaxiter);
nikcleju@2:       if lambda2/2.0 < newtontol or niter >= newtonmaxiter:
nikcleju@2:           done = 1
nikcleju@2:       else:
nikcleju@2:           done = 0
nikcleju@2:       
nikcleju@2:       #disp(sprintf('Newton iter = #d, Functional = #8.3f, Newton decrement = #8.3f, Stepsize = #8.3e', ...
nikcleju@2:       print 'Newton iter = ',niter,', Functional = ',f,', Newton decrement = ',lambda2/2.0,', Stepsize = ',stepsize
nikcleju@2: 
nikcleju@2:       if largescale:
nikcleju@2:         print '                CG Res = ',cgres,', CG Iter = ',cgiter
nikcleju@2:       else:
nikcleju@2:         print '                  H11p condition number = ',hcond
nikcleju@2:       #end
nikcleju@2:           
nikcleju@2:     #end
nikcleju@2:     return xp,up,niter
nikcleju@2: 
nikcleju@2: #function xp = l1qc_logbarrier(x0, A, At, b, epsilon, lbtol, mu, cgtol, cgmaxiter)
nikcleju@2: def l1qc_logbarrier(x0, A, At, b, epsilon, lbtol=1e-3, mu=10, cgtol=1e-8, cgmaxiter=200):
nikcleju@2:     # Solve quadratically constrained l1 minimization:
nikcleju@2:     # min ||x||_1   s.t.  ||Ax - b||_2 <= \epsilon
nikcleju@2:     #
nikcleju@2:     # Reformulate as the second-order cone program
nikcleju@2:     # min_{x,u}  sum(u)   s.t.    x - u <= 0,
nikcleju@2:     #                            -x - u <= 0,
nikcleju@2:     #      1/2(||Ax-b||^2 - \epsilon^2) <= 0
nikcleju@2:     # and use a log barrier algorithm.
nikcleju@2:     #
nikcleju@2:     # Usage:  xp = l1qc_logbarrier(x0, A, At, b, epsilon, lbtol, mu, cgtol, cgmaxiter)
nikcleju@2:     #
nikcleju@2:     # x0 - Nx1 vector, initial point.
nikcleju@2:     #
nikcleju@2:     # A - Either a handle to a function that takes a N vector and returns a K 
nikcleju@2:     #     vector , or a KxN matrix.  If A is a function handle, the algorithm
nikcleju@2:     #     operates in "largescale" mode, solving the Newton systems via the
nikcleju@2:     #     Conjugate Gradients algorithm.
nikcleju@2:     #
nikcleju@2:     # At - Handle to a function that takes a K vector and returns an N vector.
nikcleju@2:     #      If A is a KxN matrix, At is ignored.
nikcleju@2:     #
nikcleju@2:     # b - Kx1 vector of observations.
nikcleju@2:     #
nikcleju@2:     # epsilon - scalar, constraint relaxation parameter
nikcleju@2:     #
nikcleju@2:     # lbtol - The log barrier algorithm terminates when the duality gap <= lbtol.
nikcleju@2:     #         Also, the number of log barrier iterations is completely
nikcleju@2:     #         determined by lbtol.
nikcleju@2:     #         Default = 1e-3.
nikcleju@2:     #
nikcleju@2:     # mu - Factor by which to increase the barrier constant at each iteration.
nikcleju@2:     #      Default = 10.
nikcleju@2:     #
nikcleju@2:     # cgtol - Tolerance for Conjugate Gradients; ignored if A is a matrix.
nikcleju@2:     #     Default = 1e-8.
nikcleju@2:     #
nikcleju@2:     # cgmaxiter - Maximum number of iterations for Conjugate Gradients; ignored
nikcleju@2:     #     if A is a matrix.
nikcleju@2:     #     Default = 200.
nikcleju@2:     #
nikcleju@2:     # Written by: Justin Romberg, Caltech
nikcleju@2:     # Email: jrom@acm.caltech.edu
nikcleju@2:     # Created: October 2005
nikcleju@2:     #
nikcleju@2: 
nikcleju@2:     #---------------------
nikcleju@2:     # Original Matab code:
nikcleju@2:     
nikcleju@2:     #largescale = isa(A,'function_handle');
nikcleju@2:     #
nikcleju@2:     #if (nargin < 6), lbtol = 1e-3; end
nikcleju@2:     #if (nargin < 7), mu = 10; end
nikcleju@2:     #if (nargin < 8), cgtol = 1e-8; end
nikcleju@2:     #if (nargin < 9), cgmaxiter = 200; end
nikcleju@2:     #
nikcleju@2:     #newtontol = lbtol;
nikcleju@2:     #newtonmaxiter = 50;
nikcleju@2:     #
nikcleju@2:     #N = length(x0);
nikcleju@2:     #
nikcleju@2:     ## starting point --- make sure that it is feasible
nikcleju@2:     #if (largescale)
nikcleju@2:     #  if (norm(A(x0)-b) > epsilon)
nikcleju@2:     #    disp('Starting point infeasible; using x0 = At*inv(AAt)*y.');
nikcleju@2:     #    AAt = @(z) A(At(z));
nikcleju@2:     #    w = cgsolve(AAt, b, cgtol, cgmaxiter, 0);
nikcleju@2:     #    if (cgres > 1/2)
nikcleju@2:     #      disp('A*At is ill-conditioned: cannot find starting point');
nikcleju@2:     #      xp = x0;
nikcleju@2:     #      return;
nikcleju@2:     #    end
nikcleju@2:     #    x0 = At(w);
nikcleju@2:     #  end
nikcleju@2:     #else
nikcleju@2:     #  if (norm(A*x0-b) > epsilon)
nikcleju@2:     #    disp('Starting point infeasible; using x0 = At*inv(AAt)*y.');
nikcleju@2:     #    opts.POSDEF = true; opts.SYM = true;
nikcleju@2:     #    [w, hcond] = linsolve(A*A', b, opts);
nikcleju@2:     #    if (hcond < 1e-14)
nikcleju@2:     #      disp('A*At is ill-conditioned: cannot find starting point');
nikcleju@2:     #      xp = x0;
nikcleju@2:     #      return;
nikcleju@2:     #    end
nikcleju@2:     #    x0 = A'*w;
nikcleju@2:     #  end  
nikcleju@2:     #end
nikcleju@2:     #x = x0;
nikcleju@2:     #u = (0.95)*abs(x0) + (0.10)*max(abs(x0));
nikcleju@2:     #
nikcleju@2:     #disp(sprintf('Original l1 norm = #.3f, original functional = #.3f', sum(abs(x0)), sum(u)));
nikcleju@2:     #
nikcleju@2:     ## choose initial value of tau so that the duality gap after the first
nikcleju@2:     ## step will be about the origial norm
nikcleju@2:     #tau = max((2*N+1)/sum(abs(x0)), 1);
nikcleju@2:     #                                                                                                                          
nikcleju@2:     #lbiter = ceil((log(2*N+1)-log(lbtol)-log(tau))/log(mu));
nikcleju@2:     #disp(sprintf('Number of log barrier iterations = #d\n', lbiter));
nikcleju@2:     #
nikcleju@2:     #totaliter = 0;
nikcleju@2:     #
nikcleju@2:     ## Added by Nic
nikcleju@2:     #if lbiter == 0
nikcleju@2:     #    xp = zeros(size(x0));
nikcleju@2:     #end
nikcleju@2:     #
nikcleju@2:     #for ii = 1:lbiter
nikcleju@2:     #
nikcleju@2:     #  [xp, up, ntiter] = l1qc_newton(x, u, A, At, b, epsilon, tau, newtontol, newtonmaxiter, cgtol, cgmaxiter);
nikcleju@2:     #  totaliter = totaliter + ntiter;
nikcleju@2:     #  
nikcleju@2:     #  disp(sprintf('\nLog barrier iter = #d, l1 = #.3f, functional = #8.3f, tau = #8.3e, total newton iter = #d\n', ...
nikcleju@2:     #    ii, sum(abs(xp)), sum(up), tau, totaliter));
nikcleju@2:     #  
nikcleju@2:     #  x = xp;
nikcleju@2:     #  u = up;
nikcleju@2:     # 
nikcleju@2:     #  tau = mu*tau;
nikcleju@2:     #  
nikcleju@2:     #end
nikcleju@2:     #
nikcleju@2:     # End of original Matab code
nikcleju@2:     #----------------------------
nikcleju@2:     
nikcleju@2:     #largescale = isa(A,'function_handle');
nikcleju@2:     if hasattr(A, '__call__'):
nikcleju@2:         largescale = True
nikcleju@2:     else:
nikcleju@2:         largescale = False
nikcleju@2:     
nikcleju@2:     #    if (nargin < 6), lbtol = 1e-3; end
nikcleju@2:     #    if (nargin < 7), mu = 10; end
nikcleju@2:     #    if (nargin < 8), cgtol = 1e-8; end
nikcleju@2:     #    if (nargin < 9), cgmaxiter = 200; end
nikcleju@2:     # Nic: added them as optional parameteres
nikcleju@2:     
nikcleju@2:     newtontol = lbtol
nikcleju@2:     newtonmaxiter = 50
nikcleju@2:     
nikcleju@2:     #N = length(x0);
nikcleju@3:     N = x0.size
nikcleju@2:     
nikcleju@2:     # starting point --- make sure that it is feasible
nikcleju@2:     if largescale:
nikcleju@2:       if np.linalg.norm(A(x0) - b) > epsilon:
nikcleju@2:         print 'Starting point infeasible; using x0 = At*inv(AAt)*y.'
nikcleju@2:         #AAt = @(z) A(At(z));
nikcleju@2:         AAt = lambda z: A(At(z))
nikcleju@2:         # TODO: implement cgsolve
nikcleju@2:         w,cgres,cgiter = cgsolve(AAt, b, cgtol, cgmaxiter, 0)
nikcleju@3:         if (cgres > 1.0/2):
nikcleju@2:           print 'A*At is ill-conditioned: cannot find starting point'
nikcleju@2:           xp = x0.copy()
nikcleju@2:           return xp
nikcleju@2:         #end
nikcleju@2:         x0 = At(w)
nikcleju@2:       #end
nikcleju@2:     else:
nikcleju@2:       if np.linalg.norm( np.dot(A,x0) - b ) > epsilon:
nikcleju@2:         print 'Starting point infeasible; using x0 = At*inv(AAt)*y.'
nikcleju@2:         #opts.POSDEF = true; opts.SYM = true;
nikcleju@2:         #[w, hcond] = linsolve(A*A', b, opts);
nikcleju@2:         #if (hcond < 1e-14)
nikcleju@2:         #  disp('A*At is ill-conditioned: cannot find starting point');
nikcleju@2:         #  xp = x0;
nikcleju@2:         #  return;
nikcleju@2:         #end
nikcleju@2:         try:
nikcleju@2:             w = scipy.linalg.solve(np.dot(A,A.T), b, sym_pos=True)
nikcleju@3:             #w = np.linalg.solve(np.dot(A,A.T), b)
nikcleju@2:             hcond = 1.0/scipy.linalg.cond(np.dot(A,A.T))
nikcleju@2:         except scipy.linalg.LinAlgError:
nikcleju@2:             print 'A*At is ill-conditioned: cannot find starting point'
nikcleju@2:             xp = x0.copy()
nikcleju@2:             return xp
nikcleju@2:         if hcond < 1e-14:
nikcleju@2:             print 'A*At is ill-conditioned: cannot find starting point'
nikcleju@2:             xp = x0.copy()
nikcleju@2:             return xp           
nikcleju@2:         #x0 = A'*w;
nikcleju@2:         x0 = np.dot(A.T, w)
nikcleju@2:       #end  
nikcleju@2:     #end
nikcleju@2:     x = x0.copy()
nikcleju@2:     u = (0.95)*np.abs(x0) + (0.10)*np.abs(x0).max()
nikcleju@2:     
nikcleju@2:     #disp(sprintf('Original l1 norm = #.3f, original functional = #.3f', sum(abs(x0)), sum(u)));
nikcleju@2:     print 'Original l1 norm = ',np.abs(x0).sum(),'original functional = ',u.sum()
nikcleju@2:     
nikcleju@2:     # choose initial value of tau so that the duality gap after the first
nikcleju@2:     # step will be about the origial norm
nikcleju@3:     tau = max(((2*N+1.0)/np.abs(x0).sum()), 1)
nikcleju@2:                                                                                                                               
nikcleju@2:     lbiter = math.ceil((math.log(2*N+1)-math.log(lbtol)-math.log(tau))/math.log(mu))
nikcleju@2:     #disp(sprintf('Number of log barrier iterations = #d\n', lbiter));
nikcleju@2:     print 'Number of log barrier iterations = ',lbiter
nikcleju@2:     
nikcleju@2:     totaliter = 0
nikcleju@2:     
nikcleju@2:     # Added by Nic, to fix some crashing
nikcleju@2:     if lbiter == 0:
nikcleju@2:         xp = np.zeros(x0.size)
nikcleju@2:     #end
nikcleju@2:     
nikcleju@2:     #for ii = 1:lbiter
nikcleju@2:     for ii in np.arange(lbiter):
nikcleju@2:     
nikcleju@2:       xp,up,ntiter = l1qc_newton(x, u, A, At, b, epsilon, tau, newtontol, newtonmaxiter, cgtol, cgmaxiter)
nikcleju@2:       totaliter = totaliter + ntiter
nikcleju@2:       
nikcleju@2:       #disp(sprintf('\nLog barrier iter = #d, l1 = #.3f, functional = #8.3f, tau = #8.3e, total newton iter = #d\n', ...
nikcleju@2:       #  ii, sum(abs(xp)), sum(up), tau, totaliter));
nikcleju@2:       print 'Log barrier iter = ',ii,', l1 = ',np.abs(xp).sum(),', functional = ',up.sum(),', tau = ',tau,', total newton iter = ',totaliter
nikcleju@2:       x = xp.copy()
nikcleju@2:       u = up.copy()
nikcleju@2:      
nikcleju@2:       tau = mu*tau
nikcleju@2:       
nikcleju@2:     #end
nikcleju@2:     return xp
nikcleju@2: