visualizing linear inequality constraints in 2D optimization problem

vis01.m

clear all;
clc;
more off;

function result = normalize(x)
  l = norm(x);
  if l > 0.00001
    result = x/l;
  else
    result = x;
  endif
endfunction

function min = gradientDescent(f,df,initial)
  incr = 1.2;
  decr = 0.5;
  linsrchFactor = 0.01;
  x = initial;
  a = 1;
  numouter = 0;
  while 1
    d = -normalize(df(x));
    fx = f(x);
    dfx = df(x);
    numinner = 0;
    while f(x+a*d) > fx + dfx'*(a*d)*linsrchFactor || numinner > 100
      a = decr*a;
      numinner = numinner+1;
    endwhile
    x = x + a*d;
    a = incr*a;
    if norm(a*d) < 0.01 || numouter > 100
      break;
    endif
    numouter = numouter+1;
  endwhile
  min = x;
endfunction

function result = augLagrange(f, g_array, lambdas, mu, x)
  numConstraints = size(g_array,2);
  result = f(x);
  for i = 1:numConstraints
    gx = g_array{i}(x);
    result = result + lambdas(i)*gx;
    result = result + max(0,gx)*gx*mu;
  endfor
endfunction

function result = dAugLagrange(df, g_array, dg_array,lambdas, mu, x)
  numConstraints = size(g_array,2);
  result = df(x);
  for i = 1:numConstraints
    dgx = dg_array{i}(x);
    gx = g_array{i}(x);
    result = result + lambdas(i)*dgx;
    result = result + max(0,gx)*dgx*mu*2;
  endfor
endfunction

function trace = optimizeAugLagrange(f, df, g_array, dg_array, initial)
  trace = [initial];
  numConstraints = size(g_array,2);
  lambdas = zeros(numConstraints);
  mu = 1;
  x = initial;
  numiter = 0;
  while 1
    lagr = @(x) augLagrange(f,g_array,lambdas,mu,x);
    dlagr = @(x) dAugLagrange(df,g_array,dg_array,lambdas,mu,x);
  
    x = gradientDescent(lagr,dlagr,x);
    trace = [x,trace];
    if numiter > 200
      break;
    endif
    for i = 1:numConstraints
      lambdas(i) = max(0, lambdas(i) + g_array{i}(x)*2*mu);
    endfor
    mu = mu*1.01;
    numiter = numiter + 1;
  endwhile
endfunction

function result = logBarrier(f, g_array, mu, x)
  numConstraints = size(g_array,2);
  result = f(x);
  for i = 1:numConstraints
    result = result - mu * log(max(0,-g_array{i}(x)));
  endfor
endfunction

function result = logbarconstraintderiv(g,dg,mu,x)
  gx = g(x);
  dgx = normalize(dg(x));
  if(gx < 0)
    result = - mu / gx * dgx;
  else
    result = (1+gx) * dgx;
  endif
endfunction

function result = dLogBarrier(df, g_array, dg_array, mu, x)
  numConstraints = size(g_array,2);
  result = normalize(df(x));
  for i = 1:numConstraints
%    result = result - mu / g_array{i}(x) * dg_array{i}(x);
    result = result +  logbarconstraintderiv(g_array{i},dg_array{i},mu,x);
  endfor
endfunction

function trace = optimizeLogBarrier(f, df, g_array, dg_array, initial)
  trace = [initial];
  numConstraints = size(g_array,2);
  mu = 8;
  x = initial;
  numiter = 0;
  while 2
    logbarr = @(x) logBarrier(f,g_array,mu,x);
    dlogbarr = @(x) dLogBarrier(df,g_array,dg_array,mu,x);
    x = gradientDescent(logbarr,dlogbarr,x);
    trace = [x,trace];
    if numiter > 200
      break;
    endif
    mu = mu*0.92;
    numiter = numiter + 1;
  endwhile
endfunction

function result = func2D(x1,x2,f)
  zdim = size(f([0;0]),1);
  z = ones(rows(x1),columns(x1),zdim);
  for i = 1:rows(x1)
    for j = 1:columns(x1)
      x = [x1(i,j);x2(i,j)];
      v = f(x);
      for k = 1:zdim
        z(i,j,k) = v(k);
      endfor
    endfor
  endfor
  result = z;
endfunction

% define linear constraint optimization problem
% cost
f = @(x) 2*x(1)+x(2);
df = @(x) [2;1];
f = @(x) x'*(x+3);
df = @(x) 2*x+3;
f = @(x) x'*(x-1);
df = @(x) 2*x-1;
% constraints g(x) <= 0
g0 = @(x) -x(1)-1; % (A*x)^T * 1 <= 1 
dg0 = @(x) -[1;0];
g1 = @(x) -x(1)-x(2)-2;
dg1 = @(x) -[1;1];
g2 = @(x) x(1)-x(2)-3;
dg2 = @(x) [1;-1];
g2 = @(x) x(1)-x(2)*x(2);
dg2 = @(x) [1;-2*x(2)];
%g2 = @(x) x(1)*x(1)+x(2)-1;
%dg2 = @(x) [2*x(1);1];

allconstraints = {g0,g1,g2};
allconstraintderivs = {dg0,dg1,dg2};
%trace = optimizeAugLagrange(f,df,allconstraints,allconstraintderivs, [-3;-3]);
trace = optimizeLogBarrier(f,df,allconstraints,allconstraintderivs, [-2;-2]);


'now plotting'
% plot things
domain = linspace(-3,3,30);
[x,y] = meshgrid(domain,domain);


costforce = func2D(x,y,@(x)normalize(df(x)));
cost = func2D(x,y,f);

constraint0 = func2D(x,y,g0);
constforce0 = func2D(x,y,@(x)normalize(dg0(x))).*(constraint0>0);
constraint1 = func2D(x,y,g1);
constforce1 = func2D(x,y,@(x)normalize(dg1(x))).*(constraint1>0);
constraint2 = func2D(x,y,g2);
constforce2 = func2D(x,y,@(x)normalize(dg2(x))).*(constraint2>0);

logbarrforce = func2D(x,y,@(x)dLogBarrier(df,allconstraints,allconstraintderivs,1,x));

hold on;
axis equal;
cnt1=contour(x,y, cost);
colormap([0.7,0.75,0.7; 0.7,0.75,0.7]);
cnt2=contourf(x,y,constraint0, [0,1000],'--');
cnt3=contourf(x,y,constraint1, [0,1000],':');
cnt4=contourf(x,y,constraint2, [0,1000],'-.');

quivsize = 0.4;
q1=quiver(x,y,-costforce(:,:,1),-costforce(:,:,2), quivsize);
q2=quiver(x,y,-constforce0(:,:,1),-constforce0(:,:,2), quivsize);
q3=quiver(x,y,-constforce1(:,:,1),-constforce1(:,:,2), quivsize);
q4=quiver(x,y,-constforce2(:,:,1),-constforce2(:,:,2), quivsize);
q5=quiver(x,y,-logbarrforce(:,:,1),-logbarrforce(:,:,2), quivsize);
set(q1,'color','blue');
set(q2,'color','yellow');
set(q3,'color','yellow');
set(q4,'color','yellow');
set(q5,'color','red');

p1 = plot(trace(1,:),trace(2,:));
p2 = plot(trace(1,1),trace(2,1), '-o');
set(p1,'color','magenta');
set(p2,'color','red');

hold off;