[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 


 96  %


 97  function DataOut=phys_1(Data,Calib)


 98  % for icell=1:length(Data)


 99 


 100  DataOut=Data;%default


 101  DataOut.CoordType='phys'; %put flag for physical coordinates


 102  % The transform ACTS ONLY IF .CoordType='px'and Calib defined


 103  if isfield(Data,'CoordType')&& isequal(Data.CoordType,'px')&& ~isempty(Calib)


 104  if isfield(Calib,'CoordUnit')


 105  DataOut.CoordUnit=Calib.CoordUnit;


 106  else


 107  DataOut.CoordUnit='cm'; %default


 108  % elseif isfield(DataOut,'CoordUnit')


 109  % DataOut=rmfield(DataOut,'CoordUnit');


 110  end


 111  DataOut.TimeUnit='s';


 112  %transform of X,Y coordinates for vector fields


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


 114  Z=Data.ZIndex


[40]  115  else


 116  Z=0;


 117  end


 118  if isfield(Data,'X') &&isfield(Data,'Y')&&~isempty(Data.X) && ~isempty(Data.Y)


 119  [DataOut.X,DataOut.Y,DataOut.Z]=phys_XYZ(Calib,Data.X,Data.Y,Z);


 120  if isfield(Data,'U')&&isfield(Data,'V')&&~isempty(Data.U) && ~isempty(Data.V)&& isfield(Data,'dt')


 121  if ~isempty(Data.dt)


 122  [XOut_1,YOut_1]=phys_XYZ(Calib,Data.XData.U/2,Data.YData.V/2,Z);


 123  [XOut_2,YOut_2]=phys_XYZ(Calib,Data.X+Data.U/2,Data.Y+Data.V/2,Z);


 124  DataOut.U=(XOut_2XOut_1)/Data.dt;


 125  DataOut.V=(YOut_2YOut_1)/Data.dt;


 126  end


 127  end


 128  end


 129  %transform of an image or scalar: done in phys_ima


 130 


 131  %transform of spatial derivatives


 132  if isfield(Data,'X') && ~isempty(Data.X) && isfield(Data,'DjUi') && ~isempty(Data.DjUi)...


 133  && isfield(Data,'dt')


 134  if ~isempty(Data.dt)


 135  % estimate the Jacobian matrix DXpx/DXphys


 136  for ip=1:length(Data.X)


 137  [Xp1,Yp1]=phys_XYZ(Calib,Data.X(ip)+0.5,Data.Y(ip),Z);


 138  [Xm1,Ym1]=phys_XYZ(Calib,Data.X(ip)0.5,Data.Y(ip),Z);


 139  [Xp2,Yp2]=phys_XYZ(Calib,Data.X(ip),Data.Y(ip)+0.5,Z);


 140  [Xm2,Ym2]=phys_XYZ(Calib,Data.X(ip),Data.Y(ip)0.5,Z);


 141  %Jacobian matrix DXpphys/DXpx


 142  DjXi(1,1)=(Xp1Xm1);


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


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


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


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


 147  DjUi=(DjXi*DjUi')/DjXi;% =J1*M*J , curvature effects (derivatives of J) neglected


 148  DataOut.DjUi(ip,:,:)=DjUi';


 149  end


 150  DataOut.DjUi = DataOut.DjUi/Data.dt; % min(Data.DjUi(:,1,1))=DUDX


 151  end


 152  end


 153  end


 154 


 155  %%%%%%%%%%%%%%%%%%%%


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


[116]  157 


 158  if ndims(A)==3


 159  A=mean(A,3);


 160  end


 161 


 162 


[40]  163  xcorner=[];


 164  ycorner=[];


 165  npx=[];


 166  npy=[];


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


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


[40]  169  for icell=1:length(A)


 170  siz=size(A{icell});


 171  npx=[npx siz(2)];


 172  npy=[npy siz(1)];


 173  Calib=CalibIn{icell};


[116]  174  xima=[0.5 siz(2)0.5 0.5 siz(2)0.5];%image coordinates of corners


[40]  175  yima=[0.5 0.5 siz(1)0.5 siz(1)0.5];


 176  [xcorner_new,ycorner_new]=phys_XYZ(Calib,xima,yima,ZIndex);%corresponding physical coordinates


[79]  177  dx(icell)=(max(xcorner_new)min(xcorner_new))/(siz(2)1);


 178  dy(icell)=(max(ycorner_new)min(ycorner_new))/(siz(1)1);


[40]  179  xcorner=[xcorner xcorner_new];


 180  ycorner=[ycorner ycorner_new];


 181  end


 182  Rangx(1)=min(xcorner);


 183  Rangx(2)=max(xcorner);


 184  Rangy(2)=min(ycorner);


 185  Rangy(1)=max(ycorner);


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


[79]  187  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)


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


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


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


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


 192  vec_B=[];


 193  A_out={};


 194  for icell=1:length(A)


 195  Calib=CalibIn{icell};


[116]  196  if isfield(Calib,'R')  isfield(Calib,'kc') test_multi ~isequal(Calib,CalibIn{1})% the image needs to be interpolated to the new coordinates


[40]  197  zphys=0; %default


 198  if isfield(Calib,'SliceCoord') %.Z= index of plane


 199  SliceCoord=Calib.SliceCoord(ZIndex,:);


 200  zphys=SliceCoord(3); %to generalize for nonparallel planes


 201  end


[79]  202  [XIMA,YIMA]=px_XYZ(CalibIn{icell},X,Y,zphys);% image coordinates for each point in the real space grid


 203  XIMA=reshape(round(XIMA),1,npX*npY);%indices reorganized in 'line'


 204  YIMA=reshape(round(YIMA),1,npX*npY);


 205  flagin=XIMA>=1 & XIMA<=npx(icell) & YIMA >=1 & YIMA<=npy(icell);%flagin=1 inside the original image


[40]  206  testuint8=isa(A{icell},'uint8');


 207  testuint16=isa(A{icell},'uint16');


 208  if numel(siz)==2 %(B/W images)


[79]  209  vec_A=reshape(A{icell},1,npx(icell)*npy(icell));%put the original image in line


[40]  210  ind_in=find(flagin);


 211  ind_out=find(~flagin);


[79]  212  ICOMB=((XIMA1)*npy(icell)+(npy(icell)+1YIMA));


[40]  213  ICOMB=ICOMB(flagin);%index corresponding to XIMA and YIMA in the aligned original image vec_A


 214  vec_B(ind_in)=vec_A(ICOMB);


 215  vec_B(ind_out)=zeros(size(ind_out));


[79]  216  A_out{icell}=reshape(vec_B,npY,npX);%new image in real coordinates


[40]  217  elseif numel(siz)==3


 218  for icolor=1:siz(3)


 219  vec_A=reshape(A{icell}(:,:,icolor),1,npx*npy);%put the original image in line


 220  ind_in=find(flagin);


 221  ind_out=find(~flagin);


 222  ICOMB=((XIMA1)*npy+(npy+1YIMA));


 223  ICOMB=ICOMB(flagin);%index corresponding to XIMA and YIMA in the aligned original image vec_A


 224  vec_B(ind_in)=vec_A(ICOMB);


 225  vec_B(ind_out)=zeros(size(ind_out));


 226  A_out{icell}(:,:,icolor)=reshape(vec_B,npy,npx);%new image in real coordinates


 227  end


 228  end


 229  if testuint8


 230  A_out{icell}=uint8(A_out{icell});


 231  end


 232  if testuint16


 233  A_out{icell}=uint16(A_out{icell});


 234  end


[116]  235  else%


[40]  236  A_out{icell}=A{icell};%no transform


 237  Rangx=[0.5 npx0.5];%image coordiantes of corners


 238  Rangy=[npy0.5 0.5];


[116]  239  [Rangx]=phys_XYZ(Calib,Rangx,[0.5 0.5],ZIndex);%case of translations without rotation and quadratic deformation


 240  [xx,Rangy]=phys_XYZ(Calib,[0.5 0.5],Rangy,ZIndex);


[40]  241  end


 242  end


 243 


[116]  244  %


[40]  245  %'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters


 246  % function [Xphys,Yphys]=phys_XYZ(Calib,X,Y,Z)


 247  %


 248  %OUTPUT:


 249  %


 250  %INPUT:


 251  %Z: index of plane


 252  function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Z)


[116]  253  %


 254  if exist('Z','var')&& isequal(Z,round(Z))&& Z>0 && isfield(Calib,'SliceCoord')&&length(Calib.SliceCoord)>=Z


[40]  255  Zindex=Z;


 256  Zphys=Calib.SliceCoord(Zindex,3);%GENERALISER AUX CAS AVEC ANGLE


 257  else


[116]  258  Zphys=0;


[40]  259  end


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


 261  Xphys=[];


 262  Yphys=[];%default


 263  return


 264  end


[116]  265  %coordinate transform


 266  if ~isfield(Calib,'fx_fy')


 267  Calib.fx_fy=[1 1];


 268  end


 269  if ~isfield(Calib,'Tx_Ty_Tz')


 270  Calib.Tx_Ty_Tz=[0 0 1];


 271  end


 272  if ~isfield(Calib,'Cx_Cy')


 273  Calib.Cx_Cy=[0 0];


 274  end


 275  if ~isfield(Calib,'kc')


 276  Calib.kc=0;


 277  end


[40]  278  if isfield(Calib,'R')


 279  R=(Calib.R)';


[116]  280  Tx=Calib.Tx_Ty_Tz(1);


 281  Ty=Calib.Tx_Ty_Tz(2);


 282  Tz=Calib.Tx_Ty_Tz(3);


 283  f=Calib.fx_fy(1);%dpy=1; sx=1


 284  dpx=Calib.fx_fy(2)/Calib.fx_fy(1);


[40]  285  Dx=R(5)*R(7)R(4)*R(8);


 286  Dy=R(1)*R(8)R(2)*R(7);


[116]  287  D0=f*(R(2)*R(4)R(1)*R(5));


[40]  288  Z11=R(6)*R(8)R(5)*R(9);


 289  Z12=R(2)*R(9)R(3)*R(8);


 290  Z21=R(4)*R(9)R(6)*R(7);


 291  Z22=R(3)*R(7)R(1)*R(9);


 292  Zx0=R(3)*R(5)R(2)*R(6);


 293  Zy0=R(1)*R(6)R(3)*R(4);


[116]  294  A11=R(8)*TyR(5)*Tz+Z11*Zphys;


 295  A12=R(2)*TzR(8)*Tx+Z12*Zphys;


 296  A21=R(7)*Ty+R(4)*Tz+Z21*Zphys;


 297  A22=R(1)*Tz+R(7)*Tx+Z22*Zphys;


 298  X0=f*(R(5)*TxR(2)*Ty+Zx0*Zphys);


 299  Y0=f*(R(4)*Tx+R(1)*Ty+Zy0*Zphys);


[40]  300  %px to camera:


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


 302  Yd=(YCalib.Cx_Cy(2));


 303  dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd)/(f*f); %distortion factor


 304  Xu=Xd./dist_fact;%undistorted sensor coordinates


 305  Yu=Yd./dist_fact;


[40]  306  denom=Dx*Xu+Dy*Yu+D0;


 307  % denom2=denom.*denom;


 308  Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates


 309  Yphys=(A21.*Xu+A22.*Yu+Y0)./denom;


[116]  310  % Xd=(XCalib.Cx_Cy(1))/Calib.fx_fy(1); % sensor coordinates


 311  % Yd=(YCalib.Cx_Cy(2))/Calib.fx_fy(2);


 312  % dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd); %distortion factor


 313  % Xu=Xd./dist_fact;%undistorted sensor coordinates


 314  % Yu=Yd./dist_fact;


 315  % A11=R(7)*XuR(1);


 316  % A12=R(8)*XuR(2);


 317  % A21=R(7)*YuR(4);


 318  % A22=R(8)*YuR(5);


 319  % B1=(R(3)R(9)*Xu)*ZphysTz*Xu+Tx;


 320  % B2=(R(6)R(9)*Yu)*ZphysTz*Yu+Ty;


 321  % deter=A12.*A21A11.*A22;


 322  % Xphys=(A21.*B1A11.*B2)./deter;


 323  % Yphys=(A22.*B1+A12.*B2)./deter;


 324  else


 325  Xphys=Calib.Tx_Ty_Tz(1)+X/Calib.fx_fy(1);


 326  Yphys=Calib.Tx_Ty_Tz(2)+Y/Calib.fx_fy(2);


[40]  327  end


 328 


 329  %'px_XYZ': transform phys coordinates to image coordinates (px)


 330  %


 331  % OUPUT:


 332  % X,Y: array of coordinates in the image cooresponding to the input physical positions


 333  % (origin at lower leftcorner, unit=pixel)


 334 


 335  % INPUT:


 336  % Calib: structure containing the calibration parameters (read from the ImaDoc .xml file)


 337  % Xphys, Yphys: array of x,y physical coordinates


 338  % [Zphys]: corresponding array of z physical coordinates (0 by default)


 339 


 340 


 341 


 342 

