source: trunk/src/px_XYZ.m @ 568

Last change on this file since 568 was 542, checked in by sommeria, 12 years ago

bugs repaired for mask display in phys coord, display of circles in geometry_calib, introduction of x,y,z initial in proj_field/proj_plane

File size: 1.8 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)
13if ~exist('Zphys','var')
14    Zphys=0;
15end
16if ~isfield(Calib,'fx_fy')
17     Calib.fx_fy=[1 1];
18end
19if ~isfield(Calib,'Tx_Ty_Tz')
20     Calib.Tx_Ty_Tz=[0 0 1];
21end
22
23%%%%%%%%%%%%%
24% general case
25if isfield(Calib,'R')
26    R=(Calib.R)';
27    %correct z for refraction if needed
28    if isfield(Calib,'InterfaceCoord') && isfield(Calib,'RefractionIndex')
29        H=Calib.InterfaceCoord(3);
30        if H>Zphys
31            Zphys=H-(H-Zphys)/Calib.RefractionIndex; %corrected z (virtual object)
32            test_refraction=1;
33        end
34    end
35   
36    %camera coordinates
37    xc=R(1)*Xphys+R(2)*Yphys+R(3)*Zphys+Calib.Tx_Ty_Tz(1);
38    yc=R(4)*Xphys+R(5)*Yphys+R(6)*Zphys+Calib.Tx_Ty_Tz(2);
39    zc=R(7)*Xphys+R(8)*Yphys+R(9)*Zphys+Calib.Tx_Ty_Tz(3);
40   
41    %undistorted image coordinates
42    Xu=xc./zc;
43    Yu=yc./zc;
44   
45    %radial quadratic correction factor
46    if ~isfield(Calib,'kc')
47        r2=1; %no quadratic distortion
48    else
49        r2=1+Calib.kc*(Xu.*Xu+Yu.*Yu);
50    end
51   
52    %pixel coordinates
53    if ~isfield(Calib,'Cx_Cy')
54        Calib.Cx_Cy=[0 0];%default value
55    end
56    X=Calib.fx_fy(1)*Xu.*r2+Calib.Cx_Cy(1);
57    Y=Calib.fx_fy(2)*Yu.*r2+Calib.Cx_Cy(2); 
58   
59%case 'rescale'   
60else
61    X=Calib.fx_fy(1)*(Xphys+Calib.Tx_Ty_Tz(1));
62    Y=Calib.fx_fy(2)*(Yphys+Calib.Tx_Ty_Tz(2)); 
63end
64
65
66
Note: See TracBrowser for help on using the repository browser.