Audio-Visual Person Tracking A Practical Approach

% Sample proposal distribution for updated particles % % 1. Gaussian (random walk) component Lambda=Lambda0*sqrt(target.xo(3)*target.xo(4)); lamdac=sqrt(Lambda(3)*Lambda(4)); C=diag(Lambda.^2); C(3,4)=lamdac;C(4,3)=lamdac; x=x+sqrt(C)*randn(size(x,1),Np); if motion==1 x(1,:)=x(1,:)+x(5,:); x(2,:)=x(2,:)+x(6,:); end if proposal==1 && bu>0 % 2. Uniform component if motion==0 xu=diag([size(I,2)/2 size(I,1)/2 target.xo(3)/20 target.xo(4)/20])*(rand(size(x,1),Np)-.5); else xu=diag([size(I,2)/2 size(I,1)/2 target.xo(3)/20 target.xo(4)/20 2*target.xo(5:6)'])*(rand(size(x,1),Np)-.5); end xu=xu+repmat(target.xo,1,Np); idxNG=find(rand(1,Np)<bu); x(:,idxNG)=xu(:,idxNG); elseif proposal==2 % 2. Grid component % 2.a. Define grid num=[2 2 2 2]; step=[.5 .5 .04 .04]'.*[target.xo(3) target.xo(4) target.xo(3) target.xo(4)]'; for i1=1:4 g{i1}=round(target.xo(i1)-step(i1)*num(i1):step(i1):target.xo(i1)+step(i1)*num(i1)); end % 2.b. Measurement likelihood on grid pyx=zeros(length(g{1}),length(g{2}),length(g{3}),length(g{4})); for i1=1:length(g{1}) for i2=1:length(g{2}) for i3=1:length(g{3}) for i4=1:length(g{4}) bb=[g{1}(i1) g{2}(i2) g{3}(i3) g{4}(i4)]; if bb(1)>=1 && bb(1)+bb(3)-1<=size(I,2) && bb(2)>=1 && bb(2)+bb(4)-1<=size(I,1) Hface=histogram(I(bb(2):bb(2)+bb(4)-1,bb(1):bb(1)+bb(3)-1,:),NhRJ); pyx(i1,i2,i3,i3)=exp(-(1-sum(sqrt(Hface.*target.Hface)))/(2*s2c)); end end end end end % 2.c. Find good grid points and draw particles around them ig=find(pyx>pOMthr*max(pOM)); Ng=length(ig); xg=randn(length(target.xo),Np,Ng); idxNG=[]; if Ng>0 [ig1 ig2 ig3 ig4]=ind2sub(size(pyx),ig); for ig=1:Ng if motion==0 xg(:,:,ig)=sqrt(C/fgrid)*xg(:,:,ig)+repmat([g{1}(ig1(ig));g{2}(ig2(ig));g{3}(ig3(ig));g{4}(ig4(ig))],1,Np); else xg(:,:,ig)=sqrt(C/fgrid)*xg(:,:,ig)+repmat([g{1}(ig1(ig));g{2}(ig2(ig));g{3}(ig3(ig));g{4}(ig4(ig));0;0],1,Np); end end % Select a sample from x with probability bRW or from one of the xg % with probability (1-bRW)/Ng probs=cumsum([bRW (1-bRW)/Ng*ones(1,Ng)])'; probs=[0;probs(1:end-1)]; idx=rand(1,Np); for ig=1:Np t=find(repmat(idx(ig),length(probs),1)-probs>=0); idx(ig)=t(end); if idx(ig)>1 x(:,ig)=xg(:,ig,idx(ig)-1); idxNG=cat(2,idxNG,ig); end end end end % Make sure centre remains inside image x(1,x(1,:)<1)=1; x(2,x(2,:)<1)=1; x(3,x(3,:)<1)=1; x(4,x(4,:)<1)=1; x(1,x(1,:)>size(I,2))=size(I,2); x(2,x(2,:)>size(I,1))=size(I,1); % Weight update using color for state update % 1. Evaluate color matching likelihood model pOM=zeros(1,Np); for i1=1:Np bb=round([x(1,i1)-ceil(x(3,i1)/2)+1;x(2,i1)-ceil(x(4,i1)/2)+1;x(3:4,i1)]); bb(bb<1)=1; if bb(1)+bb(3)>size(I,2) bb(3)=size(I,2)-bb(1)+1; end if bb(2)+bb(4)>size(I,1) bb(4)=size(I,1)-bb(2)+1; end if bb(3)>5 && bb(4)>5 Hface=histogram(I(bb(2):bb(2)+bb(4)-1,bb(1):bb(1)+bb(3)-1,:),NhRJ); pOM(i1)=exp(-(1-sum(sqrt(Hface.*target.Hface)))/(2*s2c)); end end % 2. New weights w=w.*pOM; if sum(w)>0 w=w/sum(w); else w=repmat(1/Np,1,Np); end % Resample % if resamplingMethod==1 a=rand(1,Np); wc=cumsum(w); for i1=1:Np t=find(a(i1)-wc<=0); a(i1)=t(1); end target.x=x(:,a); else c=cumsum(w); u=rand(1)/Np; xr=zeros(size(x)); j=1; for i=1:Np while c(j)<u j=j+1; end xr(:,i)=x(:,j); u=u+1/Np; end target.x=xr; end target.w=repmat(1/Np,1,Np);