1 | %transform image coordinates (px) to physical coordinates
|
---|
2 | % then transform to polar coordinates:
|
---|
3 | %[DataOut,DataOut_1]=phys_polar(varargin)
|
---|
4 | %
|
---|
5 | % OUTPUT:
|
---|
6 | % DataOut: structure of modified data field: .X=radius, .Y=azimuth angle, .U, .V are radial and azimuthal velocity components
|
---|
7 | % DataOut_1: second data field (if two fields are in input)
|
---|
8 | %
|
---|
9 | %INPUT:
|
---|
10 | % Data: structure of input data (like UvData)
|
---|
11 | % CalibData= structure containing the field .GeometryCalib with calibration parameters
|
---|
12 | % Data_1: second input field (not mandatory)
|
---|
13 | % CalibData_1= calibration parameters for the second field
|
---|
14 |
|
---|
15 | function [DataOut,DataOut_1]=phys_polar(varargin)
|
---|
16 | Calib{1}=[];
|
---|
17 | if nargin==2||nargin==4
|
---|
18 | Data=varargin{1};
|
---|
19 | DataOut=Data;%default
|
---|
20 | DataOut_1=[];%default
|
---|
21 | CalibData=varargin{2};
|
---|
22 | if isfield(CalibData,'GeometryCalib')
|
---|
23 | Calib{1}=CalibData.GeometryCalib;
|
---|
24 | end
|
---|
25 | Calib{2}=Calib{1};
|
---|
26 | else
|
---|
27 | DataOut.Txt='wrong input: need two or four structures';
|
---|
28 | end
|
---|
29 | test_1=0;
|
---|
30 | if nargin==4
|
---|
31 | test_1=1;
|
---|
32 | Data_1=varargin{3};
|
---|
33 | DataOut_1=Data_1;%default
|
---|
34 | CalibData_1=varargin{4};
|
---|
35 | if isfield(CalibData_1,'GeometryCalib')
|
---|
36 | Calib{2}=CalibData_1.GeometryCalib;
|
---|
37 | end
|
---|
38 | end
|
---|
39 |
|
---|
40 | %parameters for polar coordinates (taken from the calibration data of the first field)
|
---|
41 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
42 | origin_xy=[0 0];%center for the polar coordinates in the original x,y coordinates
|
---|
43 | if isfield(Calib{1},'PolarCentre') && isnumeric(Calib{1}.PolarCentre)
|
---|
44 | if isequal(length(Calib{1}.PolarCentre),2);
|
---|
45 | origin_xy= Calib{1}.PolarCentre;
|
---|
46 | end
|
---|
47 | end
|
---|
48 | radius_offset=0;%reference radius used to offset the radial coordinate r
|
---|
49 | angle_offset=0; %reference angle used as new origin of the polar angle (= axis Ox by default)
|
---|
50 | if isfield(Calib{1},'PolarReferenceRadius') && isnumeric(Calib{1}.PolarReferenceRadius)
|
---|
51 | radius_offset=Calib{1}.PolarReferenceRadius;
|
---|
52 | end
|
---|
53 | if radius_offset > 0
|
---|
54 | angle_scale=radius_offset; %the azimuth is rescale in terms of the length along the reference radius
|
---|
55 | else
|
---|
56 | angle_scale=180/pi; %polar angle in degrees
|
---|
57 | end
|
---|
58 | if isfield(Calib{1},'PolarReferenceAngle') && isnumeric(Calib{1}.PolarReferenceAngle)
|
---|
59 | angle_offset=Calib{1}.PolarReferenceAngle; %offset angle (in unit of the final angle, degrees or arc length along the reference radius))
|
---|
60 | end
|
---|
61 | % new x coordinate = radius-radius_offset;
|
---|
62 | % new y coordinate = theta*angle_scale-angle_offset
|
---|
63 |
|
---|
64 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
---|
65 |
|
---|
66 | iscalar=0;
|
---|
67 | if ~isempty(Calib{1})
|
---|
68 | DataOut=phys_1(Data,Calib{1},origin_xy,radius_offset,angle_offset,angle_scale);
|
---|
69 | %case of images or scalar
|
---|
70 | if isfield(Data,'A')&isfield(Data,'AX')&~isempty(Data.AX) & isfield(Data,'AY')&...
|
---|
71 | ~isempty(Data.AY)&length(Data.A)>1
|
---|
72 | iscalar=1;
|
---|
73 | A{1}=Data.A;
|
---|
74 | end
|
---|
75 | %transform of X,Y coordinates for vector fields
|
---|
76 | if isfield(Data,'ZIndex')&~isempty(Data.ZIndex)
|
---|
77 | ZIndex=Data.ZIndex;
|
---|
78 | else
|
---|
79 | ZIndex=0;
|
---|
80 | end
|
---|
81 | end
|
---|
82 |
|
---|
83 | if test_1
|
---|
84 | DataOut_1=phys_1(Data_1,Calib{2},origin_xy,radius_offset,angle_offset,angle_scale);
|
---|
85 | if isfield(Data_1,'A')&isfield(Data_1,'AX')&~isempty(Data_1.AX) & isfield(Data_1,'AY')&...
|
---|
86 | ~isempty(Data_1.AY)&length(Data_1.A)>1
|
---|
87 | iscalar=iscalar+1;
|
---|
88 | Calib{iscalar}=Calib{2};
|
---|
89 | A{iscalar}=Data_1.A;
|
---|
90 | if isfield(Data_1,'ZIndex')&~isequal(Data_1.ZIndex,ZIndex)
|
---|
91 | DataOut.Txt='inconsistent plane indexes in the two input fields';
|
---|
92 | end
|
---|
93 | if iscalar==1% case for which only the second field is a scalar
|
---|
94 | [A,AX,AY]=phys_Ima(A,Calib,ZIndex,origin_xy,radius_offset,angle_offset,angle_scale);
|
---|
95 | DataOut_1.A=A{1};
|
---|
96 | DataOut_1.AX=AX;
|
---|
97 | DataOut_1.AY=AY;
|
---|
98 | return
|
---|
99 | end
|
---|
100 | end
|
---|
101 | end
|
---|
102 | if iscalar~=0
|
---|
103 | [A,AX,AY]=phys_Ima(A,Calib,ZIndex,origin_xy,radius_offset,angle_offset,angle_scale);%
|
---|
104 | DataOut.A=A{1};
|
---|
105 | DataOut.AX=AX;
|
---|
106 | DataOut.AY=AY;
|
---|
107 | if iscalar==2
|
---|
108 | DataOut_1.A=A{2};
|
---|
109 | DataOut_1.AX=AX;
|
---|
110 | DataOut_1.AY=AY;
|
---|
111 | end
|
---|
112 | end
|
---|
113 |
|
---|
114 | %------------------------------------------------
|
---|
115 | function DataOut=phys_1(Data,Calib,origin_xy,radius_offset,angle_offset,angle_scale)
|
---|
116 |
|
---|
117 | DataOut=Data;
|
---|
118 | DataOut.CoordType='phys'; %put flag for physical coordinates
|
---|
119 | if isfield(Calib,'CoordUnit')
|
---|
120 | DataOut.CoordUnit=Calib.CoordUnit;
|
---|
121 | else
|
---|
122 | DataOut.CoordUnit='cm'; %default
|
---|
123 | end
|
---|
124 | DataOut.TimeUnit='s';
|
---|
125 | %perform a geometry transform if Calib contains a field .GeometryCalib
|
---|
126 | if isfield(Data,'CoordType') && isequal(Data.CoordType,'px') && ~isempty(Calib)
|
---|
127 | if isfield(Data,'CoordUnit')
|
---|
128 | DataOut=rmfield(DataOut,'CoordUnit');
|
---|
129 | end
|
---|
130 | %transform of X,Y coordinates for vector fields
|
---|
131 | if isfield(Data,'ZIndex')&~isempty(Data.ZIndex)
|
---|
132 | Z=Data.ZIndex;
|
---|
133 | else
|
---|
134 | Z=0;
|
---|
135 | end
|
---|
136 | if isfield(Data,'X') &isfield(Data,'Y')&~isempty(Data.X) & ~isempty(Data.Y)
|
---|
137 | [DataOut.X,DataOut.Y,DataOut.Z]=phys_XYZ(Calib,Data.X,Data.Y,Z); %transform from pixels to physical
|
---|
138 | DataOut.X=DataOut.X-origin_xy(1);%origin of coordinates at the tank center
|
---|
139 | DataOut.Y=DataOut.Y-origin_xy(2);%origin of coordinates at the tank center
|
---|
140 | [theta,DataOut.X] = cart2pol(DataOut.X,DataOut.Y);%theta and X are the polar coordinates angle and radius
|
---|
141 | %shift and renormalize the polar coordinates
|
---|
142 | DataOut.X=DataOut.X-radius_offset;%
|
---|
143 | DataOut.Y=theta*angle_scale-angle_offset;% normalized angle: distance along reference radius
|
---|
144 | %transform velocity field if exists
|
---|
145 | if isfield(Data,'U')&isfield(Data,'V')&~isempty(Data.U) & ~isempty(Data.V)& isfield(Data,'dt')
|
---|
146 | if ~isempty(Data.dt)
|
---|
147 | [XOut_1,YOut_1]=phys_XYZ(Calib,Data.X-Data.U/2,Data.Y-Data.V/2,Z);
|
---|
148 | [XOut_2,YOut_2]=phys_XYZ(Calib,Data.X+Data.U/2,Data.Y+Data.V/2,Z);
|
---|
149 | UX=(XOut_2-XOut_1)/Data.dt;
|
---|
150 | VY=(YOut_2-YOut_1)/Data.dt;
|
---|
151 | %transform u,v into polar coordiantes
|
---|
152 | DataOut.U=UX.*cos(theta)+VY.*sin(theta);%radial velocity
|
---|
153 | DataOut.V=(-UX.*sin(theta)+VY.*cos(theta));%./(DataOut.X)%+radius_ref);%angular velocity calculated
|
---|
154 | %shift and renormalize the angular velocity
|
---|
155 | end
|
---|
156 | end
|
---|
157 | end
|
---|
158 | end
|
---|
159 |
|
---|
160 |
|
---|
161 | %%%%%%%%%%%%%%%%%%%%
|
---|
162 | function [A_out,Rangx,Rangy]=phys_Ima(A,CalibIn,ZIndex,origin_xy,radius_offset,angle_offset,angle_scale)
|
---|
163 | xcorner=[];
|
---|
164 | ycorner=[];
|
---|
165 | npx=[];
|
---|
166 | npy=[];
|
---|
167 |
|
---|
168 | for icell=1:length(A)
|
---|
169 | siz=size(A{icell});
|
---|
170 | npx=[npx siz(2)];
|
---|
171 | npy=[npy siz(1)];
|
---|
172 | zphys=0; %default
|
---|
173 | if isfield(CalibIn{icell},'SliceCoord') %.Z= index of plane
|
---|
174 | SliceCoord=CalibIn{icell}.SliceCoord(ZIndex,:);
|
---|
175 | zphys=SliceCoord(3); %to generalize for non-parallel planes
|
---|
176 | end
|
---|
177 | xima=[0.5 siz(2)-0.5 0.5 siz(2)-0.5];%image coordiantes of corners
|
---|
178 | yima=[0.5 0.5 siz(1)-0.5 siz(1)-0.5];
|
---|
179 | [xcorner_new,ycorner_new]=phys_XYZ(CalibIn{icell},xima,yima,ZIndex);%corresponding physical coordinates
|
---|
180 | %transform the corner coordinates into polar ones
|
---|
181 | xcorner_new=xcorner_new-origin_xy(1);%shift to the origin of the polar coordinates
|
---|
182 | ycorner_new=ycorner_new-origin_xy(2);%shift to the origin of the polar coordinates
|
---|
183 | [theta,xcorner_new] = cart2pol(xcorner_new,ycorner_new);%theta and X are the polar coordinates angle and radius
|
---|
184 | if (max(theta)-min(theta))>pi %if the polar origin is inside the image
|
---|
185 | xcorner_new=[0 max(xcorner_new)];
|
---|
186 | theta=[-pi pi];
|
---|
187 | end
|
---|
188 | %shift and renormalize the polar coordinates
|
---|
189 | xcorner_new=xcorner_new-radius_offset;%
|
---|
190 | ycorner_new=theta*angle_scale-angle_offset;% normalized angle: distance along reference radius
|
---|
191 | xcorner=[xcorner xcorner_new];
|
---|
192 | ycorner=[ycorner ycorner_new];
|
---|
193 | end
|
---|
194 | Rangx(1)=min(xcorner);
|
---|
195 | Rangx(2)=max(xcorner);
|
---|
196 | Rangy(2)=min(ycorner);
|
---|
197 | Rangy(1)=max(ycorner);
|
---|
198 | % test_multi=(max(npx)~=min(npx)) | (max(npy)~=min(npy));
|
---|
199 | npx=max(npx);
|
---|
200 | npy=max(npy);
|
---|
201 | x=linspace(Rangx(1),Rangx(2),npx);
|
---|
202 | y=linspace(Rangy(1),Rangy(2),npy);
|
---|
203 | [X,Y]=meshgrid(x,y);%grid in physical coordinates
|
---|
204 | %transform X, Y in cartesian
|
---|
205 | X=X+radius_offset;%
|
---|
206 | Y=(Y+angle_offset)/angle_scale;% normalized angle: distance along reference radius
|
---|
207 | [X,Y] = pol2cart(Y,X);
|
---|
208 | X=X+origin_xy(1);%shift to the origin of the polar coordinates
|
---|
209 | Y=Y+origin_xy(2);%shift to the origin of the polar coordinates
|
---|
210 | for icell=1:length(A)
|
---|
211 | [XIMA,YIMA]=px_XYZ(CalibIn{icell},X,Y,zphys);%corresponding image indices for each point in the real space grid
|
---|
212 | XIMA=reshape(round(XIMA),1,npx*npy);%indices reorganized in 'line'
|
---|
213 | YIMA=reshape(round(YIMA),1,npx*npy);
|
---|
214 | flagin=XIMA>=1 & XIMA<=npx & YIMA >=1 & YIMA<=npy;%flagin=1 inside the original image
|
---|
215 | vec_A=reshape(A{icell}(:,:,1),1,npx*npy);%put the original image in line
|
---|
216 | ind_in=find(flagin);
|
---|
217 | ind_out=find(~flagin);
|
---|
218 | ICOMB=((XIMA-1)*npy+(npy+1-YIMA));
|
---|
219 | ICOMB=ICOMB(flagin);%index corresponding to XIMA and YIMA in the aligned original image vec_A
|
---|
220 | vec_B(ind_in)=vec_A(ICOMB);
|
---|
221 | vec_B(ind_out)=zeros(size(ind_out));
|
---|
222 | A_out{icell}=reshape(vec_B,npy,npx);%new image in real coordinates
|
---|
223 | end
|
---|
224 | %Rangx=Rangx-radius_offset;
|
---|
225 |
|
---|
226 | %'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters
|
---|
227 | % function [Xphys,Yphys]=phys_XYZ(Calib,X,Y,Z)
|
---|
228 | %
|
---|
229 | %OUTPUT:
|
---|
230 | %
|
---|
231 | %INPUT:
|
---|
232 | %Z: index of plane
|
---|
233 | function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Z)
|
---|
234 | if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'SliceCoord')&length(Calib.SliceCoord)>=Z
|
---|
235 | Zindex=Z;
|
---|
236 | Zphys=Calib.SliceCoord(Zindex,3);%GENERALISER AUX CAS AVEC ANGLE
|
---|
237 | else
|
---|
238 | % if exist('Z','var')
|
---|
239 | % Zphys=Z;
|
---|
240 | % else
|
---|
241 | Zphys=0;
|
---|
242 | % end
|
---|
243 | end
|
---|
244 | if ~exist('X','var')||~exist('Y','var')
|
---|
245 | Xphys=[];
|
---|
246 | Yphys=[];%default
|
---|
247 | return
|
---|
248 | end
|
---|
249 | Xphys=X;%default
|
---|
250 | Yphys=Y;
|
---|
251 | %image transform
|
---|
252 | if isfield(Calib,'R')
|
---|
253 | R=(Calib.R)';
|
---|
254 | Dx=R(5)*R(7)-R(4)*R(8);
|
---|
255 | Dy=R(1)*R(8)-R(2)*R(7);
|
---|
256 | D0=Calib.f*(R(2)*R(4)-R(1)*R(5));
|
---|
257 | Z11=R(6)*R(8)-R(5)*R(9);
|
---|
258 | Z12=R(2)*R(9)-R(3)*R(8);
|
---|
259 | Z21=R(4)*R(9)-R(6)*R(7);
|
---|
260 | Z22=R(3)*R(7)-R(1)*R(9);
|
---|
261 | Zx0=R(3)*R(5)-R(2)*R(6);
|
---|
262 | Zy0=R(1)*R(6)-R(3)*R(4);
|
---|
263 | A11=R(8)*Calib.Ty-R(5)*Calib.Tz+Z11*Zphys;
|
---|
264 | A12=R(2)*Calib.Tz-R(8)*Calib.Tx+Z12*Zphys;
|
---|
265 | A21=-R(7)*Calib.Ty+R(4)*Calib.Tz+Z21*Zphys;
|
---|
266 | A22=-R(1)*Calib.Tz+R(7)*Calib.Tx+Z11*Zphys;
|
---|
267 | X0=Calib.f*(R(5)*Calib.Tx-R(2)*Calib.Ty+Zx0*Zphys);
|
---|
268 | Y0=Calib.f*(-R(4)*Calib.Tx+R(1)*Calib.Ty+Zy0*Zphys);
|
---|
269 | %px to camera:
|
---|
270 | Xd=(Calib.dpx/Calib.sx)*(X-Calib.Cx); % sensor coordinates
|
---|
271 | Yd=Calib.dpy*(Y-Calib.Cy);
|
---|
272 | dist_fact=1+Calib.kappa1*(Xd.*Xd+Yd.*Yd); %distortion factor
|
---|
273 | Xu=dist_fact.*Xd;%undistorted sensor coordinates
|
---|
274 | Yu=dist_fact.*Yd;
|
---|
275 | denom=Dx*Xu+Dy*Yu+D0;
|
---|
276 | % denom2=denom.*denom;
|
---|
277 | Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates
|
---|
278 | Yphys=(A21.*Xu+A22.*Yu+Y0)./denom;
|
---|
279 | end
|
---|
280 |
|
---|
281 | %'px_XYZ': transform phys coordinates to image coordinates (px)
|
---|
282 | %
|
---|
283 | % OUPUT:
|
---|
284 | % X,Y: array of coordinates in the image cooresponding to the input physical positions
|
---|
285 | % (origin at lower leftcorner, unit=pixel)
|
---|
286 |
|
---|
287 | % INPUT:
|
---|
288 | % Calib: structure containing the calibration parameters (read from the ImaDoc .xml file)
|
---|
289 | % Xphys, Yphys: array of x,y physical coordinates
|
---|
290 | % [Zphys]: corresponding array of z physical coordinates (0 by default)
|
---|
291 |
|
---|
292 |
|
---|
293 | function [X,Y]=px_XYZ(Calib,Xphys,Yphys,Zphys)
|
---|
294 | X=[];%default
|
---|
295 | Y=[];
|
---|
296 | % if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'PlanePos')&length(Calib.PlanePos)>=Z
|
---|
297 | % Zindex=Z;
|
---|
298 | % planepos=Calib.PlanePos{Zindex};
|
---|
299 | % zphys=planepos(3);%A GENERALISER CAS AVEC ANGLE
|
---|
300 | % else
|
---|
301 | % zphys=0;
|
---|
302 | % end
|
---|
303 | if ~exist('Zphys','var')
|
---|
304 | Zphys=0;
|
---|
305 | end
|
---|
306 |
|
---|
307 | %%%%%%%%%%%%%
|
---|
308 | if isfield(Calib,'R')
|
---|
309 | R=(Calib.R)';
|
---|
310 | xc=R(1)*Xphys+R(2)*Yphys+R(3)*Zphys+Calib.Tx;
|
---|
311 | yc=R(4)*Xphys+R(5)*Yphys+R(6)*Zphys+Calib.Ty;
|
---|
312 | zc=R(7)*Xphys+R(8)*Yphys+R(9)*Zphys+Calib.Tz;
|
---|
313 | %undistorted image coordinates
|
---|
314 | Xu=Calib.f*xc./zc;
|
---|
315 | Yu=Calib.f*yc./zc;
|
---|
316 | %distorted image coordinates
|
---|
317 | distortion=(Calib.kappa1)*(Xu.*Xu+Yu.*Yu)+1; %A REVOIR
|
---|
318 | % distortion=1;
|
---|
319 | Xd=Xu./distortion;
|
---|
320 | Yd=Yu./distortion;
|
---|
321 | %pixel coordinates
|
---|
322 | X=Xd*Calib.sx/Calib.dpx+Calib.Cx;
|
---|
323 | Y=Yd/Calib.dpy+Calib.Cy;
|
---|
324 |
|
---|
325 | elseif isfield(Calib,'Pxcmx')&isfield(Calib,'Pxcmy')%old calib
|
---|
326 | X=Xphys*Calib.Pxcmx;
|
---|
327 | Y=Yphys*Calib.Pxcmy;
|
---|
328 | end
|
---|
329 |
|
---|
330 |
|
---|