px_XYZ

'px_XYZ': transform phys coordinates to image coordinates (px)

 OUPUT:
 X,Y: array of coordinates in the image cooresponding to the input physical positions 
                    (origin at lower leftcorner, unit=pixel)

0001 %'px_XYZ': transform phys coordinates to image coordinates (px)
0002 %
0003 % OUPUT:
0004 % X,Y: array of coordinates in the image cooresponding to the input physical positions
0005 %                    (origin at lower leftcorner, unit=pixel)
0006 
0007 % INPUT:
0008 % Calib: structure containing the calibration parameters (read from the ImaDoc .xml file)
0009 % Xphys, Yphys: array of x,y physical coordinates
0010 % [Zphys]: corresponding array of z physical coordinates (0 by default)
0011 
0012 
0013 function [X,Y]=px_XYZ(Calib,Xphys,Yphys,Zphys)
0014 X=[];%default
0015 Y=[];
0016 % if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'PlanePos')&length(Calib.PlanePos)>=Z
0017 %     Zindex=Z;
0018 %     planepos=Calib.PlanePos{Zindex};
0019 %     zphys=planepos(3);%A GENERALISER CAS AVEC ANGLE
0020 % else
0021 %     zphys=0;
0022 % end
0023 if ~exist('Zphys','var')
0024     Zphys=0;
0025 end
0026 
0027 %%%%%%%%%%%%%
0028 if isfield(Calib,'R')
0029     R=(Calib.R)';
0030     xc=R(1)*Xphys+R(2)*Yphys+R(3)*Zphys+Calib.Tx;
0031     yc=R(4)*Xphys+R(5)*Yphys+R(6)*Zphys+Calib.Ty;
0032     zc=R(7)*Xphys+R(8)*Yphys+R(9)*Zphys+Calib.Tz;
0033 %undistorted image coordinates
0034     Xu=Calib.f*xc./zc;
0035     Yu=Calib.f*yc./zc;
0036 %distorted image coordinates
0037     distortion=(Calib.kappa1)*(Xu.*Xu+Yu.*Yu)+1; %A REVOIR
0038 % distortion=1;
0039     Xd=Xu./distortion;
0040     Yd=Yu./distortion;
0041 %pixel coordinates
0042     X=Xd*Calib.sx/Calib.dpx+Calib.Cx;
0043     Y=Yd/Calib.dpy+Calib.Cy;
0044 
0045 elseif isfield(Calib,'Pxcmx')&isfield(Calib,'Pxcmy')%old calib
0046         X=Xphys*Calib.Pxcmx;
0047         Y=Yphys*Calib.Pxcmy;
0048 end
0049 
0050