Changeset 157 for trunk

Timestamp:

Dec 19, 2010, 10:14:56 PM (14 years ago)

Author:

sommeria

Message:

bug repair on phys and phys_polar

Location:

trunk/src/transform_field

Files:

• phys.m (modified) (1 diff)
• phys_polar.m (modified) (2 diffs)

trunk/src/transform_field/phys.m

@@ -113 +113 @@
     %transform of X,Y coordinates for vector fields
     if isfield(Data,'ZIndex') && ~isempty(Data.ZIndex)&&~isnan(Data.ZIndex)
-        Z=Data.ZIndex
+        Z=Data.ZIndex;
     else
         Z=0;

trunk/src/transform_field/phys_polar.m

@@ -181 +181 @@
 end
 
- 
+%------------------------------------------------------------------------
+%'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters
+% function [Xphys,Yphys]=phys_XYZ(Calib,X,Y,Z)
+%
+%OUTPUT:
+%
+%INPUT:
+%Z: index of plane
+function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Z)
+%------------------------------------------------------------------------
+if exist('Z','var')&& isequal(Z,round(Z))&& Z>0 && isfield(Calib,'SliceCoord')&&length(Calib.SliceCoord)>=Z
+    Zindex=Z;
+    Zphys=Calib.SliceCoord(Zindex,3);%GENERALISER AUX CAS AVEC ANGLE
+else
+    Zphys=0;
+end
+if ~exist('X','var')||~exist('Y','var')
+    Xphys=[];
+    Yphys=[];%default
+    return
+end
+%coordinate transform
+if ~isfield(Calib,'fx_fy')
+     Calib.fx_fy=[1 1];
+end
+if ~isfield(Calib,'Tx_Ty_Tz')
+     Calib.Tx_Ty_Tz=[0 0 1];
+end
+if ~isfield(Calib,'Cx_Cy')
+     Calib.Cx_Cy=[0 0];
+end
+if ~isfield(Calib,'kc')
+     Calib.kc=0;
+end
+if isfield(Calib,'R')
+    R=(Calib.R)';
+    Tx=Calib.Tx_Ty_Tz(1);
+    Ty=Calib.Tx_Ty_Tz(2);
+    Tz=Calib.Tx_Ty_Tz(3);
+    f=Calib.fx_fy(1);%dpy=1; sx=1
+    dpx=Calib.fx_fy(2)/Calib.fx_fy(1);
+    Dx=R(5)*R(7)-R(4)*R(8);
+    Dy=R(1)*R(8)-R(2)*R(7);
+    D0=f*(R(2)*R(4)-R(1)*R(5));
+    Z11=R(6)*R(8)-R(5)*R(9);
+    Z12=R(2)*R(9)-R(3)*R(8);  
+    Z21=R(4)*R(9)-R(6)*R(7);
+    Z22=R(3)*R(7)-R(1)*R(9);
+    Zx0=R(3)*R(5)-R(2)*R(6);
+    Zy0=R(1)*R(6)-R(3)*R(4);
+    A11=R(8)*Ty-R(5)*Tz+Z11*Zphys;
+    A12=R(2)*Tz-R(8)*Tx+Z12*Zphys;
+    A21=-R(7)*Ty+R(4)*Tz+Z21*Zphys;
+    A22=-R(1)*Tz+R(7)*Tx+Z22*Zphys;
+    X0=f*(R(5)*Tx-R(2)*Ty+Zx0*Zphys);
+    Y0=f*(-R(4)*Tx+R(1)*Ty+Zy0*Zphys);
+        %px to camera:
+    Xd=dpx*(X-Calib.Cx_Cy(1)); % sensor coordinates
+    Yd=(Y-Calib.Cx_Cy(2));
+    dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd)/(f*f); %distortion factor
+    Xu=Xd./dist_fact;%undistorted sensor coordinates
+    Yu=Yd./dist_fact;
+    denom=Dx*Xu+Dy*Yu+D0;
+    % denom2=denom.*denom;
+    Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates
+    Yphys=(A21.*Xu+A22.*Yu+Y0)./denom;
+%     Xd=(X-Calib.Cx_Cy(1))/Calib.fx_fy(1); % sensor coordinates
+%     Yd=(Y-Calib.Cx_Cy(2))/Calib.fx_fy(2);
+%     dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd); %distortion factor
+%     Xu=Xd./dist_fact;%undistorted sensor coordinates
+%     Yu=Yd./dist_fact;
+%     A11=R(7)*Xu-R(1);
+%     A12=R(8)*Xu-R(2);
+%     A21=R(7)*Yu-R(4);
+%     A22=R(8)*Yu-R(5);
+%     B1=(R(3)-R(9)*Xu)*Zphys-Tz*Xu+Tx;
+%     B2=(R(6)-R(9)*Yu)*Zphys-Tz*Yu+Ty;
+%     deter=A12.*A21-A11.*A22;
+%     Xphys=(A21.*B1-A11.*B2)./deter;
+%     Yphys=(-A22.*B1+A12.*B2)./deter;
+else
+    Xphys=-Calib.Tx_Ty_Tz(1)+X/Calib.fx_fy(1);
+    Yphys=-Calib.Tx_Ty_Tz(2)+Y/Calib.fx_fy(2);
+end
 %%%%%%%%%%%%%%%%%%%%
 function [A_out,Rangx,Rangy]=phys_Ima(A,CalibIn,ZIndex,origin_xy,radius_offset,angle_offset,angle_scale)
@@ -247 +330 @@
 %Rangx=Rangx-radius_offset;
 
-%'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters
-% function [Xphys,Yphys]=phys_XYZ(Calib,X,Y,Z)
-%
-%OUTPUT:
-%
-%INPUT:
-%Z: index of plane
-function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Z)
-if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'SliceCoord')&length(Calib.SliceCoord)>=Z
-    Zindex=Z;
-    Zphys=Calib.SliceCoord(Zindex,3);%GENERALISER AUX CAS AVEC ANGLE
-else
-%     if exist('Z','var')
-%         Zphys=Z;
-%     else
-        Zphys=0;
-%     end
-end
-if ~exist('X','var')||~exist('Y','var')
-    Xphys=[];
-    Yphys=[];%default
-    return
-end
-Xphys=X;%default
-Yphys=Y;
-%image transform
-if isfield(Calib,'R')
-    R=(Calib.R)';
-    Dx=R(5)*R(7)-R(4)*R(8);
-    Dy=R(1)*R(8)-R(2)*R(7);
-    D0=Calib.f*(R(2)*R(4)-R(1)*R(5));
-    Z11=R(6)*R(8)-R(5)*R(9);
-    Z12=R(2)*R(9)-R(3)*R(8);  
-    Z21=R(4)*R(9)-R(6)*R(7);
-    Z22=R(3)*R(7)-R(1)*R(9);
-    Zx0=R(3)*R(5)-R(2)*R(6);
-    Zy0=R(1)*R(6)-R(3)*R(4);
-    A11=R(8)*Calib.Ty-R(5)*Calib.Tz+Z11*Zphys;
-    A12=R(2)*Calib.Tz-R(8)*Calib.Tx+Z12*Zphys;
-    A21=-R(7)*Calib.Ty+R(4)*Calib.Tz+Z21*Zphys;
-    A22=-R(1)*Calib.Tz+R(7)*Calib.Tx+Z11*Zphys;
-    X0=Calib.f*(R(5)*Calib.Tx-R(2)*Calib.Ty+Zx0*Zphys);
-    Y0=Calib.f*(-R(4)*Calib.Tx+R(1)*Calib.Ty+Zy0*Zphys);
-        %px to camera:
-    Xd=(Calib.dpx/Calib.sx)*(X-Calib.Cx); % sensor coordinates
-    Yd=Calib.dpy*(Y-Calib.Cy);
-    dist_fact=1+Calib.kappa1*(Xd.*Xd+Yd.*Yd); %distortion factor
-    Xu=dist_fact.*Xd;%undistorted sensor coordinates
-    Yu=dist_fact.*Yd;
-    denom=Dx*Xu+Dy*Yu+D0;
-    % denom2=denom.*denom;
-    Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates
-    Yphys=(A21.*Xu+A22.*Yu+Y0)./denom;
-end
-
-%'px_XYZ': transform phys coordinates to image coordinates (px)
-%
-% OUPUT:
-% X,Y: array of coordinates in the image cooresponding to the input physical positions 
-%                    (origin at lower leftcorner, unit=pixel)
-
-% INPUT:
-% Calib: structure containing the calibration parameters (read from the ImaDoc .xml file)
-% Xphys, Yphys: array of x,y physical coordinates
-% [Zphys]: corresponding array of z physical coordinates (0 by default)
-
-
-function [X,Y]=px_XYZ(Calib,Xphys,Yphys,Zphys)
-X=[];%default
-Y=[];
-% if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'PlanePos')&length(Calib.PlanePos)>=Z
-%     Zindex=Z;
-%     planepos=Calib.PlanePos{Zindex};
-%     zphys=planepos(3);%A GENERALISER CAS AVEC ANGLE
-% else
-%     zphys=0;
+
+
+% 
+% function [X,Y]=px_XYZ(Calib,Xphys,Yphys,Zphys)
+% X=[];%default
+% Y=[];
+% % if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'PlanePos')&length(Calib.PlanePos)>=Z
+% %     Zindex=Z;
+% %     planepos=Calib.PlanePos{Zindex};
+% %     zphys=planepos(3);%A GENERALISER CAS AVEC ANGLE
+% % else
+% %     zphys=0;
+% % end
+% if ~exist('Zphys','var')
+%     Zphys=0;
 % end
-if ~exist('Zphys','var')
-    Zphys=0;
-end
-
-%%%%%%%%%%%%%
-if isfield(Calib,'R')
-    R=(Calib.R)';
-    xc=R(1)*Xphys+R(2)*Yphys+R(3)*Zphys+Calib.Tx;
-    yc=R(4)*Xphys+R(5)*Yphys+R(6)*Zphys+Calib.Ty;
-    zc=R(7)*Xphys+R(8)*Yphys+R(9)*Zphys+Calib.Tz;
-%undistorted image coordinates
-    Xu=Calib.f*xc./zc;
-    Yu=Calib.f*yc./zc;
-%distorted image coordinates
-    distortion=(Calib.kappa1)*(Xu.*Xu+Yu.*Yu)+1; %A REVOIR
-% distortion=1;
-    Xd=Xu./distortion;
-    Yd=Yu./distortion;
-%pixel coordinates
-    X=Xd*Calib.sx/Calib.dpx+Calib.Cx;
-    Y=Yd/Calib.dpy+Calib.Cy;
-
-elseif isfield(Calib,'Pxcmx')&isfield(Calib,'Pxcmy')%old calib  
-        X=Xphys*Calib.Pxcmx;
-        Y=Yphys*Calib.Pxcmy;
-end
-
-
+% 
+% %%%%%%%%%%%%%
+% if isfield(Calib,'R')
+%     R=(Calib.R)';
+%     xc=R(1)*Xphys+R(2)*Yphys+R(3)*Zphys+Calib.Tx;
+%     yc=R(4)*Xphys+R(5)*Yphys+R(6)*Zphys+Calib.Ty;
+%     zc=R(7)*Xphys+R(8)*Yphys+R(9)*Zphys+Calib.Tz;
+% %undistorted image coordinates
+%     Xu=Calib.f*xc./zc;
+%     Yu=Calib.f*yc./zc;
+% %distorted image coordinates
+%     distortion=(Calib.kappa1)*(Xu.*Xu+Yu.*Yu)+1; %A REVOIR
+% % distortion=1;
+%     Xd=Xu./distortion;
+%     Yd=Yu./distortion;
+% %pixel coordinates
+%     X=Xd*Calib.sx/Calib.dpx+Calib.Cx;
+%     Y=Yd/Calib.dpy+Calib.Cy;
+% 
+% elseif isfield(Calib,'Pxcmx')&isfield(Calib,'Pxcmy')%old calib  
+%         X=Xphys*Calib.Pxcmx;
+%         Y=Yphys*Calib.Pxcmy;
+% end
+% 
+