Changeset 114 for trunk/src/px_XYZ.m
- Timestamp:
- Oct 14, 2010, 6:35:03 PM (13 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/src/px_XYZ.m
r109 r114 11 11 12 12 function [X,Y]=px_XYZ(Calib,Xphys,Yphys,Zphys) 13 X=[];%default14 Y=[];15 13 % if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'PlanePos')&length(Calib.PlanePos)>=Z 16 14 % Zindex=Z; … … 23 21 Zphys=0; 24 22 end 25 if ~isfield(Calib,'f ')26 Calib.f=1;23 if ~isfield(Calib,'fx_fy') 24 Calib.fx_fy=[1 1]; 27 25 end 28 if ~isfield(Calib,' kappa1')29 Calib.kappa1=0;26 if ~isfield(Calib,'Tx_Ty_Tz') 27 Calib.Tx_Ty_Tz=[0 0 1]; 30 28 end 31 if ~isfield(Calib,'sx') 32 Calib.sx=1; 33 end 34 if ~isfield(Calib,'dpx') 35 Calib.dpx=1; 36 end 37 if ~isfield(Calib,'dpy') 38 Calib.dpy=1; 39 end 40 if ~isfield(Calib,'Cx') 41 Calib.Cx=0; 42 end 43 if ~isfield(Calib,'Cy') 44 Calib.Cy=0; 45 end 29 % if ~isfield(Calib,'kappa1') 30 % Calib.kappa1=0; 31 % end 32 % if ~isfield(Calib,'sx') 33 % Calib.sx=1; 34 % end 35 % if ~isfield(Calib,'dpx') 36 % Calib.dpx=1; 37 % end 38 % if ~isfield(Calib,'dpy') 39 % Calib.dpy=1; 40 % end 41 46 42 %%%%%%%%%%%%% 47 43 if isfield(Calib,'R') 48 44 R=(Calib.R)'; 49 xc=R(1)*Xphys+R(2)*Yphys+R(3)*Zphys+Calib.Tx; 50 yc=R(4)*Xphys+R(5)*Yphys+R(6)*Zphys+Calib.Ty; 51 zc=R(7)*Xphys+R(8)*Yphys+R(9)*Zphys+Calib.Tz; 45 %camera coordinates 46 xc=R(1)*Xphys+R(2)*Yphys+R(3)*Zphys+Calib.Tx_Ty_Tz(1); 47 yc=R(4)*Xphys+R(5)*Yphys+R(6)*Zphys+Calib.Tx_Ty_Tz(2); 48 zc=R(7)*Xphys+R(8)*Yphys+R(9)*Zphys+Calib.Tx_Ty_Tz(3); 52 49 %undistorted image coordinates 53 Xu=Calib.f*xc./zc; 54 Yu=Calib.f*yc./zc; 55 %distorted image coordinates 56 distortion=(Calib.kappa1)*(Xu.*Xu+Yu.*Yu)+1; %A REVOIR 57 % distortion=1; 58 Xd=Xu./distortion; 59 Yd=Yu./distortion; 50 Xu=xc./zc; 51 Yu=yc./zc; 52 %radial quadratic correction factor 53 if ~isfield(Calib,'kc') 54 r2=1; %no quadratic distortion 55 else 56 r2=1+Calib.kc*(Xu.*Xu+Yu.*Yu); 57 end 60 58 %pixel coordinates 61 X=Xd*Calib.sx/Calib.dpx+Calib.Cx; 62 Y=Yd/Calib.dpy+Calib.Cy; 63 64 elseif isfield(Calib,'Pxcmx')&isfield(Calib,'Pxcmy')%old calib 65 X=Xphys*Calib.Pxcmx; 66 Y=Yphys*Calib.Pxcmy; 59 if ~isfield(Calib,'Cx_Cy') 60 Calib.Cx_Cy=[0 0];%default value 61 end 62 X=Calib.fx_fy(1)*Xu.*r2+Calib.Cx_Cy(1); 63 Y=Calib.fx_fy(2)*Yu.*r2+Calib.Cx_Cy(2); 64 %OLD CONVENTION (Wilson)undistorted image coordinates 65 % Xu=Calib.f*xc./zc; 66 % Yu=Calib.f*yc./zc; 67 % %distorted image coordinates 68 % distortion=(Calib.kappa1)*(Xu.*Xu+Yu.*Yu)+1; %A REVOIR 69 % % distortion=1; 70 % Xd=Xu./distortion; 71 % Yd=Yu./distortion; 72 % %pixel coordinates 73 % X=Xd*Calib.sx/Calib.dpx+Calib.Cx; 74 % Y=Yd/Calib.dpy+Calib.Cy; 75 else %case 'rescale' 76 X=Calib.fx_fy(1)*(Xphys+Calib.Tx_Ty_Tz(1)); 77 Y=Calib.fx_fy(2)*(Yphys+Calib.Tx_Ty_Tz(2)); 67 78 end 68 79
Note: See TracChangeset
for help on using the changeset viewer.