[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 


 17  function [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)


 26  Calib{1}=[];


 27  if nargin==2nargin==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};


 36  else


 37  DataOut.Txt='wrong input: need two or four structures';


 38  end


 39  test_1=0;


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


 48  end


 49  iscalar=0;


 50  if ~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


 58  end


 59  %transform of X,Y coordinates for vector fields


[116]  60  if isfield(Data,'ZIndex')&&~isempty(Data.ZIndex)&&~isnan(Data.ZIndex)


[40]  61  ZIndex=Data.ZIndex;


 62  else


[116]  63  ZIndex=1;


[40]  64  end


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


 83  end


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


 94  end


 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  %


 110  function DataOut=phys_1(Data,Calib)


 111  % for icell=1:length(Data)


 112 


 113  DataOut=Data;%default


[167]  114  % DataOut.CoordUnit=Calib.CoordUnit; %put flag for physical coordinates


[209]  115  if 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]  120  end


[40]  121  % The transform ACTS ONLY IF .CoordType='px'and Calib defined


[158]  122  if 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.XData.U/2,Data.YData.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_2XOut_1)/Data.dt;


 150  DataOut.V=(YOut_2YOut_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)=(Xp1Xm1);


 168  DjXi(2,1)=(Yp1Ym1);


 169  DjXi(1,2)=(Xp2Xm2);


 170  DjXi(2,2)=(Yp2Ym2);


 171  DjUi(:,:)=Data.DjUi(ip,:,:);


 172  DjUi=(DjXi*DjUi')/DjXi;% =J1*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


 178  end


 179 


 180  %%%%%%%%%%%%%%%%%%%%


 181  function [A_out,Rangx,Rangy]=phys_Ima(A,CalibIn,ZIndex)


 182  xcorner=[];


 183  ycorner=[];


 184  npx=[];


 185  npy=[];


[116]  186  dx=ones(1,length(A));


 187  dy=ones(1,length(A));


[40]  188  for 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];


 200  end


 201  Rangx(1)=min(xcorner);


 202  Rangx(2)=max(xcorner);


 203  Rangy(2)=min(ycorner);


 204  Rangy(1)=max(ycorner);


[116]  205  test_multi=(max(npx)~=min(npx))  (max(npy)~=min(npy)); %different image lengths


[79]  206  npX=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)


 207  npY=1+round((Rangy(1)Rangy(2))/min(dy));


 208  x=linspace(Rangx(1),Rangx(2),npX);


 209  y=linspace(Rangy(1),Rangy(2),npY);


[40]  210  [X,Y]=meshgrid(x,y);%grid in physical coordiantes


 211  vec_B=[];


 212  A_out={};


 213  for 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 nonparallel planes


[202]  220  if isfield(Calib,'InterfaceCoord') && isfield(Calib,'RefractionIndex')


 221  H=Calib.InterfaceCoord(3);


 222  if H>zphys


 223  zphys=H(Hzphys)/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=((XIMA1)*npy(icell)+(npy(icell)+1YIMA));


[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=((XIMA1)*npy+(npy+1YIMA));


 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 npx0.5];%image coordiantes of corners


 265  Rangy=[npy0.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


 269  end


 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]  279  function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Zindex)


[116]  280  %


[209]  281  testangle=0;


 282  test_refraction=0;


 283  if 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)*(1cos_om);


 291  norm_plane(1)=OmAxis(1)*coeff+OmAxis(2)*sin_om;


 292  norm_plane(2)=OmAxis(2)*coeffOmAxis(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(HZ0)/Calib.RefractionIndex; %corrected z (virtual object)


 303  test_refraction=1;


[202]  304  end


 305  end


[40]  306  else


[209]  307  Z0=0;


 308  Z0virt=0;


[40]  309  end


 310  if ~exist('X','var')~exist('Y','var')


 311  Xphys=[];


 312  Yphys=[];%default


 313  return


 314  end


[116]  315  %coordinate transform


 316  if ~isfield(Calib,'fx_fy')


 317  Calib.fx_fy=[1 1];


 318  end


 319  if ~isfield(Calib,'Tx_Ty_Tz')


 320  Calib.Tx_Ty_Tz=[0 0 1];


 321  end


 322  if ~isfield(Calib,'Cx_Cy')


 323  Calib.Cx_Cy=[0 0];


 324  end


 325  if ~isfield(Calib,'kc')


 326  Calib.kc=0;


 327  end


[40]  328  if 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)*TyR(5)*Tz+Z11*Z0virt;


 359  A12=R(2)*TzR(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)*TxR(2)*Ty+Zx0*Z0virt);


 363  Y0=f*(R(4)*Tx+R(1)*Ty+Zy0*Z0virt);


[40]  364  %px to camera:


[116]  365  Xd=dpx*(XCalib.Cx_Cy(1)); % sensor coordinates


 366  Yd=(YCalib.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]  378  else


 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]  381  end


 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 

