source: trunk/src/phys_XYZ.m @ 1110

Last change on this file since 1110 was 1110, checked in by sommeria, 3 years ago

calibration with plane angl corrected

File size: 5.6 KB
Line 
1%------------------------------------------------------------------------
2%'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters
3% function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Zindex)
4%
5%OUTPUT:
6% Xphys,Yphys,Zphys: vector of phys coordinates corresponding to the input vector of image coordinates
7%INPUT:
8% Calib: Matlab structure containing the calibration parameters (pinhole camera model, see
9% http://servforge.legi.grenoble-inp.fr/projects/soft-uvmat/wiki/UvmatHelp#GeometryCalib) and the
10%    parameters describing the illumination plane(s)
11%    .Tx_Ty_Tz: translation (3 phys coordinates) defining the origine of the camera frame
12%    .R : rotation matrix from phys to camera frame
13%    .fx_fy: focal length along each direction of the image
14% X, Y: vectors of X and Y image coordinates
15% ZIndex: index defining the current illumination plane in a volume scan
16
17%=======================================================================
18% Copyright 2008-2022, LEGI UMR 5519 / CNRS UGA G-INP, Grenoble, France
19%   http://www.legi.grenoble-inp.fr
20%   Joel.Sommeria - Joel.Sommeria (A) legi.cnrs.fr
21%
22%     This file is part of the toolbox UVMAT.
23%
24%     UVMAT is free software; you can redistribute it and/or modify
25%     it under the terms of the GNU General Public License as published
26%     by the Free Software Foundation; either version 2 of the license,
27%     or (at your option) any later version.
28%
29%     UVMAT is distributed in the hope that it will be useful,
30%     but WITHOUT ANY WARRANTY; without even the implied warranty of
31%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
32%     GNU General Public License (see LICENSE.txt) for more details.
33%=======================================================================
34
35function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Zindex)
36%------------------------------------------------------------------------
37testangle=0;% =1 if the illumination plane is tilted with respect to the horizontal plane Xphys Yphys
38test_refraction=0;% =1 if the considered points are viewed through an horizontal interface (located at z=Calib.InterfaceCoord(3)')
39Zphys=0; %default output
40if exist('Zindex','var')&& isequal(Zindex,round(Zindex))&& Zindex>0 && isfield(Calib,'SliceCoord')&&size(Calib.SliceCoord,1)>=Zindex
41    if isfield(Calib, 'SliceAngle') && size(Calib.SliceAngle,1)>=Zindex && ~isequal(Calib.SliceAngle(Zindex,:),[0 0 0])
42        testangle=1;
43        norm_plane=angle2normal(Calib.SliceAngle(Zindex,:));% coordinates UVMAT-httpsof the unit vector normal to the current illumination plane
44    end
45    Z0=Calib.SliceCoord(Zindex,3);%horizontal plane z=cte
46    Z0virt=Z0;
47    if isfield(Calib,'InterfaceCoord') && isfield(Calib,'RefractionIndex')
48        H=Calib.InterfaceCoord(3);% z position of the water surface
49        if H>Z0
50            Z0virt=H-(H-Z0)/Calib.RefractionIndex; %corrected z (virtual object)
51            test_refraction=1;
52        end
53    end
54else
55    Z0=0;
56    Z0virt=0;
57end
58if ~exist('X','var')||~exist('Y','var')
59    Xphys=[];
60    Yphys=[];%default
61    return
62end
63%coordinate transform
64if ~isfield(Calib,'fx_fy')
65    Calib.fx_fy=[1 1];
66end
67if ~isfield(Calib,'Tx_Ty_Tz')
68    Calib.Tx_Ty_Tz=[0 0 1];
69end
70if ~isfield(Calib,'Cx_Cy')
71    Calib.Cx_Cy=[0 0];
72end
73if ~isfield(Calib,'kc')
74    Calib.kc=0;
75end
76if isfield(Calib,'R')
77    R=(Calib.R)';
78    c=Z0virt;
79    cvirt=Z0virt;
80    if testangle
81        % equation of the illumination plane: z=ax+by+c
82        a=-norm_plane(1)/norm_plane(3);
83        b=-norm_plane(2)/norm_plane(3);
84        if test_refraction
85            avirt=a/Calib.RefractionIndex;
86            bvirt=b/Calib.RefractionIndex;
87        else
88            avirt=a;
89            bvirt=b;
90        end
91        cvirt=Z0virt-avirt*Calib.SliceCoord(Zindex,1)-bvirt*Calib.SliceCoord(Zindex,2);% Z0 = (virtual) z coordinate on the rotation axis (assumed horizontal)
92                               % c=z coordinate at (x,y)=(0,0)
93        c=Z0virt-a*Calib.SliceCoord(Zindex,1)-b*Calib.SliceCoord(Zindex,2);
94        R(1)=R(1)+avirt*R(3);
95        R(2)=R(2)+bvirt*R(3);
96        R(4)=R(4)+avirt*R(6);
97        R(5)=R(5)+bvirt*R(6);
98        R(7)=R(7)+avirt*R(9);
99        R(8)=R(8)+bvirt*R(9);         
100    end
101    Tx=Calib.Tx_Ty_Tz(1);
102    Ty=Calib.Tx_Ty_Tz(2);
103    Tz=Calib.Tx_Ty_Tz(3);
104    Dx=R(5)*R(7)-R(4)*R(8);
105    Dy=R(1)*R(8)-R(2)*R(7);
106    D0=(R(2)*R(4)-R(1)*R(5));
107    Z11=R(6)*R(8)-R(5)*R(9);
108    Z12=R(2)*R(9)-R(3)*R(8);
109    Z21=R(4)*R(9)-R(6)*R(7);
110    Z22=R(3)*R(7)-R(1)*R(9);
111     Zx0=R(3)*R(5)-R(2)*R(6);
112     Zy0=R(1)*R(6)-R(3)*R(4);
113    B11=R(8)*Ty-R(5)*Tz+Z11*cvirt;
114    B12=R(2)*Tz-R(8)*Tx+Z12*cvirt;
115    B21=-R(7)*Ty+R(4)*Tz+Z21*cvirt;
116    B22=-R(1)*Tz+R(7)*Tx+Z22*cvirt;
117    X0=R(5)*Tx-R(2)*Ty+Zx0*cvirt;
118    Y0=-R(4)*Tx+R(1)*Ty+Zy0*cvirt;
119    %px to camera:
120    Xd=(X-Calib.Cx_Cy(1))/Calib.fx_fy(1); % sensor coordinates
121    Yd=(Y-Calib.Cx_Cy(2))/Calib.fx_fy(2);
122    dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd);% distortion factor, first approximation Xu,Yu=Xd,Yd
123    test=0;
124    niter=0;
125    while test==0 && niter<10
126        dist_fact_old=dist_fact;     
127        Xu=Xd./dist_fact;%undistorted sensor coordinates, second iteration
128        Yu=Yd./dist_fact;
129        dist_fact=1+Calib.kc*(Xu.*Xu+Yu.*Yu);% distortion factor,next approximation
130        test=max(max(abs(dist_fact-dist_fact_old)))<0.00001; % reducing the relative error to 10^-5 forthe inversion of the quadraticcorrection
131        niter=niter+1;
132    end
133    denom=Dx*Xu+Dy*Yu+D0;
134    Xphys=(B11.*Xu+B12.*Yu+X0)./denom;%world coordinates
135    Yphys=(B21.*Xu+B22.*Yu+Y0)./denom;
136    if testangle
137        Zphys=c+a*Xphys+b*Yphys;
138    else
139        Zphys=Z0;
140    end
141else
142    Xphys=-Calib.Tx_Ty_Tz(1)+X/Calib.fx_fy(1);
143    Yphys=-Calib.Tx_Ty_Tz(2)+Y/Calib.fx_fy(2);
144end
145
Note: See TracBrowser for help on using the repository browser.