[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 | X=[];%default |
---|

| 14 | Y=[]; |
---|

| 15 | % if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'PlanePos')&length(Calib.PlanePos)>=Z |
---|

| 16 | % Zindex=Z; |
---|

| 17 | % planepos=Calib.PlanePos{Zindex}; |
---|

| 18 | % zphys=planepos(3);%A GENERALISER CAS AVEC ANGLE |
---|

| 19 | % else |
---|

| 20 | % zphys=0; |
---|

| 21 | % end |
---|

| 22 | if ~exist('Zphys','var') |
---|

| 23 | Zphys=0; |
---|

| 24 | end |
---|

[84] | 25 | if ~isfield(Calib,'f') |
---|

| 26 | Calib.f=1; |
---|

| 27 | end |
---|

| 28 | if ~isfield(Calib,'kappa1') |
---|

| 29 | Calib.kappa1=0; |
---|

| 30 | 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 |
---|

[40] | 46 | %%%%%%%%%%%%% |
---|

| 47 | if isfield(Calib,'R') |
---|

| 48 | 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; |
---|

| 52 | %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; |
---|

| 60 | %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; |
---|

[92] | 67 | end |
---|

| 68 | |
---|

| 69 | |
---|

| 70 | |
---|