Changeset 241


Ignore:
Timestamp:
Apr 19, 2011, 10:21:07 AM (10 years ago)
Author:
sommeria
Message:

phys_XYZ extracted as a main function, and corrected

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/src/transform_field/phys.m

    r211 r241  
    269269end
    270270
    271 %------------------------------------------------------------------------
    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
    279 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)>=Zindex
    284     if isfield(Calib, 'SliceAngle') && ~isequal(Calib.SliceAngle,[0 0 0])
    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;
    299     if isfield(Calib,'InterfaceCoord') && isfield(Calib,'RefractionIndex')
    300         H=Calib.InterfaceCoord(3);
    301         if H>Z0
    302             Z0virt=H-(H-Z0)/Calib.RefractionIndex; %corrected z (virtual object)
    303             test_refraction=1;
    304         end
    305     end   
    306 else
    307     Z0=0;
    308     Z0virt=0;
    309 end
    310 if ~exist('X','var')||~exist('Y','var')
    311     Xphys=[];
    312     Yphys=[];%default
    313     return
    314 end
    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
    328 if isfield(Calib,'R')
    329     R=(Calib.R)';
    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
    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);
    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)*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);
    364         %px to camera:
    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;
    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;
    373     if testangle
    374         Zphys=Z0+a*Xphys+b*Yphys;
    375     else
    376         Zphys=Z0;
    377     end
    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);
    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
    392 % [Z0]: corresponding array of z physical coordinates (0 by default)
    393 
    394 
    395 
    396 
Note: See TracChangeset for help on using the changeset viewer.