Changeset 157 for trunk/src/transform_field/phys_polar.m
 Timestamp:
 Dec 19, 2010, 10:14:56 PM (13 years ago)
 File:

 1 edited
Legend:
 Unmodified
 Added
 Removed

trunk/src/transform_field/phys_polar.m
r93 r157 181 181 end 182 182 183 183 % 184 %'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters 185 % function [Xphys,Yphys]=phys_XYZ(Calib,X,Y,Z) 186 % 187 %OUTPUT: 188 % 189 %INPUT: 190 %Z: index of plane 191 function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Z) 192 % 193 if exist('Z','var')&& isequal(Z,round(Z))&& Z>0 && isfield(Calib,'SliceCoord')&&length(Calib.SliceCoord)>=Z 194 Zindex=Z; 195 Zphys=Calib.SliceCoord(Zindex,3);%GENERALISER AUX CAS AVEC ANGLE 196 else 197 Zphys=0; 198 end 199 if ~exist('X','var')~exist('Y','var') 200 Xphys=[]; 201 Yphys=[];%default 202 return 203 end 204 %coordinate transform 205 if ~isfield(Calib,'fx_fy') 206 Calib.fx_fy=[1 1]; 207 end 208 if ~isfield(Calib,'Tx_Ty_Tz') 209 Calib.Tx_Ty_Tz=[0 0 1]; 210 end 211 if ~isfield(Calib,'Cx_Cy') 212 Calib.Cx_Cy=[0 0]; 213 end 214 if ~isfield(Calib,'kc') 215 Calib.kc=0; 216 end 217 if isfield(Calib,'R') 218 R=(Calib.R)'; 219 Tx=Calib.Tx_Ty_Tz(1); 220 Ty=Calib.Tx_Ty_Tz(2); 221 Tz=Calib.Tx_Ty_Tz(3); 222 f=Calib.fx_fy(1);%dpy=1; sx=1 223 dpx=Calib.fx_fy(2)/Calib.fx_fy(1); 224 Dx=R(5)*R(7)R(4)*R(8); 225 Dy=R(1)*R(8)R(2)*R(7); 226 D0=f*(R(2)*R(4)R(1)*R(5)); 227 Z11=R(6)*R(8)R(5)*R(9); 228 Z12=R(2)*R(9)R(3)*R(8); 229 Z21=R(4)*R(9)R(6)*R(7); 230 Z22=R(3)*R(7)R(1)*R(9); 231 Zx0=R(3)*R(5)R(2)*R(6); 232 Zy0=R(1)*R(6)R(3)*R(4); 233 A11=R(8)*TyR(5)*Tz+Z11*Zphys; 234 A12=R(2)*TzR(8)*Tx+Z12*Zphys; 235 A21=R(7)*Ty+R(4)*Tz+Z21*Zphys; 236 A22=R(1)*Tz+R(7)*Tx+Z22*Zphys; 237 X0=f*(R(5)*TxR(2)*Ty+Zx0*Zphys); 238 Y0=f*(R(4)*Tx+R(1)*Ty+Zy0*Zphys); 239 %px to camera: 240 Xd=dpx*(XCalib.Cx_Cy(1)); % sensor coordinates 241 Yd=(YCalib.Cx_Cy(2)); 242 dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd)/(f*f); %distortion factor 243 Xu=Xd./dist_fact;%undistorted sensor coordinates 244 Yu=Yd./dist_fact; 245 denom=Dx*Xu+Dy*Yu+D0; 246 % denom2=denom.*denom; 247 Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates 248 Yphys=(A21.*Xu+A22.*Yu+Y0)./denom; 249 % Xd=(XCalib.Cx_Cy(1))/Calib.fx_fy(1); % sensor coordinates 250 % Yd=(YCalib.Cx_Cy(2))/Calib.fx_fy(2); 251 % dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd); %distortion factor 252 % Xu=Xd./dist_fact;%undistorted sensor coordinates 253 % Yu=Yd./dist_fact; 254 % A11=R(7)*XuR(1); 255 % A12=R(8)*XuR(2); 256 % A21=R(7)*YuR(4); 257 % A22=R(8)*YuR(5); 258 % B1=(R(3)R(9)*Xu)*ZphysTz*Xu+Tx; 259 % B2=(R(6)R(9)*Yu)*ZphysTz*Yu+Ty; 260 % deter=A12.*A21A11.*A22; 261 % Xphys=(A21.*B1A11.*B2)./deter; 262 % Yphys=(A22.*B1+A12.*B2)./deter; 263 else 264 Xphys=Calib.Tx_Ty_Tz(1)+X/Calib.fx_fy(1); 265 Yphys=Calib.Tx_Ty_Tz(2)+Y/Calib.fx_fy(2); 266 end 184 267 %%%%%%%%%%%%%%%%%%%% 185 268 function [A_out,Rangx,Rangy]=phys_Ima(A,CalibIn,ZIndex,origin_xy,radius_offset,angle_offset,angle_scale) … … 247 330 %Rangx=Rangxradius_offset; 248 331 249 %'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters 250 % function [Xphys,Yphys]=phys_XYZ(Calib,X,Y,Z) 251 % 252 %OUTPUT: 253 % 254 %INPUT: 255 %Z: index of plane 256 function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Z) 257 if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'SliceCoord')&length(Calib.SliceCoord)>=Z 258 Zindex=Z; 259 Zphys=Calib.SliceCoord(Zindex,3);%GENERALISER AUX CAS AVEC ANGLE 260 else 261 % if exist('Z','var') 262 % Zphys=Z; 263 % else 264 Zphys=0; 265 % end 266 end 267 if ~exist('X','var')~exist('Y','var') 268 Xphys=[]; 269 Yphys=[];%default 270 return 271 end 272 Xphys=X;%default 273 Yphys=Y; 274 %image transform 275 if isfield(Calib,'R') 276 R=(Calib.R)'; 277 Dx=R(5)*R(7)R(4)*R(8); 278 Dy=R(1)*R(8)R(2)*R(7); 279 D0=Calib.f*(R(2)*R(4)R(1)*R(5)); 280 Z11=R(6)*R(8)R(5)*R(9); 281 Z12=R(2)*R(9)R(3)*R(8); 282 Z21=R(4)*R(9)R(6)*R(7); 283 Z22=R(3)*R(7)R(1)*R(9); 284 Zx0=R(3)*R(5)R(2)*R(6); 285 Zy0=R(1)*R(6)R(3)*R(4); 286 A11=R(8)*Calib.TyR(5)*Calib.Tz+Z11*Zphys; 287 A12=R(2)*Calib.TzR(8)*Calib.Tx+Z12*Zphys; 288 A21=R(7)*Calib.Ty+R(4)*Calib.Tz+Z21*Zphys; 289 A22=R(1)*Calib.Tz+R(7)*Calib.Tx+Z11*Zphys; 290 X0=Calib.f*(R(5)*Calib.TxR(2)*Calib.Ty+Zx0*Zphys); 291 Y0=Calib.f*(R(4)*Calib.Tx+R(1)*Calib.Ty+Zy0*Zphys); 292 %px to camera: 293 Xd=(Calib.dpx/Calib.sx)*(XCalib.Cx); % sensor coordinates 294 Yd=Calib.dpy*(YCalib.Cy); 295 dist_fact=1+Calib.kappa1*(Xd.*Xd+Yd.*Yd); %distortion factor 296 Xu=dist_fact.*Xd;%undistorted sensor coordinates 297 Yu=dist_fact.*Yd; 298 denom=Dx*Xu+Dy*Yu+D0; 299 % denom2=denom.*denom; 300 Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates 301 Yphys=(A21.*Xu+A22.*Yu+Y0)./denom; 302 end 303 304 %'px_XYZ': transform phys coordinates to image coordinates (px) 305 % 306 % OUPUT: 307 % X,Y: array of coordinates in the image cooresponding to the input physical positions 308 % (origin at lower leftcorner, unit=pixel) 309 310 % INPUT: 311 % Calib: structure containing the calibration parameters (read from the ImaDoc .xml file) 312 % Xphys, Yphys: array of x,y physical coordinates 313 % [Zphys]: corresponding array of z physical coordinates (0 by default) 314 315 316 function [X,Y]=px_XYZ(Calib,Xphys,Yphys,Zphys) 317 X=[];%default 318 Y=[]; 319 % if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'PlanePos')&length(Calib.PlanePos)>=Z 320 % Zindex=Z; 321 % planepos=Calib.PlanePos{Zindex}; 322 % zphys=planepos(3);%A GENERALISER CAS AVEC ANGLE 323 % else 324 % zphys=0; 332 333 334 % 335 % function [X,Y]=px_XYZ(Calib,Xphys,Yphys,Zphys) 336 % X=[];%default 337 % Y=[]; 338 % % if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'PlanePos')&length(Calib.PlanePos)>=Z 339 % % Zindex=Z; 340 % % planepos=Calib.PlanePos{Zindex}; 341 % % zphys=planepos(3);%A GENERALISER CAS AVEC ANGLE 342 % % else 343 % % zphys=0; 344 % % end 345 % if ~exist('Zphys','var') 346 % Zphys=0; 325 347 % end 326 if ~exist('Zphys','var') 327 Zphys=0; 328 end 329 330 %%%%%%%%%%%%% 331 if isfield(Calib,'R') 332 R=(Calib.R)'; 333 xc=R(1)*Xphys+R(2)*Yphys+R(3)*Zphys+Calib.Tx; 334 yc=R(4)*Xphys+R(5)*Yphys+R(6)*Zphys+Calib.Ty; 335 zc=R(7)*Xphys+R(8)*Yphys+R(9)*Zphys+Calib.Tz; 336 %undistorted image coordinates 337 Xu=Calib.f*xc./zc; 338 Yu=Calib.f*yc./zc; 339 %distorted image coordinates 340 distortion=(Calib.kappa1)*(Xu.*Xu+Yu.*Yu)+1; %A REVOIR 341 % distortion=1; 342 Xd=Xu./distortion; 343 Yd=Yu./distortion; 344 %pixel coordinates 345 X=Xd*Calib.sx/Calib.dpx+Calib.Cx; 346 Y=Yd/Calib.dpy+Calib.Cy; 347 348 elseif isfield(Calib,'Pxcmx')&isfield(Calib,'Pxcmy')%old calib 349 X=Xphys*Calib.Pxcmx; 350 Y=Yphys*Calib.Pxcmy; 351 end 352 353 348 % 349 % %%%%%%%%%%%%% 350 % if isfield(Calib,'R') 351 % R=(Calib.R)'; 352 % xc=R(1)*Xphys+R(2)*Yphys+R(3)*Zphys+Calib.Tx; 353 % yc=R(4)*Xphys+R(5)*Yphys+R(6)*Zphys+Calib.Ty; 354 % zc=R(7)*Xphys+R(8)*Yphys+R(9)*Zphys+Calib.Tz; 355 % %undistorted image coordinates 356 % Xu=Calib.f*xc./zc; 357 % Yu=Calib.f*yc./zc; 358 % %distorted image coordinates 359 % distortion=(Calib.kappa1)*(Xu.*Xu+Yu.*Yu)+1; %A REVOIR 360 % % distortion=1; 361 % Xd=Xu./distortion; 362 % Yd=Yu./distortion; 363 % %pixel coordinates 364 % X=Xd*Calib.sx/Calib.dpx+Calib.Cx; 365 % Y=Yd/Calib.dpy+Calib.Cy; 366 % 367 % elseif isfield(Calib,'Pxcmx')&isfield(Calib,'Pxcmy')%old calib 368 % X=Xphys*Calib.Pxcmx; 369 % Y=Yphys*Calib.Pxcmy; 370 % end 371 % 372
Note: See TracChangeset
for help on using the changeset viewer.