Changeset 241 for trunk/src/transform_field/phys.m
 Timestamp:
 Apr 19, 2011, 10:21:07 AM (13 years ago)
 File:

 1 edited
Legend:
 Unmodified
 Added
 Removed

trunk/src/transform_field/phys.m
r211 r241 269 269 end 270 270 271 %272 %'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters273 % function [Xphys,Yphys]=phys_XYZ(Calib,X,Y,Z)274 %275 %OUTPUT:276 %277 %INPUT:278 %Z: index of plane279 function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Zindex)280 %281 testangle=0;282 test_refraction=0;283 if exist('Zindex','var')&& isequal(Zindex,round(Zindex))&& Zindex>0 && isfield(Calib,'SliceCoord')&&length(Calib.SliceCoord)>=Zindex284 if isfield(Calib, 'SliceAngle') && ~isequal(Calib.SliceAngle,[0 0 0])285 testangle=1;286 om=norm(Calib.SliceAngle(Zindex,:));%norm of rotation angle in radians287 OmAxis=Calib.SliceAngle(Zindex,:)/om; %unit vector marking the rotation axis288 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 else296 Z0=Calib.SliceCoord(Zindex,3);%horizontal plane z=cte297 end298 Z0virt=Z0;299 if isfield(Calib,'InterfaceCoord') && isfield(Calib,'RefractionIndex')300 H=Calib.InterfaceCoord(3);301 if H>Z0302 Z0virt=H(HZ0)/Calib.RefractionIndex; %corrected z (virtual object)303 test_refraction=1;304 end305 end306 else307 Z0=0;308 Z0virt=0;309 end310 if ~exist('X','var')~exist('Y','var')311 Xphys=[];312 Yphys=[];%default313 return314 end315 %coordinate transform316 if ~isfield(Calib,'fx_fy')317 Calib.fx_fy=[1 1];318 end319 if ~isfield(Calib,'Tx_Ty_Tz')320 Calib.Tx_Ty_Tz=[0 0 1];321 end322 if ~isfield(Calib,'Cx_Cy')323 Calib.Cx_Cy=[0 0];324 end325 if ~isfield(Calib,'kc')326 Calib.kc=0;327 end328 if isfield(Calib,'R')329 R=(Calib.R)';330 if testangle331 a=norm_plane(1)/norm_plane(3);332 b=norm_plane(2)/norm_plane(3);333 if test_refraction334 a=a/Calib.RefractionIndex;335 b=b/Calib.RefractionIndex;336 end337 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 end344 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=1348 dpx=Calib.fx_fy(2)/Calib.fx_fy(1);349 Dx=R(5)*R(7)R(4)*R(8);350 Dy=R(1)*R(8)R(2)*R(7);351 D0=f*(R(2)*R(4)R(1)*R(5));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);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);364 %px to camera:365 Xd=dpx*(XCalib.Cx_Cy(1)); % sensor coordinates366 Yd=(YCalib.Cx_Cy(2));367 dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd)/(f*f); %distortion factor368 Xu=Xd./dist_fact;%undistorted sensor coordinates369 Yu=Yd./dist_fact;370 denom=Dx*Xu+Dy*Yu+D0;371 Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates372 Yphys=(A21.*Xu+A22.*Yu+Y0)./denom;373 if testangle374 Zphys=Z0+a*Xphys+b*Yphys;375 else376 Zphys=Z0;377 end378 else379 Xphys=Calib.Tx_Ty_Tz(1)+X/Calib.fx_fy(1);380 Yphys=Calib.Tx_Ty_Tz(2)+Y/Calib.fx_fy(2);381 end382 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 positions387 % (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 coordinates392 % [Z0]: corresponding array of z physical coordinates (0 by default)393 394 395 396
Note: See TracChangeset
for help on using the changeset viewer.