[2]  1  %'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters


 2  % function [Xphys,Yphys]=phys_XYZ(Calib,X,Y,Z)


 3  %


 4  %OUTPUT:


 5  %


 6  %INPUT:


 7  %Z: index of plane


 8  function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Z)


 9  if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'SliceCoord')&length(Calib.SliceCoord)>=Z


 10  Zindex=Z;


 11  Zphys=Calib.SliceCoord(Zindex,3);%GENERALISER AUX CAS AVEC ANGLE


 12  else


 13  % if exist('Z','var')


 14  % Zphys=Z;


 15  % else


 16  Zphys=0;


 17  % end


 18  end


 19  if ~exist('X','var')~exist('Y','var')


 20  Xphys=[];


 21  Yphys=[];%default


 22  return


 23  end


 24  Xphys=X;%default


 25  Yphys=Y;


 26  %image transform


 27  if isfield(Calib,'R')


 28  R=(Calib.R)';


 29  Dx=R(5)*R(7)R(4)*R(8);


 30  Dy=R(1)*R(8)R(2)*R(7);


 31  D0=Calib.f*(R(2)*R(4)R(1)*R(5));


 32  Z11=R(6)*R(8)R(5)*R(9);


 33  Z12=R(2)*R(9)R(3)*R(8);


 34  Z21=R(4)*R(9)R(6)*R(7);


 35  Z22=R(3)*R(7)R(1)*R(9);


 36  Zx0=R(3)*R(5)R(2)*R(6);


 37  Zy0=R(1)*R(6)R(3)*R(4);


 38  A11=R(8)*Calib.TyR(5)*Calib.Tz+Z11*Zphys;


 39  A12=R(2)*Calib.TzR(8)*Calib.Tx+Z12*Zphys;


 40  A21=R(7)*Calib.Ty+R(4)*Calib.Tz+Z21*Zphys;


 41  A22=R(1)*Calib.Tz+R(7)*Calib.Tx+Z11*Zphys;


 42  X0=Calib.f*(R(5)*Calib.TxR(2)*Calib.Ty+Zx0*Zphys);


 43  Y0=Calib.f*(R(4)*Calib.Tx+R(1)*Calib.Ty+Zy0*Zphys);


 44  %px to camera:


 45  Xd=(Calib.dpx/Calib.sx)*(XCalib.Cx); % sensor coordinates


 46  Yd=Calib.dpy*(YCalib.Cy);


 47  dist_fact=1+Calib.kappa1*(Xd.*Xd+Yd.*Yd); %distortion factor


 48  Xu=dist_fact.*Xd;%undistorted sensor coordinates


 49  Yu=dist_fact.*Yd;


 50  denom=Dx*Xu+Dy*Yu+D0;


 51  % denom2=denom.*denom;


 52  Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates


 53  Yphys=(A21.*Xu+A22.*Yu+Y0)./denom;


 54  end

