[109] | 1 | %'px_XYZ': transform physical to image coordinates. |
---|
| 2 | %------------------------------------------------------------------------ |
---|
| 3 | %[X,Y]=px_XYZ(Calib,Xphys,Yphys,Zphys) |
---|
| 4 | %------------------------------------------------------------------------ |
---|
| 5 | % OUTPUT: |
---|
| 6 | % [X,Y]: image coordinates(in pixels) |
---|
| 7 | %------------------------------------------------------------------------ |
---|
| 8 | % INPUT: |
---|
| 9 | % Calib: structure containing calibration parameters |
---|
| 10 | % Xphys,Yphys,Zphys; vectors of physical coordinates for a set of points |
---|
[40] | 11 | |
---|
| 12 | function [X,Y]=px_XYZ(Calib,Xphys,Yphys,Zphys) |
---|
| 13 | % if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'PlanePos')&length(Calib.PlanePos)>=Z |
---|
| 14 | % Zindex=Z; |
---|
| 15 | % planepos=Calib.PlanePos{Zindex}; |
---|
| 16 | % zphys=planepos(3);%A GENERALISER CAS AVEC ANGLE |
---|
| 17 | % else |
---|
| 18 | % zphys=0; |
---|
| 19 | % end |
---|
| 20 | if ~exist('Zphys','var') |
---|
| 21 | Zphys=0; |
---|
| 22 | end |
---|
[114] | 23 | if ~isfield(Calib,'fx_fy') |
---|
| 24 | Calib.fx_fy=[1 1]; |
---|
[84] | 25 | end |
---|
[114] | 26 | if ~isfield(Calib,'Tx_Ty_Tz') |
---|
| 27 | Calib.Tx_Ty_Tz=[0 0 1]; |
---|
[84] | 28 | end |
---|
[114] | 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 | |
---|
[40] | 42 | %%%%%%%%%%%%% |
---|
| 43 | if isfield(Calib,'R') |
---|
| 44 | R=(Calib.R)'; |
---|
[114] | 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); |
---|
[40] | 49 | %undistorted image coordinates |
---|
[114] | 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 |
---|
[40] | 58 | %pixel coordinates |
---|
[114] | 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)); |
---|
[92] | 78 | end |
---|
| 79 | |
---|
| 80 | |
---|
| 81 | |
---|