function  out=getWorldCoordsB(I,yp,ithresh)
% reads in cam.txt and proj.txt and processes yp, out is XYZ of a mat5
cam=textread('cam.txt','%f');
cam=reshape(cam,[4,3]); cam=cam';
proj=textread('proj.txt','%f');
proj=reshape(proj,[4,2]);
proj=proj';


camMatrix=cam;
projMatrix=proj;

[M N]=size(yp);
d=double(camMatrix);
p=double(projMatrix);
yp=double(yp);

X=zeros(M,N);
Y=X;
Z=X;

mwc=d;
mwp=p;

for n=1:N
    for m=1:M
        if I(m,n)>=ithresh
            xc=double(n);
            yc=double(m);
            

            c(1,1)=mwc(1,1)-mwc(3,1)*xc;
            c(1,2)=mwc(1,2)-mwc(3,2)*xc; 
            c(1,3)=mwc(1,3)-mwc(3,3)*xc;

            c(2,1)=mwc(2,1)-mwc(3,1)*yc;
            c(2,2)=mwc(2,2)-mwc(3,2)*yc;
            c(2,3)=mwc(2,3)-mwc(3,3)*yc;

            c(3,1)=mwp(1,1)-mwp(2,1)*yp(m,n);
            c(3,2)=mwp(1,2)-mwp(2,2)*yp(m,n); 
            c(3,3)=mwp(1,3)-mwp(2,3)*yp(m,n);

            d(1,1)=xc-mwc(1,4);
            d(2,1)=yc-mwc(2,4);
            d(3,1)=yp(m,n)-mwp(1,4);

            P=inv(c)*d;

            
            X(m,n)=P(1);
            Y(m,n)=P(2);
            Z(m,n)=P(3);
        else
            X(m,n)=0;
            Y(m,n)=0;
            Z(m,n)=0;
        end
        
%         if floor(Y(m,n))>22  && floor(Y(m,n))<28 && floor(X(m,n))>52 && floor(X(m,n))<60
%             testingthing=[X(m,n), Y(m,n), Z(m,n)]
%         end        
        
%           if n>1000 && n<1100 && m>200 && m<300 && X(m,n)~=0
%               testingthing=[X(m,n), Y(m,n), Z(m,n), yp(m,n)]
%           end
        
    end
end

out=zeros(M,N,3);
out(:,:,1)=X(:,:);
out(:,:,2)=Y(:,:);
out(:,:,3)=Z(:,:);