l1l1_triangle_init.m

数学优化工具箱

function opts=l1l1_triangle_init(opts);

%opts.CameraArray
%opts.PointsArray
%opts.xbounds
%opts.ybounds
%opts.e
%opts.epsilon


if size(opts.CameraArray,1) ~= size(opts.PointsArray,1)
    fprintf('Number of frames and number of cameras do not match\n');
end

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

% build Q matrices
opts.Q = zeros(size(opts.CameraArray));
for i = 1:opts.n;
    opts.Q(i,1,:)=opts.CameraArray(i,3,:)*opts.PointsArray(i,1) - opts.CameraArray(i,1,:);
    opts.Q(i,2,:)=opts.CameraArray(i,3,:)*opts.PointsArray(i,2) - opts.CameraArray(i,2,:);
    opts.Q(i,3,:)=opts.CameraArray(i,3,:);
end

opts = gamma_initialize(opts);



% construct the objective variable
% order of variables
% x,y,z,[t1,t2,r,a,b]
% 3+ 5*n

% more efficient construction later on


Alinear = zeros(14*opts.n,3+5*opts.n);
clinear = zeros(14*opts.n,1);
Acone = zeros(6*opts.n, 3+5*opts.n);
ccone = zeros(6*opts.n,1);

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

b=zeros(3,1);
btemplate=zeros(5,1);
btemplate(3)=1;

for i = 1:opts.n;
    l=opts.xbounds(i,1);
    u=opts.xbounds(i,2);

    L=opts.ybounds(i,1);
    U=opts.ybounds(i,2);


    b =   cat(1,b,btemplate);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % The linear constraints
    srow=(i-1)*14;
    scol=(i-1)*5+3;

    Alinear(srow+1,scol+1)=-L;
    Alinear(srow+1,scol+2)=-L;
    Alinear(srow+1,scol+5)=-(u-l);
    clinear(srow+1) = -L*u;

    Alinear(srow+2,1) = (u-l)*opts.Q(i,3,1);
    Alinear(srow+2,2) = (u-l)*opts.Q(i,3,2);
    Alinear(srow+2,3) = (u-l)*opts.Q(i,3,3);
    Alinear(srow+2,scol+1) = -U;
    Alinear(srow+2,scol+2) = -U;
    Alinear(srow+2,scol+5) = -(u-l);
    clinear(srow+2) = -(u-l)*opts.Q(i,3,4)-U*l;


    Alinear(srow+3,scol+1)=U;
    Alinear(srow+3,scol+2)=U;
    Alinear(srow+3,scol+5)=(u-l);
    clinear(srow+3) = U*u;


    Alinear(srow+4,1) = -(u-l)*opts.Q(i,3,1);
    Alinear(srow+4,2) = -(u-l)*opts.Q(i,3,2);
    Alinear(srow+4,3) = -(u-l)*opts.Q(i,3,3);
    Alinear(srow+4,scol+1) = L;
    Alinear(srow+4,scol+2) = L;
    Alinear(srow+4,scol+5) = (u-l);
    clinear(srow+4) = (u-l)*opts.Q(i,3,4) + L*l;

    Alinear(srow+5,scol+4)=-1;
    clinear(srow+5) = 0;

    Alinear(srow+6,scol+3)=-1;
    Alinear(srow+6,scol+4)= 1;
    clinear(srow+6) = 0;

    Alinear(srow+7,1:3)=-opts.Q(i,1,1:3);
    Alinear(srow+7,scol+1)=-1;
    clinear(srow+7) = opts.Q(i,1,4);

    Alinear(srow+8,1:3)=opts.Q(i,1,1:3);
    Alinear(srow+8,scol+1)=-1;
    clinear(srow+8) = -opts.Q(i,1,4);

    Alinear(srow+9,1:3)=-opts.Q(i,2,1:3);
    Alinear(srow+9,scol+2)=-1;
    clinear(srow+9) = opts.Q(i,2,4);

    Alinear(srow+10,1:3)=opts.Q(i,2,1:3);
    Alinear(srow+10,scol+2)=-1;
    clinear(srow+10) = -opts.Q(i,2,4);

    Alinear(srow+11,1:3)=-opts.Q(i,3,1:3);
    clinear(srow+11) = -L+opts.Q(i,3,4);


    Alinear(srow+12,1:3)=opts.Q(i,3,1:3);
    clinear(srow+12) = U-opts.Q(i,3,4);


    Alinear(srow+13,scol+1) = -1;
    Alinear(srow+13,scol+2) = -1;
    clinear(srow+13) = -l;

    Alinear(srow+14,scol+1) = 1;
    Alinear(srow+14,scol+2) = 1;
    clinear(srow+14)= u;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Cone constraints
    srow=(i-1)*6;
    scol=(i-1)*5+3;

    Acone(srow+1,scol+4) = -1;
    Acone(srow+1,scol+5) = -1;
    %   scone(srow+1) = 0;

    Acone(srow+2,scol+1) = 2*sqrt(l)/(u-l);
    Acone(srow+2,scol+2) = 2*sqrt(l)/(u-l);
    ccone(srow+2) = 2*sqrt(l)*u/(u-l);

    Acone(srow+3,scol+4) = -1;
    Acone(srow+3,scol+5) = 1;
    %  scone(srow+3) = 0;

    Acone(srow+4,1:3) = -opts.Q(i,3,1:3);
    Acone(srow+4,scol+3) = -1;
    Acone(srow+4,scol+4) = 1;
    Acone(srow+4,scol+5) = 1;
    ccone(srow+4) = opts.Q(i,3,4);

    Acone(srow+5,scol+1) = -2*sqrt(u)/(u-l);
    Acone(srow+5,scol+2) = -2*sqrt(u)/(u-l);
    ccone(srow+5) = -2*sqrt(u)*l/(u-l);

    Acone(srow+6,1:3) = opts.Q(i,3,1:3);
    Acone(srow+6,scol+3) = -1;
    Acone(srow+6,scol+4) = 1;
    Acone(srow+6,scol+5) = -1;
    ccone(srow+6) = -opts.Q(i,3,4);

end

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