source: trunk/src/private/phys_XYZ.m @ 8

Last change on this file since 8 was 8, checked in by gostiaux, 11 years ago
File size: 1.7 KB
Line 
1%'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters
2% function [Xphys,Yphys]=phys_XYZ(Calib,X,Y,Z)
3%
4%OUTPUT:
5%
6%INPUT:
7%Z: index of plane
8function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Z)
9if exist('Z','var')& isequal(Z,round(Z))& Z>0 & isfield(Calib,'SliceCoord')&length(Calib.SliceCoord)>=Z
10    Zindex=Z;
11    Zphys=Calib.SliceCoord(Zindex,3);%GENERALISER AUX CAS AVEC ANGLE
12else
13%     if exist('Z','var')
14%         Zphys=Z;
15%     else
16        Zphys=0;
17%     end
18end
19if ~exist('X','var')||~exist('Y','var')
20    Xphys=[];
21    Yphys=[];%default
22    return
23end
24Xphys=X;%default
25Yphys=Y;
26%image transform
27if isfield(Calib,'R')
28    R=(Calib.R)';
29    Dx=R(5)*R(7)-R(4)*R(8);
30    Dy=R(1)*R(8)-R(2)*R(7);
31    D0=Calib.f*(R(2)*R(4)-R(1)*R(5));
32    Z11=R(6)*R(8)-R(5)*R(9);
33    Z12=R(2)*R(9)-R(3)*R(8); 
34    Z21=R(4)*R(9)-R(6)*R(7);
35    Z22=R(3)*R(7)-R(1)*R(9);
36    Zx0=R(3)*R(5)-R(2)*R(6);
37    Zy0=R(1)*R(6)-R(3)*R(4);
38    A11=R(8)*Calib.Ty-R(5)*Calib.Tz+Z11*Zphys;
39    A12=R(2)*Calib.Tz-R(8)*Calib.Tx+Z12*Zphys;
40    A21=-R(7)*Calib.Ty+R(4)*Calib.Tz+Z21*Zphys;
41    A22=-R(1)*Calib.Tz+R(7)*Calib.Tx+Z11*Zphys;
42    X0=Calib.f*(R(5)*Calib.Tx-R(2)*Calib.Ty+Zx0*Zphys);
43    Y0=Calib.f*(-R(4)*Calib.Tx+R(1)*Calib.Ty+Zy0*Zphys);
44        %px to camera:
45    Xd=(Calib.dpx/Calib.sx)*(X-Calib.Cx); % sensor coordinates
46    Yd=Calib.dpy*(Y-Calib.Cy);
47    dist_fact=1+Calib.kappa1*(Xd.*Xd+Yd.*Yd); %distortion factor
48    Xu=dist_fact.*Xd;%undistorted sensor coordinates
49    Yu=dist_fact.*Yd;
50    denom=Dx*Xu+Dy*Yu+D0;
51    % denom2=denom.*denom;
52    Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates
53    Yphys=(A21.*Xu+A22.*Yu+Y0)./denom;
54end
Note: See TracBrowser for help on using the repository browser.