l1l1_resection_init.m

数学优化工具箱

function opts=l1l1_resection_init(opts);

%opts.CameraArray :
%opts.PointsArray :    Image space
%opts.StructureArray : World Space

%opts.xbounds : Numerator bounds
%opts.ybounds : Denominator bounds
%opts.e : upper bound on the minima
%opts.epsilon : tolerance


if size(opts.StructureArray,1) ~= size(opts.PointsArray,1)
    fprintf('Number of world space and image space points do not match\n');
end

% this is needed early already;

opts.n = size(opts.PointsArray,1);

opts.dim1 = size(opts.PointsArray,2)+1;
opts.dim2 = size(opts.StructureArray,2)+1; % dimension of the matrix we

opts = gamma_initialize(opts);



% construct the objective variable
% order of variables

nvar = opts.dim1*opts.dim2+ opts.dim1-1 + (2+opts.dim1)*(opts.n-1);
nlinear = ((opts.dim1-1)*2+10)*(opts.n-1) + 2*(opts.dim1-1) +1 ;
ncone = 6*(opts.n-1);

Alinear = zeros(nlinear,nvar);
clinear = zeros(nlinear,1);
Acone = zeros(ncone,nvar);
ccone = zeros(ncone,1);

K.f = 1;
K.l = nlinear-1;
K.q=3*ones(2*(opts.n-1),1);


b=zeros(opts.dim1*opts.dim2+opts.dim1-1 ,1 );
b(opts.dim1*opts.dim2+1:end)=1; %t_1k

btemplate=zeros((2+opts.dim1),1);
btemplate(opts.dim1)=1; % r_j

X = [opts.StructureArray(1,:) 1];
Alinear(1,(opts.dim1-1)*opts.dim2+1:opts.dim1*opts.dim2) = X;
clinear(1) = 1;

srow =1;
scol=opts.dim1*opts.dim2;
for k = 1:opts.dim1-1;
    %    Alinear(srow+k,(k-1)*opts.dim2+1:k*opts.dim2)=X;
    Alinear(srow+2*k-1,scol+k) = -1; % t_{ik}
    clinear(srow+2*k-1) = opts.PointsArray(1,k);
    Alinear(srow+2*k-1,opts.dim2*(k-1)+1:opts.dim2*k) = X;

    Alinear(srow+2*k,scol+k) = -1; % t_{ik}
    clinear(srow+2*k) = -opts.PointsArray(1,k);
    Alinear(srow+2*k,opts.dim2*(k-1)+1:opts.dim2*k) = -X;

end


srow = srow + 2*(opts.dim1-1);
scol = scol + opts.dim1-1;


for i = 2:opts.n;
    l=opts.xbounds(i,1); % numerator lower  bound
    u=opts.xbounds(i,2); % numerator upper bound

    L=opts.ybounds(i,1); % denominator lower bound
    U=opts.ybounds(i,2); % denominator upper bound

    X=[opts.StructureArray(i,:) 1];

    %[P_11,P_12,...P_1dim2,...,P_dim1dim2,t_j1,t_j2,...t_jdim1-1,r,a,b]
    b =   cat(1,b,btemplate); % objective function

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % The linear constraints
    %srow=(i-1)*((opts.dim1-1)*2+10);

    %scol=opts.dim1*opts.dim2+(i-1)*(2+opts.dim1);
    
  
%    [scol scol+opts.dim1-1 scol+opts.dim1 scol+opts.dim1+1 scol+opts.dim1+2]
    Alinear(srow+1,scol+1:scol+opts.dim1-1)=-L;
    Alinear(srow+1,scol+opts.dim1+2)=-(u-l);
    clinear(srow+1) = -L*u;

    Alinear(srow+2,scol+1:scol+opts.dim1-1)=-U;
    Alinear(srow+2,scol+opts.dim1+2)=-(u-l);
    Alinear(srow+2,(opts.dim1-1)*opts.dim2+1:opts.dim1*opts.dim2 ) = (u-l)*X;
    clinear(srow+2) = -U*l;


    Alinear(srow+3,scol+1:scol+opts.dim1-1)=U;
    Alinear(srow+3,scol+opts.dim1+2)=(u-l);
    clinear(srow+3) = U*u;

    Alinear(srow+4,scol+1:scol+opts.dim1-1)=L;
    Alinear(srow+4,scol+opts.dim1+2)=(u-l);
    Alinear(srow+4,(opts.dim1-1)*opts.dim2+1:opts.dim1*opts.dim2 ) = -(u-l)*X;
    clinear(srow+4) = L*l;


    Alinear(srow+5,scol+opts.dim1+1)=-1;
    clinear(srow+5) = 0;

    Alinear(srow+6,scol+opts.dim1)=-1;
    Alinear(srow+6,scol+opts.dim1+1)= 1;
    clinear(srow+6) = 0;

    for k = 1:opts.dim1-1;
        Alinear(srow+6+2*k-1,(k-1)*opts.dim2+1:k*opts.dim2)=X;
        Alinear(srow+6+2*k-1,(opts.dim1-1)*opts.dim2+1:opts.dim1*opts.dim2 )=-opts.PointsArray(i,k)*X;
        Alinear(srow+6+2*k-1,scol+k)=-1;
        clinear(srow+6+2*k-1) = 0;

        Alinear(srow+6+2*k,(k-1)*opts.dim2+1:k*opts.dim2)=-X;
        Alinear(srow+6+2*k,(opts.dim1-1)*opts.dim2+1:opts.dim1*opts.dim2 )=opts.PointsArray(i,k)*X;
        Alinear(srow+6+2*k,scol+k)=-1;
        clinear(srow+6+2*k) = 0;
    end


    Alinear(srow+7+2*(opts.dim1-1),(opts.dim1-1)*opts.dim2+1:opts.dim1*opts.dim2 )=-X;
    clinear(srow+7+2*(opts.dim1-1)) = -L;

    Alinear(srow+8+2*(opts.dim1-1),(opts.dim1-1)*opts.dim2+1:opts.dim1*opts.dim2 )=X;
    clinear(srow+8+2*(opts.dim1-1)) = U;


    Alinear(srow+9+2*(opts.dim1-1),scol+1:scol+opts.dim1-1) = -1;
    clinear(srow+9+2*(opts.dim1-1))= -l;

    Alinear(srow+10+2*(opts.dim1-1),scol+1:scol+opts.dim1-1) = 1;
    clinear(srow+10+2*(opts.dim1-1))= u;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Cone constraints
    crow=(i-2)*6;

    Acone(crow+1,scol+1+opts.dim1)= -1;
    Acone(crow+1,scol+2+opts.dim1) = -1;


    Acone(crow+2,scol+1:scol+opts.dim1-1) = 2*sqrt(l)/(u-l);
    ccone(crow+2) = 2*sqrt(l)*u/(u-l);

    Acone(crow+3,scol+1+opts.dim1)= -1;
    Acone(crow+3,scol+2+opts.dim1) = 1;

    %------------

    Acone(crow+4,scol+opts.dim1)= -1;
    Acone(crow+4,scol+1+opts.dim1)= 1;
    Acone(crow+4,scol+2+opts.dim1) = 1;
    Acone(crow+4,(opts.dim1-1)*opts.dim2+1:opts.dim1*opts.dim2 )=-X;

    Acone(crow+5,scol+1:scol+opts.dim1-1) = -2*sqrt(u)/(u-l);
    ccone(crow+5) = -2*sqrt(u)*l/(u-l);

    Acone(crow+6,scol+opts.dim1)= -1;
    Acone(crow+6,scol+1+opts.dim1)= 1;
    Acone(crow+6,scol+2+opts.dim1) = -1;
    Acone(crow+6,(opts.dim1-1)*opts.dim2+1:opts.dim1*opts.dim2 )=X;

    srow = srow+ 2*(opts.dim1-1) +10;
    scol = scol + 2+opts.dim1;


end
%keyboard;
opts.Alinear = sparse(Alinear);
opts.clinear = sparse(clinear);
opts.Acone = sparse(Acone);
opts.ccone = sparse(ccone);
opts.b = sparse(-b);
opts.K = K;
opts.A=sparse([opts.Alinear;opts.Acone]);
opts.c=sparse([opts.clinear;opts.ccone]);
%keyboard;