phys_XYZ

'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters
 function [Xphys,Yphys]=phys_XYZ(Calib,X,Y,Z)

OUTPUT:

INPUT:
Z: index of plane

0001 %'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters
0002 % function [Xphys,Yphys]=phys_XYZ(Calib,X,Y,Z)
0003 %
0004 %OUTPUT:
0005 %
0006 %INPUT:
0007 %Z: index of plane
0008 function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Z)
0009 if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'SliceCoord')&length(Calib.SliceCoord)>=Z
0010     Zindex=Z;
0011     Zphys=Calib.SliceCoord(Zindex,3);%GENERALISER AUX CAS AVEC ANGLE
0012 else
0013 %     if exist('Z','var')
0014 %         Zphys=Z;
0015 %     else
0016         Zphys=0;
0017 %     end
0018 end
0019 if ~exist('X','var')|~exist('Y','var')
0020     Xphys=[];
0021     Yphys=[];%default
0022     return
0023 end
0024 Xphys=X;%default
0025 Yphys=Y;
0026 %image transform
0027 if isfield(Calib,'R')
0028     R=(Calib.R)';
0029     Dx=R(5)*R(7)-R(4)*R(8);
0030     Dy=R(1)*R(8)-R(2)*R(7);
0031     D0=Calib.f*(R(2)*R(4)-R(1)*R(5));
0032     Z11=R(6)*R(8)-R(5)*R(9);
0033     Z12=R(2)*R(9)-R(3)*R(8);  
0034     Z21=R(4)*R(9)-R(6)*R(7);
0035     Z22=R(3)*R(7)-R(1)*R(9);
0036     Zx0=R(3)*R(5)-R(2)*R(6);
0037     Zy0=R(1)*R(6)-R(3)*R(4);
0038     A11=R(8)*Calib.Ty-R(5)*Calib.Tz+Z11*Zphys;
0039     A12=R(2)*Calib.Tz-R(8)*Calib.Tx+Z12*Zphys;
0040     A21=-R(7)*Calib.Ty+R(4)*Calib.Tz+Z21*Zphys;
0041     A22=-R(1)*Calib.Tz+R(7)*Calib.Tx+Z11*Zphys;
0042     X0=Calib.f*(R(5)*Calib.Tx-R(2)*Calib.Ty+Zx0*Zphys);
0043     Y0=Calib.f*(-R(4)*Calib.Tx+R(1)*Calib.Ty+Zy0*Zphys);
0044         %px to camera:
0045     Xd=(Calib.dpx/Calib.sx)*(X-Calib.Cx); % sensor coordinates
0046     Yd=Calib.dpy*(Y-Calib.Cy);
0047     dist_fact=1+Calib.kappa1*(Xd.*Xd+Yd.*Yd); %distortion factor
0048     Xu=dist_fact.*Xd;%undistorted sensor coordinates
0049     Yu=dist_fact.*Yd;
0050     denom=Dx*Xu+Dy*Yu+D0;
0051     % denom2=denom.*denom;
0052     Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates
0053     Yphys=(A21.*Xu+A22.*Yu+Y0)./denom;
0054 end