Changeset 114 for trunk/src/px_XYZ.m

Timestamp:

Oct 14, 2010, 6:35:03 PM (14 years ago)

Author:

sommeria

Message:

geometric calibration procedures updated

File:

• trunk/src/px_XYZ.m (modified) (2 diffs)

trunk/src/px_XYZ.m

@@ -11 +11 @@
 
 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;
@@ -23 +21 @@
     Zphys=0;
 end
-if ~isfield(Calib,'f')
-    Calib.f=1;
+if ~isfield(Calib,'fx_fy')
+     Calib.fx_fy=[1 1];
 end
-if ~isfield(Calib,'kappa1')
-    Calib.kappa1=0;
+if ~isfield(Calib,'Tx_Ty_Tz')
+     Calib.Tx_Ty_Tz=[0 0 1];
 end
-if ~isfield(Calib,'sx')
-    Calib.sx=1;
-end
-if ~isfield(Calib,'dpx')
-    Calib.dpx=1;
-end
-if ~isfield(Calib,'dpy')
-    Calib.dpy=1;
-end
-if ~isfield(Calib,'Cx')
-    Calib.Cx=0;
-end
-if ~isfield(Calib,'Cy')
-    Calib.Cy=0;
-end
+% if ~isfield(Calib,'kappa1')
+%     Calib.kappa1=0;
+% end
+% if ~isfield(Calib,'sx')
+%     Calib.sx=1;
+% end
+% if ~isfield(Calib,'dpx')
+%     Calib.dpx=1;
+% end
+% if ~isfield(Calib,'dpy')
+%     Calib.dpy=1;
+% 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;
+    %camera coordinates
+    xc=R(1)*Xphys+R(2)*Yphys+R(3)*Zphys+Calib.Tx_Ty_Tz(1);
+    yc=R(4)*Xphys+R(5)*Yphys+R(6)*Zphys+Calib.Tx_Ty_Tz(2);
+    zc=R(7)*Xphys+R(8)*Yphys+R(9)*Zphys+Calib.Tx_Ty_Tz(3);
 %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;
+    Xu=xc./zc;
+    Yu=yc./zc;
+%radial quadratic correction factor
+    if ~isfield(Calib,'kc')
+        r2=1; %no quadratic distortion
+    else
+        r2=1+Calib.kc*(Xu.*Xu+Yu.*Yu);
+    end
 %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;
+    if ~isfield(Calib,'Cx_Cy')
+        Calib.Cx_Cy=[0 0];%default value
+    end
+    X=Calib.fx_fy(1)*Xu.*r2+Calib.Cx_Cy(1);
+    Y=Calib.fx_fy(2)*Yu.*r2+Calib.Cx_Cy(2);    
+%OLD CONVENTION (Wilson)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;
+else %case 'rescale'
+    X=Calib.fx_fy(1)*(Xphys+Calib.Tx_Ty_Tz(1));
+    Y=Calib.fx_fy(2)*(Yphys+Calib.Tx_Ty_Tz(2));  
 end