l2l2_triangulation.m

数学优化工具箱

function [U,iter]=l2l2_triangulation(u,P,epsperc,maxerror,maxiter);
%input u,P


fprintf('\n\n******** Starting (L2,L2) Triangulation Algorithm ********\n\n');

nbrimages=length(P);
%epsdiff=1e-7;
epsdiff=0.0;

if nargin<3 || isempty(epsperc),
    epsperc=0.05;
else;
	epsperc = 1-epsperc;
end
if nargin < 5;
  maxiter = 200;
end

if nargin<4,
    maxerror=15;
end

%fprintf('EPS: %f\n',epsperc);

Pt={};
%translate all image points to origin
cc=zeros(3,nbrimages);
for ii=1:nbrimages,
    Tim=eye(3);
    Tim(1:2,3)=-u(:,ii);
    Pt{ii}=Tim*P{ii};
    tmp=pflat(null(Pt{ii}));
    cc(:,ii)=tmp(1:3);
end


%rotate, scale & translate camera Pt{1} such that third row=[1,0,0,0]
% without effecting cost-function
v=Pt{1}(3,1:3)';
[uu,ss,vv]=svd(v);
RR=uu*sign(uu(1,:)*v);
t0=cc(:,1);

tmp=RR'*(cc(:,2:end)-t0*ones(1,nbrimages-1));
T=[RR*std(tmp(:)),t0;[0,0,0,1]];

imscale=0;
for ii=1:nbrimages,
    Pt{ii}=Pt{ii}*T;
    Pt{ii}=Pt{ii}/norm(Pt{ii}(3,:));
    imscale=imscale+norm(Pt{ii}(1:2,:));
end
imscale=imscale/nbrimages;
for ii=1:nbrimages,
    Pt{ii}(1:2,:)=Pt{ii}(1:2,:)/imscale;
end


P0=Pt{1};

%upper & lower bound...

thr = maxerror/imscale; %maximum number of pixels for a single term
yLB=0.1;yUB=10;
[rect_yL,rect_yU]=l2_triangulation_bound(Pt,thr,yLB,yUB);
xL=0;xU=0.1;

hh=l2_triangulation_loop(Pt,rect_yL,rect_yU,xL,xU,yLB,yUB);

vopt=sum(hh.res);

U=[1;hh.y(1:3)];

rect_LB=hh.lowerbound;
rect={hh};


iter=1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while iter <= maxiter, %Branch and Bound-loop
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   [vk,vkindex]=min(rect_LB);
   
   vdiff=(vopt-vk);
   perc=(vopt-vk)/vopt;
   
   disp(['Iter: ',num2str(iter),' Residual: ',num2str(vopt*imscale),' Approximation gap: ',num2str(perc*100),'% Regions: ',num2str(length(rect))]);

   if (vdiff<epsdiff) || (perc<epsperc)
       %'voila'
       break;
   end
   
   %branch on vkindex
   h=rect{vkindex};
%   index_z=3+nbrimages+[1:3:3*(nbrimages-1)]; %location of z_2,z_3,...,z_nbrimages
%   index_z=3+nbrimages+[1:3:3*(min(nbrimages,4)-1)]; %location of z_2,z_3,...,z_nbrimages

   %denominator to branch on
%   [slask,pp]=max(h.res(2:min(nbrimages,4))'-h.y(index_z));
%   [slask,pp]=max(h.res(2:nbrimages)'-h.y(index_z));
   [slask,pp]=max(rect_yU(:,vkindex)-rect_yL(:,vkindex)); %largest interval
   
   tmpyL=rect_yL(pp,vkindex);
   tmpyU=rect_yU(pp,vkindex);
   
   %branching strategy....
   
   bestsol=h.lambda(pp);
   alfa=0.1; %minimum shortage of interval relative original
   if (bestsol-tmpyL)/(tmpyU-tmpyL)<alfa,
       newborder=tmpyL+(tmpyU-tmpyL)*alfa;
   elseif (tmpyU-bestsol)/(tmpyU-tmpyL)<alfa;
       newborder=tmpyU-(tmpyU-tmpyL)*alfa;
   else
       newborder=bestsol;
   end
   
   
%   bisect=(tmpyU+tmpyL)/2;
%   bestsol=h.lambda(pp);
%   alfa=0.8;
%   newborder=bestsol*alfa+bisect*(1-alfa);
   
%   newborder=(tmpyU+tmpyL)/2; %bisection
   
%       newborder=h.lambda(pp); %best solution


   curr_yL1=rect_yL(:,vkindex);
   curr_yU1=rect_yU(:,vkindex);
   
   curr_yL2=curr_yL1;
   curr_yU2=curr_yU1;
   
   curr_yU1(pp)=newborder;
   curr_yL2(pp)=newborder;
   
   rect_yL=[rect_yL(:,1:vkindex-1),curr_yL1,curr_yL2,rect_yL(:,vkindex+1:end)];
   rect_yU=[rect_yU(:,1:vkindex-1),curr_yU1,curr_yU2,rect_yU(:,vkindex+1:end)];
   
   h1=l2_triangulation_loop(Pt,curr_yL1,curr_yU1,xL,xU,yLB,yUB);
   h2=l2_triangulation_loop(Pt,curr_yL2,curr_yU2,xL,xU,yLB,yUB);
   
   vopt1=sum(h1.res);
   vopt2=sum(h2.res);
   
   rect={rect{1:vkindex-1},h1,h2,rect{vkindex+1:end}};
   rect_LB=[rect_LB(1:vkindex-1),h1.lowerbound,h2.lowerbound,rect_LB(vkindex+1:end)];
   
   if vopt1<vopt,
       vopt=vopt1;
       U=[1;h1.y(1:3)];
   end
   if vopt2<vopt,
       vopt=vopt2;
       U=[1;h2.y(1:3)];
   end
   
   %screen and remove useless rectangles...
   removeindex=[];
   for ii=1:length(rect);
       if rect{ii}.lowerbound>vopt,
           %remove!
           removeindex(end+1)=ii;
       end
   end
   rect(removeindex)=[];
   rect_yL(:,removeindex)=[];
   rect_yU(:,removeindex)=[];
   rect_LB(removeindex)=[];
   
   iter=iter+1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end, %Branch and Bound-loop
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


U=pflat(T*U);
U=U(1:3);
fprintf('******** Ending (L2,L2) Triangulation Algorithm ********\n\n');