source: trunk/src/px_XYZ.m @ 378

Last change on this file since 378 was 114, checked in by sommeria, 14 years ago

geometric calibration procedures updated

File size: 2.3 KB
Line 
1%'px_XYZ': transform physical to image coordinates.
2%------------------------------------------------------------------------
3%[X,Y]=px_XYZ(Calib,Xphys,Yphys,Zphys)
4%------------------------------------------------------------------------           
5% OUTPUT:
6% [X,Y]: image coordinates(in pixels)
7%------------------------------------------------------------------------
8% INPUT:
9% Calib: structure containing calibration parameters
10% Xphys,Yphys,Zphys; vectors of physical coordinates for a set of points
11
12function [X,Y]=px_XYZ(Calib,Xphys,Yphys,Zphys)
13% if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'PlanePos')&length(Calib.PlanePos)>=Z
14%     Zindex=Z;
15%     planepos=Calib.PlanePos{Zindex};
16%     zphys=planepos(3);%A GENERALISER CAS AVEC ANGLE
17% else
18%     zphys=0;
19% end
20if ~exist('Zphys','var')
21    Zphys=0;
22end
23if ~isfield(Calib,'fx_fy')
24     Calib.fx_fy=[1 1];
25end
26if ~isfield(Calib,'Tx_Ty_Tz')
27     Calib.Tx_Ty_Tz=[0 0 1];
28end
29% if ~isfield(Calib,'kappa1')
30%     Calib.kappa1=0;
31% end
32% if ~isfield(Calib,'sx')
33%     Calib.sx=1;
34% end
35% if ~isfield(Calib,'dpx')
36%     Calib.dpx=1;
37% end
38% if ~isfield(Calib,'dpy')
39%     Calib.dpy=1;
40% end
41
42%%%%%%%%%%%%%
43if isfield(Calib,'R')
44    R=(Calib.R)';
45    %camera coordinates
46    xc=R(1)*Xphys+R(2)*Yphys+R(3)*Zphys+Calib.Tx_Ty_Tz(1);
47    yc=R(4)*Xphys+R(5)*Yphys+R(6)*Zphys+Calib.Tx_Ty_Tz(2);
48    zc=R(7)*Xphys+R(8)*Yphys+R(9)*Zphys+Calib.Tx_Ty_Tz(3);
49%undistorted image coordinates
50    Xu=xc./zc;
51    Yu=yc./zc;
52%radial quadratic correction factor
53    if ~isfield(Calib,'kc')
54        r2=1; %no quadratic distortion
55    else
56        r2=1+Calib.kc*(Xu.*Xu+Yu.*Yu);
57    end
58%pixel coordinates
59    if ~isfield(Calib,'Cx_Cy')
60        Calib.Cx_Cy=[0 0];%default value
61    end
62    X=Calib.fx_fy(1)*Xu.*r2+Calib.Cx_Cy(1);
63    Y=Calib.fx_fy(2)*Yu.*r2+Calib.Cx_Cy(2);   
64%OLD CONVENTION (Wilson)undistorted image coordinates
65%     Xu=Calib.f*xc./zc;
66%     Yu=Calib.f*yc./zc;   
67% %distorted image coordinates
68%     distortion=(Calib.kappa1)*(Xu.*Xu+Yu.*Yu)+1; %A REVOIR
69% % distortion=1;
70%     Xd=Xu./distortion;
71%     Yd=Yu./distortion;
72% %pixel coordinates
73%     X=Xd*Calib.sx/Calib.dpx+Calib.Cx;
74%     Y=Yd/Calib.dpy+Calib.Cy;
75else %case 'rescale'
76    X=Calib.fx_fy(1)*(Xphys+Calib.Tx_Ty_Tz(1));
77    Y=Calib.fx_fy(2)*(Yphys+Calib.Tx_Ty_Tz(2)); 
78end
79
80
81
Note: See TracBrowser for help on using the repository browser.