[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('Zphys','var') |
---|

| 14 | Zphys=0; |
---|

| 15 | end |
---|

[114] | 16 | if ~isfield(Calib,'fx_fy') |
---|

| 17 | Calib.fx_fy=[1 1]; |
---|

[84] | 18 | end |
---|

[114] | 19 | if ~isfield(Calib,'Tx_Ty_Tz') |
---|

| 20 | Calib.Tx_Ty_Tz=[0 0 1]; |
---|

[84] | 21 | end |
---|

[114] | 22 | |
---|

[40] | 23 | %%%%%%%%%%%%% |
---|

[542] | 24 | % general case |
---|

[40] | 25 | if isfield(Calib,'R') |
---|

| 26 | R=(Calib.R)'; |
---|

[542] | 27 | %correct z for refraction if needed |
---|

| 28 | if isfield(Calib,'InterfaceCoord') && isfield(Calib,'RefractionIndex') |
---|

| 29 | H=Calib.InterfaceCoord(3); |
---|

| 30 | if H>Zphys |
---|

| 31 | Zphys=H-(H-Zphys)/Calib.RefractionIndex; %corrected z (virtual object) |
---|

| 32 | test_refraction=1; |
---|

| 33 | end |
---|

| 34 | end |
---|

| 35 | |
---|

[114] | 36 | %camera coordinates |
---|

| 37 | xc=R(1)*Xphys+R(2)*Yphys+R(3)*Zphys+Calib.Tx_Ty_Tz(1); |
---|

| 38 | yc=R(4)*Xphys+R(5)*Yphys+R(6)*Zphys+Calib.Tx_Ty_Tz(2); |
---|

| 39 | zc=R(7)*Xphys+R(8)*Yphys+R(9)*Zphys+Calib.Tx_Ty_Tz(3); |
---|

[542] | 40 | |
---|

| 41 | %undistorted image coordinates |
---|

[114] | 42 | Xu=xc./zc; |
---|

| 43 | Yu=yc./zc; |
---|

[542] | 44 | |
---|

| 45 | %radial quadratic correction factor |
---|

[114] | 46 | if ~isfield(Calib,'kc') |
---|

| 47 | r2=1; %no quadratic distortion |
---|

| 48 | else |
---|

| 49 | r2=1+Calib.kc*(Xu.*Xu+Yu.*Yu); |
---|

| 50 | end |
---|

[542] | 51 | |
---|

| 52 | %pixel coordinates |
---|

[114] | 53 | if ~isfield(Calib,'Cx_Cy') |
---|

| 54 | Calib.Cx_Cy=[0 0];%default value |
---|

| 55 | end |
---|

| 56 | X=Calib.fx_fy(1)*Xu.*r2+Calib.Cx_Cy(1); |
---|

[542] | 57 | Y=Calib.fx_fy(2)*Yu.*r2+Calib.Cx_Cy(2); |
---|

| 58 | |
---|

| 59 | %case 'rescale' |
---|

| 60 | else |
---|

[114] | 61 | X=Calib.fx_fy(1)*(Xphys+Calib.Tx_Ty_Tz(1)); |
---|

| 62 | Y=Calib.fx_fy(2)*(Yphys+Calib.Tx_Ty_Tz(2)); |
---|

[92] | 63 | end |
---|

| 64 | |
---|

| 65 | |
---|

| 66 | |
---|