source: trunk/src/transform_field/phys.m @ 211

Last change on this file since 211 was 211, checked in by sommeria, 14 years ago

bug repair for phys

File size: 15.1 KB
RevLine 
[40]1%'phys': transforms image (px) to real world (phys) coordinates using geometric calibration parameters
2% DataOut=phys(Data,CalibData) , transform one input field
3% [DataOut,DataOut_1]=phys(Data,CalibData,Data_1,CalibData_1), transform two input fields
4
5% OUTPUT:
6% DataOut:   structure representing the modified field
7% DataOut_1: structure representing the second modified field
8
9%INPUT:
10% Data:  structure of input data
11%       with fields .A (image or scalar matrix), AX, AY
12%       .X,.Y,.U,.V, .DjUi
13%       .ZIndex: index of plane in multilevel case
14%       .CoordType='phys' or 'px', The function ACTS ONLY IF .CoordType='px'
15% CalibData: structure containing calibration parameters or a subtree Calib.GeometryCalib =calibration data (tsai parameters)
16
17function [DataOut,DataOut_1]=phys(varargin)
18% A FAIRE: 1- verifier si DataIn est une 'field structure'(.ListVarName'):
19% chercher ListVarAttribute, for each field (cell of variables):
20%   .CoordType: 'phys' or 'px'   (default==phys, no transform)
21%   .scale_factor: =dt (to transform displacement into velocity) default=1
22%   .covariance: 'scalar', 'coord', 'D_i': covariant (like velocity), 'D^i': contravariant (like gradient), 'D^jD_i' (like strain tensor)
23%   (default='coord' if .Role='coord_x,_y...,
24%            'D_i' if '.Role='vector_x,...',
25%              'scalar', else (thenno change except scale factor)
26Calib{1}=[];
27if nargin==2||nargin==4 % nargin =nbre of input variables
28    Data=varargin{1};
29    DataOut=Data;%default
30    DataOut_1=[];%default
31    CalibData=varargin{2};
32    if isfield(CalibData,'GeometryCalib')
33        Calib{1}=CalibData.GeometryCalib;
34    end
35    Calib{2}=Calib{1};
36else
37    DataOut.Txt='wrong input: need two or four structures';
38end
39test_1=0;
40if nargin==4
41    test_1=1;
42    Data_1=varargin{3};
43    DataOut_1=Data_1;%default
44    CalibData_1=varargin{4};
45    if isfield(CalibData_1,'GeometryCalib')
46        Calib{2}=CalibData_1.GeometryCalib;
47    end
48end
49iscalar=0;
50if  ~isempty(Calib{1})
51    DataOut=phys_1(Data,Calib{1});
52    %case of images or scalar: in case of two input fields, we need to project the transform of on the same regular grid
53    if isfield(Data,'A') && isfield(Data,'AX') && ~isempty(Data.AX) && isfield(Data,'AY')&&...
54                                           ~isempty(Data.AY) && length(Data.A)>1
55        iscalar=1;
56        A{1}=Data.A;
57    end
58end
59%transform of X,Y coordinates for vector fields
[116]60if isfield(Data,'ZIndex')&&~isempty(Data.ZIndex)&&~isnan(Data.ZIndex)
[40]61    ZIndex=Data.ZIndex;
62else
[116]63    ZIndex=1;
[40]64end
65if test_1
66    DataOut_1=phys_1(Data_1,Calib{2});
67    if isfield(Data_1,'A')&&isfield(Data_1,'AX')&&~isempty(Data_1.AX) && isfield(Data_1,'AY')&&...
68                                       ~isempty(Data_1.AY)&&length(Data_1.A)>1
69          iscalar=iscalar+1;
70          Calib{iscalar}=Calib{2};
71          A{iscalar}=Data_1.A;
72          if isfield(Data_1,'ZIndex') && ~isequal(Data_1.ZIndex,ZIndex)
73              DataOut.Txt='inconsistent plane indexes in the two input fields';
74          end
75          if iscalar==1% case for which only the second field is a scalar
76               [A,AX,AY]=phys_Ima(A,Calib,ZIndex);
77               DataOut_1.A=A{1};
78               DataOut_1.AX=AX;
79               DataOut_1.AY=AY;
80               return
81          end
82    end
83end
84if iscalar~=0
85    [A,AX,AY]=phys_Ima(A,Calib,ZIndex);%TODO : introduire interp2_uvmat ds phys_ima
86    DataOut.A=A{1};
87    DataOut.AX=AX;
88    DataOut.AY=AY;
89    if iscalar==2
90        DataOut_1.A=A{2};
91        DataOut_1.AX=AX;
92        DataOut_1.AY=AY;
93    end
94end
95
[209]96% DataOut.VarDimName{2}
97% DataOut.VarDimName{3}
98% DataOut.VarDimName{4}
99% DataOut.VarDimName{5}
100% DataOut.VarDimName{6}
101% DataOut.VarDimName{7}
102% DataOut.VarAttribute{1}
103% DataOut.VarAttribute{2}
104% DataOut.VarAttribute{3}
105% DataOut.VarAttribute{4}
106% DataOut.VarAttribute{5}
107% DataOut.VarAttribute{6}
108% DataOut.VarAttribute{7}
[40]109%------------------------------------------------
110function DataOut=phys_1(Data,Calib)
111% for icell=1:length(Data)
112
113DataOut=Data;%default
[167]114% DataOut.CoordUnit=Calib.CoordUnit; %put flag for physical coordinates
[209]115if isfield(Calib,'SliceCoord') && isfield(Data,'ZIndex')&&~isempty(Data.ZIndex)&&~isnan(Data.ZIndex)
116    DataOut.PlaneCoord=Calib.SliceCoord(Data.ZIndex,:);% transfer the slice position
117    if isfield(Calib,'SliceAngle') % transfer the slice rotation angles
118        DataOut.PlaneAngle=Calib.SliceAngle(Data.ZIndex,:);
119    end
[151]120end
[40]121% The transform ACTS ONLY IF .CoordType='px'and Calib defined
[158]122if isfield(Data,'CoordUnit')%&& isequal(Data.CoordType,'px')&& ~isempty(Calib)
[40]123    if isfield(Calib,'CoordUnit')
124        DataOut.CoordUnit=Calib.CoordUnit;
125    else
126        DataOut.CoordUnit='cm'; %default
127    end
128    DataOut.TimeUnit='s';
129    %transform of X,Y coordinates for vector fields
[209]130    test_z=0;
[116]131    if isfield(Data,'ZIndex') && ~isempty(Data.ZIndex)&&~isnan(Data.ZIndex)
[157]132        Z=Data.ZIndex;
[209]133        test_z=1;
[40]134    else
135        Z=0;
136    end
137    if isfield(Data,'X') &&isfield(Data,'Y')&&~isempty(Data.X) && ~isempty(Data.Y)
138        [DataOut.X,DataOut.Y,DataOut.Z]=phys_XYZ(Calib,Data.X,Data.Y,Z);
[209]139        if test_z
140             DataOut.ListVarName=[DataOut.ListVarName(1:2) {'Z'} DataOut.ListVarName(3:end)];
141             DataOut.VarDimName=[DataOut.VarDimName(1:2) DataOut.VarDimName(1) DataOut.VarDimName(3:end)];
142             ZAttribute{1}.Role='coord_z';
143             DataOut.VarAttribute=[DataOut.VarAttribute(1:2) ZAttribute DataOut.VarAttribute(3:end)];
144        end
[40]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            DataOut.U=(XOut_2-XOut_1)/Data.dt;
150            DataOut.V=(YOut_2-YOut_1)/Data.dt;
151            end
152        end
153    end
154    %transform of an image or scalar: done in phys_ima
155     
156    %transform of spatial derivatives
157    if isfield(Data,'X') && ~isempty(Data.X) && isfield(Data,'DjUi') && ~isempty(Data.DjUi)...
158          && isfield(Data,'dt')   
159        if ~isempty(Data.dt)
160            % estimate the Jacobian matrix DXpx/DXphys
161            for ip=1:length(Data.X)
162                [Xp1,Yp1]=phys_XYZ(Calib,Data.X(ip)+0.5,Data.Y(ip),Z);
163                [Xm1,Ym1]=phys_XYZ(Calib,Data.X(ip)-0.5,Data.Y(ip),Z);
164                [Xp2,Yp2]=phys_XYZ(Calib,Data.X(ip),Data.Y(ip)+0.5,Z);
165                [Xm2,Ym2]=phys_XYZ(Calib,Data.X(ip),Data.Y(ip)-0.5,Z);
166            %Jacobian matrix DXpphys/DXpx
167               DjXi(1,1)=(Xp1-Xm1);
168               DjXi(2,1)=(Yp1-Ym1);
169               DjXi(1,2)=(Xp2-Xm2);
170               DjXi(2,2)=(Yp2-Ym2);
171               DjUi(:,:)=Data.DjUi(ip,:,:);
172               DjUi=(DjXi*DjUi')/DjXi;% =J-1*M*J , curvature effects (derivatives of J) neglected
173               DataOut.DjUi(ip,:,:)=DjUi';
174            end
175            DataOut.DjUi =  DataOut.DjUi/Data.dt;   %     min(Data.DjUi(:,1,1))=DUDX                         
176        end
177    end
178end
179
180%%%%%%%%%%%%%%%%%%%%
181function [A_out,Rangx,Rangy]=phys_Ima(A,CalibIn,ZIndex)
182xcorner=[];
183ycorner=[];
184npx=[];
185npy=[];
[116]186dx=ones(1,length(A));
187dy=ones(1,length(A));
[40]188for icell=1:length(A)
189    siz=size(A{icell});
190    npx=[npx siz(2)];
191    npy=[npy siz(1)];
192    Calib=CalibIn{icell};
[116]193    xima=[0.5 siz(2)-0.5 0.5 siz(2)-0.5];%image coordinates of corners
[40]194    yima=[0.5 0.5 siz(1)-0.5 siz(1)-0.5];
195    [xcorner_new,ycorner_new]=phys_XYZ(Calib,xima,yima,ZIndex);%corresponding physical coordinates
[79]196    dx(icell)=(max(xcorner_new)-min(xcorner_new))/(siz(2)-1);
197    dy(icell)=(max(ycorner_new)-min(ycorner_new))/(siz(1)-1);
[40]198    xcorner=[xcorner xcorner_new];
199    ycorner=[ycorner ycorner_new];
200end
201Rangx(1)=min(xcorner);
202Rangx(2)=max(xcorner);
203Rangy(2)=min(ycorner);
204Rangy(1)=max(ycorner);
[116]205test_multi=(max(npx)~=min(npx)) || (max(npy)~=min(npy)); %different image lengths
[79]206npX=1+round((Rangx(2)-Rangx(1))/min(dx));% nbre of pixels in the new image (use the finest resolution min(dx) in the set of images)
207npY=1+round((Rangy(1)-Rangy(2))/min(dy));
208x=linspace(Rangx(1),Rangx(2),npX);
209y=linspace(Rangy(1),Rangy(2),npY);
[40]210[X,Y]=meshgrid(x,y);%grid in physical coordiantes
211vec_B=[];
212A_out={};
213for icell=1:length(A)
214    Calib=CalibIn{icell};
[116]215    if isfield(Calib,'R') || isfield(Calib,'kc')|| test_multi ||~isequal(Calib,CalibIn{1})% the image needs to be interpolated to the new coordinates
[40]216        zphys=0; %default
217        if isfield(Calib,'SliceCoord') %.Z= index of plane
218           SliceCoord=Calib.SliceCoord(ZIndex,:);
219           zphys=SliceCoord(3); %to generalize for non-parallel planes
[202]220           if isfield(Calib,'InterfaceCoord') && isfield(Calib,'RefractionIndex')
221                H=Calib.InterfaceCoord(3);
222                if H>zphys
223                    zphys=H-(H-zphys)/Calib.RefractionIndex; %corrected z (virtual object)
224                end
225           end
[40]226        end
[79]227        [XIMA,YIMA]=px_XYZ(CalibIn{icell},X,Y,zphys);% image coordinates for each point in the real space grid
228        XIMA=reshape(round(XIMA),1,npX*npY);%indices reorganized in 'line'
229        YIMA=reshape(round(YIMA),1,npX*npY);
230        flagin=XIMA>=1 & XIMA<=npx(icell) & YIMA >=1 & YIMA<=npy(icell);%flagin=1 inside the original image
[40]231        testuint8=isa(A{icell},'uint8');
232        testuint16=isa(A{icell},'uint16');
233        if numel(siz)==2 %(B/W images)
[79]234            vec_A=reshape(A{icell},1,npx(icell)*npy(icell));%put the original image in line
[209]235            %ind_in=find(flagin);
[40]236            ind_out=find(~flagin);
[79]237            ICOMB=((XIMA-1)*npy(icell)+(npy(icell)+1-YIMA));
[40]238            ICOMB=ICOMB(flagin);%index corresponding to XIMA and YIMA in the aligned original image vec_A
[209]239            %vec_B(ind_in)=vec_A(ICOMB);
240            vec_B(flagin)=vec_A(ICOMB);
241            vec_B(~flagin)=zeros(size(ind_out));
242%             vec_B(ind_out)=zeros(size(ind_out));
[79]243            A_out{icell}=reshape(vec_B,npY,npX);%new image in real coordinates
[40]244        elseif numel(siz)==3     
245            for icolor=1:siz(3)
246                vec_A=reshape(A{icell}(:,:,icolor),1,npx*npy);%put the original image in line
[209]247               % ind_in=find(flagin);
[40]248                ind_out=find(~flagin);
249                ICOMB=((XIMA-1)*npy+(npy+1-YIMA));
250                ICOMB=ICOMB(flagin);%index corresponding to XIMA and YIMA in the aligned original image vec_A
[209]251                vec_B(flagin)=vec_A(ICOMB);
252                vec_B(~flagin)=zeros(size(ind_out));
[40]253                A_out{icell}(:,:,icolor)=reshape(vec_B,npy,npx);%new image in real coordinates
254            end
255        end
256        if testuint8
257            A_out{icell}=uint8(A_out{icell});
258        end
259        if testuint16
260            A_out{icell}=uint16(A_out{icell});
261        end
[116]262    else%     
[40]263        A_out{icell}=A{icell};%no transform
264        Rangx=[0.5 npx-0.5];%image coordiantes of corners
265        Rangy=[npy-0.5 0.5];
[116]266        [Rangx]=phys_XYZ(Calib,Rangx,[0.5 0.5],ZIndex);%case of translations without rotation and quadratic deformation
267        [xx,Rangy]=phys_XYZ(Calib,[0.5 0.5],Rangy,ZIndex);
[40]268    end
269end
270
[116]271%------------------------------------------------------------------------
[40]272%'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters
273% function [Xphys,Yphys]=phys_XYZ(Calib,X,Y,Z)
274%
275%OUTPUT:
276%
277%INPUT:
278%Z: index of plane
[209]279function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Zindex)
[116]280%------------------------------------------------------------------------
[209]281testangle=0;
282test_refraction=0;
283if exist('Zindex','var')&& isequal(Zindex,round(Zindex))&& Zindex>0 && isfield(Calib,'SliceCoord')&&length(Calib.SliceCoord)>=Zindex
[211]284    if isfield(Calib, 'SliceAngle') && ~isequal(Calib.SliceAngle,[0 0 0])
[209]285        testangle=1;
286        om=norm(Calib.SliceAngle(Zindex,:));%norm of rotation angle in radians
287        OmAxis=Calib.SliceAngle(Zindex,:)/om; %unit vector marking the rotation axis
288        cos_om=cos(pi*om/180);
289        sin_om=sin(pi*om/180);
290        coeff=OmAxis(3)*(1-cos_om);
291        norm_plane(1)=OmAxis(1)*coeff+OmAxis(2)*sin_om;
292        norm_plane(2)=OmAxis(2)*coeff-OmAxis(1)*sin_om;
293        norm_plane(3)=OmAxis(3)*coeff+cos_om;
294        Z0=norm_plane*Calib.SliceCoord(Zindex,:)'/norm_plane(3);
295    else
296        Z0=Calib.SliceCoord(Zindex,3);%horizontal plane z=cte
297    end
298    Z0virt=Z0;
[202]299    if isfield(Calib,'InterfaceCoord') && isfield(Calib,'RefractionIndex')
300        H=Calib.InterfaceCoord(3);
[209]301        if H>Z0
302            Z0virt=H-(H-Z0)/Calib.RefractionIndex; %corrected z (virtual object)
303            test_refraction=1;
[202]304        end
305    end   
[40]306else
[209]307    Z0=0;
308    Z0virt=0;
[40]309end
310if ~exist('X','var')||~exist('Y','var')
311    Xphys=[];
312    Yphys=[];%default
313    return
314end
[116]315%coordinate transform
316if ~isfield(Calib,'fx_fy')
317     Calib.fx_fy=[1 1];
318end
319if ~isfield(Calib,'Tx_Ty_Tz')
320     Calib.Tx_Ty_Tz=[0 0 1];
321end
322if ~isfield(Calib,'Cx_Cy')
323     Calib.Cx_Cy=[0 0];
324end
325if ~isfield(Calib,'kc')
326     Calib.kc=0;
327end
[40]328if isfield(Calib,'R')
329    R=(Calib.R)';
[209]330    if testangle
331        a=-norm_plane(1)/norm_plane(3);
332        b=-norm_plane(2)/norm_plane(3);
333        if test_refraction
334            a=a/Calib.RefractionIndex;
335            b=b/Calib.RefractionIndex;
336        end
337        R(1)=R(1)+a*R(3);
338        R(2)=R(2)+b*R(3);
339        R(4)=R(4)+a*R(6);
340        R(5)=R(5)+b*R(6);
341        R(7)=R(7)+a*R(9);
342        R(8)=R(8)+b*R(9);
343    end
[116]344    Tx=Calib.Tx_Ty_Tz(1);
345    Ty=Calib.Tx_Ty_Tz(2);
346    Tz=Calib.Tx_Ty_Tz(3);
347    f=Calib.fx_fy(1);%dpy=1; sx=1
348    dpx=Calib.fx_fy(2)/Calib.fx_fy(1);
[40]349    Dx=R(5)*R(7)-R(4)*R(8);
350    Dy=R(1)*R(8)-R(2)*R(7);
[116]351    D0=f*(R(2)*R(4)-R(1)*R(5));
[40]352    Z11=R(6)*R(8)-R(5)*R(9);
353    Z12=R(2)*R(9)-R(3)*R(8); 
354    Z21=R(4)*R(9)-R(6)*R(7);
355    Z22=R(3)*R(7)-R(1)*R(9);
356    Zx0=R(3)*R(5)-R(2)*R(6);
357    Zy0=R(1)*R(6)-R(3)*R(4);
[209]358    A11=R(8)*Ty-R(5)*Tz+Z11*Z0virt;
359    A12=R(2)*Tz-R(8)*Tx+Z12*Z0virt;
360    A21=-R(7)*Ty+R(4)*Tz+Z21*Z0virt;
361    A22=-R(1)*Tz+R(7)*Tx+Z22*Z0virt;
362    X0=f*(R(5)*Tx-R(2)*Ty+Zx0*Z0virt);
363    Y0=f*(-R(4)*Tx+R(1)*Ty+Zy0*Z0virt);
[40]364        %px to camera:
[116]365    Xd=dpx*(X-Calib.Cx_Cy(1)); % sensor coordinates
366    Yd=(Y-Calib.Cx_Cy(2));
367    dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd)/(f*f); %distortion factor
368    Xu=Xd./dist_fact;%undistorted sensor coordinates
369    Yu=Yd./dist_fact;
[40]370    denom=Dx*Xu+Dy*Yu+D0;
371    Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates
372    Yphys=(A21.*Xu+A22.*Yu+Y0)./denom;
[209]373    if testangle
374        Zphys=Z0+a*Xphys+b*Yphys;
375    else
376        Zphys=Z0;
377    end
[116]378else
379    Xphys=-Calib.Tx_Ty_Tz(1)+X/Calib.fx_fy(1);
380    Yphys=-Calib.Tx_Ty_Tz(2)+Y/Calib.fx_fy(2);
[40]381end
382
383%'px_XYZ': transform phys coordinates to image coordinates (px)
384%
385% OUPUT:
386% X,Y: array of coordinates in the image cooresponding to the input physical positions
387%                    (origin at lower leftcorner, unit=pixel)
388
389% INPUT:
390% Calib: structure containing the calibration parameters (read from the ImaDoc .xml file)
391% Xphys, Yphys: array of x,y physical coordinates
[209]392% [Z0]: corresponding array of z physical coordinates (0 by default)
[40]393
394
395
396
Note: See TracBrowser for help on using the repository browser.