Changeset 157


Ignore:
Timestamp:
Dec 19, 2010, 10:14:56 PM (10 years ago)
Author:
sommeria
Message:

bug repair on phys and phys_polar

Location:
trunk/src/transform_field
Files:
2 edited

Legend:

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

    r151 r157  
    113113    %transform of X,Y coordinates for vector fields
    114114    if isfield(Data,'ZIndex') && ~isempty(Data.ZIndex)&&~isnan(Data.ZIndex)
    115         Z=Data.ZIndex
     115        Z=Data.ZIndex;
    116116    else
    117117        Z=0;
  • trunk/src/transform_field/phys_polar.m

    r93 r157  
    181181end
    182182
    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
     191function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Z)
     192%------------------------------------------------------------------------
     193if 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
     196else
     197    Zphys=0;
     198end
     199if ~exist('X','var')||~exist('Y','var')
     200    Xphys=[];
     201    Yphys=[];%default
     202    return
     203end
     204%coordinate transform
     205if ~isfield(Calib,'fx_fy')
     206     Calib.fx_fy=[1 1];
     207end
     208if ~isfield(Calib,'Tx_Ty_Tz')
     209     Calib.Tx_Ty_Tz=[0 0 1];
     210end
     211if ~isfield(Calib,'Cx_Cy')
     212     Calib.Cx_Cy=[0 0];
     213end
     214if ~isfield(Calib,'kc')
     215     Calib.kc=0;
     216end
     217if 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)*Ty-R(5)*Tz+Z11*Zphys;
     234    A12=R(2)*Tz-R(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)*Tx-R(2)*Ty+Zx0*Zphys);
     238    Y0=f*(-R(4)*Tx+R(1)*Ty+Zy0*Zphys);
     239        %px to camera:
     240    Xd=dpx*(X-Calib.Cx_Cy(1)); % sensor coordinates
     241    Yd=(Y-Calib.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=(X-Calib.Cx_Cy(1))/Calib.fx_fy(1); % sensor coordinates
     250%     Yd=(Y-Calib.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)*Xu-R(1);
     255%     A12=R(8)*Xu-R(2);
     256%     A21=R(7)*Yu-R(4);
     257%     A22=R(8)*Yu-R(5);
     258%     B1=(R(3)-R(9)*Xu)*Zphys-Tz*Xu+Tx;
     259%     B2=(R(6)-R(9)*Yu)*Zphys-Tz*Yu+Ty;
     260%     deter=A12.*A21-A11.*A22;
     261%     Xphys=(A21.*B1-A11.*B2)./deter;
     262%     Yphys=(-A22.*B1+A12.*B2)./deter;
     263else
     264    Xphys=-Calib.Tx_Ty_Tz(1)+X/Calib.fx_fy(1);
     265    Yphys=-Calib.Tx_Ty_Tz(2)+Y/Calib.fx_fy(2);
     266end
    184267%%%%%%%%%%%%%%%%%%%%
    185268function [A_out,Rangx,Rangy]=phys_Ima(A,CalibIn,ZIndex,origin_xy,radius_offset,angle_offset,angle_scale)
     
    247330%Rangx=Rangx-radius_offset;
    248331
    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.Ty-R(5)*Calib.Tz+Z11*Zphys;
    287     A12=R(2)*Calib.Tz-R(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.Tx-R(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)*(X-Calib.Cx); % sensor coordinates
    294     Yd=Calib.dpy*(Y-Calib.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;
    325347% 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.