l2_triangulation_bound.m

数学优化工具箱

function [rect_yL,rect_yU]=l2_triangulation_bound(Pt,thr,yLB,yUB);

nbrimages=length(Pt);
P0=Pt{1};


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% OPTIMIZATION - bounds
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%variables: x1,x2,x3, a2,...,an
vars=2+nbrimages;


%sedumi matrices
At_l=sparse(zeros(0,vars)); %linear inequalities
c_l=sparse(zeros(0,1));
At=sparse(zeros(0,vars)); %cone constraints
c=sparse(zeros(0,1));
clear K;
K.l=0;
K.q=[];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%first residual
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%3-cone
Atmp=sparse(zeros(3,vars));
ctmp=sparse(zeros(3,1));

%radius
ctmp(1)=thr;

%coefficients

%f1=u11*1-p1'*U
%f2=u21*1-p2'*U
Atmp(2:3,1:3)=P0(1:2,2:4);
ctmp(2:3)=-P0(1:2,1);

At=[At;Atmp];
c=[c;ctmp];
K.q=[K.q,3];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%second residual + remaining...
%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for cnt=1:nbrimages-1,
    %indexing...
    indexa=3+cnt;   %indexa=a2

    %cone constraint for a2

    Ptmp=Pt{cnt+1}; %second camera

    %4-cone
    Atmp=sparse(zeros(4,vars));
    ctmp=sparse(zeros(4,1));

    %radius: lambda+a2
    %lambda=p3*U

    Atmp(1,1:3)=-Ptmp(3,2:4);
    Atmp(1,indexa)=-1; %a2
    ctmp(1)=Ptmp(3,1);

    %coefficients

    %f1=u11*lambda-p1'*U
    %f2=u21*lambda-p2'*U
    Atmp(2:3,1:3)=2*Ptmp(1:2,2:4);
    ctmp(2:3)=-2*Ptmp(1:2,1);

    %p3'*U-a2
    Atmp(4,1:3)=-Ptmp(3,2:4);
    Atmp(4,indexa)=1;
    ctmp(4)=Ptmp(3,1);

    At=[At;Atmp];
    c=[c;ctmp];
    K.q=[K.q,4];
    
    %linear inequalities...
    %thr^2*lambda_i-alpha_i>=0
    Atmp=sparse(zeros(1,vars));
    ctmp=sparse(zeros(1,1));
    
    Atmp(1,1:3)=-Ptmp(3,2:4)*thr^2;
    Atmp(1,indexa)=1;
    ctmp(1)=thr^2*Ptmp(3,1);

    At_l=[At_l;Atmp];
    c_l=[c_l;ctmp];
    K.l=K.l+1;
    
    %depth should be positive
    Atmp=sparse(zeros(1,vars));
    ctmp=sparse(zeros(1,1));
    Atmp(1,1:3)=-Ptmp(3,2:4)';
    ctmp(1)=Ptmp(3,1)-yLB; %homogeneous one of Ptmp
    At_l=[At_l;Atmp];
    c_l=[c_l;ctmp];
    K.l=K.l+1;
    
    %depth should be less than yUB
    Atmp=sparse(zeros(1,vars));
    ctmp=sparse(zeros(1,1));
    Atmp(1,1:3)=Ptmp(3,2:4)';
    ctmp(1)=yUB-Ptmp(3,1); %homogeneous one of Ptmp
    At_l=[At_l;Atmp];
    c_l=[c_l;ctmp];
    K.l=K.l+1;
    
end %cnt


b=sparse(zeros(vars,1));
pars=[];
pars.fid=0;


n=min(nbrimages-1,3);
rect_yL = zeros(n,1);
rect_yU = zeros(n,1);

for ii=1:n,
    Ptmp=Pt{ii+1};
    b(1:3)=Ptmp(3,2:4)';
    

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % SEDUMI

    %parallell to first image plane
    if norm(Ptmp(3,:)-[1,0,0,0])<1e-10,
        rect_yL(ii)=1;
        rect_yU(ii)=1;
    else
        [x,yU,infoU]=sedumi([At_l;At],b,[c_l;c],K,pars);
        [x,yL,infoL]=sedumi([At_l;At],-b,[c_l;c],K,pars);
    
        if infoU.numerr==1 || infoL.numerr==1,
            rect_yL(ii)=yLB;
            rect_yU(ii)=yUB;
        else
            rect_yL(ii)=max(b'*yL+Ptmp(3,1),yLB);
            rect_yU(ii)=min(b'*yU+Ptmp(3,1),yUB);
        end
    end
    
end