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 |
---|
11 | |
---|
12 | function [X,Y]=px_XYZ(Calib,Xphys,Yphys,Zphys) |
---|
13 | if ~exist('Zphys','var') |
---|
14 | Zphys=0; |
---|
15 | end |
---|
16 | if ~isfield(Calib,'fx_fy') |
---|
17 | Calib.fx_fy=[1 1]; |
---|
18 | end |
---|
19 | if ~isfield(Calib,'Tx_Ty_Tz') |
---|
20 | Calib.Tx_Ty_Tz=[0 0 1]; |
---|
21 | end |
---|
22 | |
---|
23 | %%%%%%%%%%%%% |
---|
24 | % general case |
---|
25 | if isfield(Calib,'R') |
---|
26 | R=(Calib.R)'; |
---|
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 | |
---|
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); |
---|
40 | |
---|
41 | %undistorted image coordinates |
---|
42 | Xu=xc./zc; |
---|
43 | Yu=yc./zc; |
---|
44 | |
---|
45 | %radial quadratic correction factor |
---|
46 | if ~isfield(Calib,'kc') |
---|
47 | r2=1; %no quadratic distortion |
---|
48 | else |
---|
49 | r2=1+Calib.kc*(Xu.*Xu+Yu.*Yu); |
---|
50 | end |
---|
51 | |
---|
52 | %pixel coordinates |
---|
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); |
---|
57 | Y=Calib.fx_fy(2)*Yu.*r2+Calib.Cx_Cy(2); |
---|
58 | |
---|
59 | %case 'rescale' |
---|
60 | else |
---|
61 | X=Calib.fx_fy(1)*(Xphys+Calib.Tx_Ty_Tz(1)); |
---|
62 | Y=Calib.fx_fy(2)*(Yphys+Calib.Tx_Ty_Tz(2)); |
---|
63 | end |
---|
64 | |
---|
65 | |
---|
66 | |
---|