1 | %------------------------------------------------------------------------ |
---|
2 | %'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters |
---|
3 | % function [Xphys,Yphys]=phys_XYZ(Calib,X,Y,Z) |
---|
4 | % |
---|
5 | %OUTPUT: |
---|
6 | % |
---|
7 | %INPUT: |
---|
8 | %Z: index of plane |
---|
9 | |
---|
10 | %======================================================================= |
---|
11 | % Copyright 2008-2018, LEGI UMR 5519 / CNRS UGA G-INP, Grenoble, France |
---|
12 | % http://www.legi.grenoble-inp.fr |
---|
13 | % Joel.Sommeria - Joel.Sommeria (A) legi.cnrs.fr |
---|
14 | % |
---|
15 | % This file is part of the toolbox UVMAT. |
---|
16 | % |
---|
17 | % UVMAT is free software; you can redistribute it and/or modify |
---|
18 | % it under the terms of the GNU General Public License as published |
---|
19 | % by the Free Software Foundation; either version 2 of the license, |
---|
20 | % or (at your option) any later version. |
---|
21 | % |
---|
22 | % UVMAT is distributed in the hope that it will be useful, |
---|
23 | % but WITHOUT ANY WARRANTY; without even the implied warranty of |
---|
24 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
---|
25 | % GNU General Public License (see LICENSE.txt) for more details. |
---|
26 | %======================================================================= |
---|
27 | |
---|
28 | function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Zindex) |
---|
29 | %------------------------------------------------------------------------ |
---|
30 | testangle=0; |
---|
31 | test_refraction=0; |
---|
32 | if exist('Zindex','var')&& isequal(Zindex,round(Zindex))&& Zindex>0 && isfield(Calib,'SliceCoord')&&length(Calib.SliceCoord)>=Zindex |
---|
33 | if isfield(Calib, 'SliceAngle') && ~isequal(Calib.SliceAngle,[0 0 0]) && ~isequal(Calib.SliceAngle(Zindex,:),[0 0 0]) |
---|
34 | testangle=1; |
---|
35 | om=norm(Calib.SliceAngle(Zindex,:));%norm of rotation angle in radians |
---|
36 | OmAxis=Calib.SliceAngle(Zindex,:)/om; %unit vector marking the rotation axis |
---|
37 | cos_om=cos(pi*om/180); |
---|
38 | sin_om=sin(pi*om/180); |
---|
39 | coeff=OmAxis(3)*(1-cos_om); |
---|
40 | norm_plane(1)=OmAxis(1)*coeff+OmAxis(2)*sin_om; |
---|
41 | norm_plane(2)=OmAxis(2)*coeff-OmAxis(1)*sin_om; |
---|
42 | norm_plane(3)=OmAxis(3)*coeff+cos_om; |
---|
43 | % Z0=norm_plane*Calib.SliceCoord(Zindex,:)'/norm_plane(3); |
---|
44 | end |
---|
45 | Z0=Calib.SliceCoord(Zindex,3);%horizontal plane z=cte |
---|
46 | % end |
---|
47 | Z0virt=Z0; |
---|
48 | if isfield(Calib,'InterfaceCoord') && isfield(Calib,'RefractionIndex') |
---|
49 | H=Calib.InterfaceCoord(3); |
---|
50 | if H>Z0 |
---|
51 | Z0virt=H-(H-Z0)/Calib.RefractionIndex; %corrected z (virtual object) |
---|
52 | test_refraction=1; |
---|
53 | end |
---|
54 | end |
---|
55 | else |
---|
56 | Z0=0; |
---|
57 | Z0virt=0; |
---|
58 | end |
---|
59 | if ~exist('X','var')||~exist('Y','var') |
---|
60 | Xphys=[]; |
---|
61 | Yphys=[];%default |
---|
62 | return |
---|
63 | end |
---|
64 | %coordinate transform |
---|
65 | if ~isfield(Calib,'fx_fy') |
---|
66 | Calib.fx_fy=[1 1]; |
---|
67 | end |
---|
68 | if ~isfield(Calib,'Tx_Ty_Tz') |
---|
69 | Calib.Tx_Ty_Tz=[0 0 1]; |
---|
70 | end |
---|
71 | if ~isfield(Calib,'Cx_Cy') |
---|
72 | Calib.Cx_Cy=[0 0]; |
---|
73 | end |
---|
74 | if ~isfield(Calib,'kc') |
---|
75 | Calib.kc=0; |
---|
76 | end |
---|
77 | if isfield(Calib,'R') |
---|
78 | R=(Calib.R)'; |
---|
79 | if testangle |
---|
80 | a=-norm_plane(1)/norm_plane(3); |
---|
81 | b=-norm_plane(2)/norm_plane(3); |
---|
82 | if test_refraction |
---|
83 | a=a/Calib.RefractionIndex; |
---|
84 | b=b/Calib.RefractionIndex; |
---|
85 | end |
---|
86 | R(1)=R(1)+a*R(3); |
---|
87 | R(2)=R(2)+b*R(3); |
---|
88 | R(4)=R(4)+a*R(6); |
---|
89 | R(5)=R(5)+b*R(6); |
---|
90 | R(7)=R(7)+a*R(9); |
---|
91 | R(8)=R(8)+b*R(9); |
---|
92 | end |
---|
93 | Tx=Calib.Tx_Ty_Tz(1); |
---|
94 | Ty=Calib.Tx_Ty_Tz(2); |
---|
95 | Tz=Calib.Tx_Ty_Tz(3); |
---|
96 | f=Calib.fx_fy(1);%dpy=1; sx=1 |
---|
97 | %dpx=Calib.fx_fy(2)/Calib.fx_fy(1); |
---|
98 | Dx=R(5)*R(7)-R(4)*R(8); |
---|
99 | Dy=R(1)*R(8)-R(2)*R(7); |
---|
100 | D0=(R(2)*R(4)-R(1)*R(5)); |
---|
101 | Z11=R(6)*R(8)-R(5)*R(9); |
---|
102 | Z12=R(2)*R(9)-R(3)*R(8); |
---|
103 | Z21=R(4)*R(9)-R(6)*R(7); |
---|
104 | Z22=R(3)*R(7)-R(1)*R(9); |
---|
105 | Zx0=R(3)*R(5)-R(2)*R(6); |
---|
106 | Zy0=R(1)*R(6)-R(3)*R(4); |
---|
107 | A11=R(8)*Ty-R(5)*Tz+Z11*Z0virt; |
---|
108 | A12=R(2)*Tz-R(8)*Tx+Z12*Z0virt; |
---|
109 | A21=-R(7)*Ty+R(4)*Tz+Z21*Z0virt; |
---|
110 | A22=-R(1)*Tz+R(7)*Tx+Z22*Z0virt; |
---|
111 | % X0=Calib.fx_fy(1)*(R(5)*Tx-R(2)*Ty+Zx0*Z0virt); |
---|
112 | % Y0=Calib.fx_fy(2)*(-R(4)*Tx+R(1)*Ty+Zy0*Z0virt); |
---|
113 | X0=(R(5)*Tx-R(2)*Ty+Zx0*Z0virt); |
---|
114 | Y0=(-R(4)*Tx+R(1)*Ty+Zy0*Z0virt); |
---|
115 | %px to camera: |
---|
116 | % Xd=dpx*(X-Calib.Cx_Cy(1)); % sensor coordinates |
---|
117 | % Yd=(Y-Calib.Cx_Cy(2)); |
---|
118 | Xd=(X-Calib.Cx_Cy(1))/Calib.fx_fy(1); % sensor coordinates |
---|
119 | Yd=(Y-Calib.Cx_Cy(2))/Calib.fx_fy(2); |
---|
120 | dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd);%/(f*f); %distortion factor |
---|
121 | Xu=Xd./dist_fact;%undistorted sensor coordinates |
---|
122 | Yu=Yd./dist_fact; |
---|
123 | denom=Dx*Xu+Dy*Yu+D0; |
---|
124 | Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates |
---|
125 | Yphys=(A21.*Xu+A22.*Yu+Y0)./denom; |
---|
126 | if testangle |
---|
127 | Zphys=Z0+a*Xphys+b*Yphys; |
---|
128 | else |
---|
129 | Zphys=Z0; |
---|
130 | end |
---|
131 | else |
---|
132 | Xphys=-Calib.Tx_Ty_Tz(1)+X/Calib.fx_fy(1); |
---|
133 | Yphys=-Calib.Tx_Ty_Tz(2)+Y/Calib.fx_fy(2); |
---|
134 | end |
---|
135 | |
---|
136 | %'px_XYZ': transform phys coordinates to image coordinates (px) |
---|
137 | % |
---|
138 | % OUTPUT: |
---|
139 | % X,Y: array of coordinates in the image cooresponding to the input physical positions |
---|
140 | % (origin at lower leftcorner, unit=pixel) |
---|
141 | |
---|
142 | % INPUT: |
---|
143 | % Calib: structure containing the calibration parameters (read from the ImaDoc .xml file) |
---|
144 | % Xphys, Yphys: array of x,y physical coordinates |
---|
145 | % [Z0]: corresponding array of z physical coordinates (0 by default) |
---|