[2] | 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
|
---|
| 8 | function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Z)
|
---|
| 9 | if 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
|
---|
| 12 | else
|
---|
| 13 | % if exist('Z','var')
|
---|
| 14 | % Zphys=Z;
|
---|
| 15 | % else
|
---|
| 16 | Zphys=0;
|
---|
| 17 | % end
|
---|
| 18 | end
|
---|
| 19 | if ~exist('X','var')||~exist('Y','var')
|
---|
| 20 | Xphys=[];
|
---|
| 21 | Yphys=[];%default
|
---|
| 22 | return
|
---|
| 23 | end
|
---|
| 24 | Xphys=X;%default
|
---|
| 25 | Yphys=Y;
|
---|
| 26 | %image transform
|
---|
| 27 | if 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;
|
---|
| 54 | end
|
---|