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