source: trunk/src/phys_XYZ.m

Last change on this file was 1127, checked in by g7moreau, 5 months ago

Update Joel email

File size: 5.8 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-2024, LEGI UMR 5519 / CNRS UGA G-INP, Grenoble, France
19%   http://www.legi.grenoble-inp.fr
20%   Joel.Sommeria - Joel.Sommeria (A) univ-grenoble-alpes.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,Slice,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 isempty(Slice)
41    Slice=Calib;%old convention < 2022
42end
43if exist('Zindex','var')&& isequal(Zindex,round(Zindex))&& Zindex>0 && isfield(Slice,'SliceCoord')&&size(Slice.SliceCoord,1)>=Zindex
44    if isfield(Slice, 'SliceAngle') && size(Slice.SliceAngle,1)>=Zindex && ~isequal(Slice.SliceAngle(Zindex,:),[0 0 0])
45        testangle=1;
46        norm_plane=angle2normal(Slice.SliceAngle(Zindex,:));% coordinates UVMAT-httpsof the unit vector normal to the current illumination plane
47    end
48    Z0=Slice.SliceCoord(Zindex,3);%horizontal plane z=cte
49    Z0virt=Z0;
50    if isfield(Slice,'InterfaceCoord') && isfield(Slice,'RefractionIndex')
51        H=Slice.InterfaceCoord(3);% z position of the water surface
52        if H>Z0
53            Z0virt=H-(H-Z0)/Slice.RefractionIndex; %corrected z (virtual object)
54            test_refraction=1;
55        end
56    end
57else
58    Z0=0;
59    Z0virt=0;
60end
61if ~exist('X','var')||~exist('Y','var')
62    Xphys=[];
63    Yphys=[];%default
64    return
65end
66%coordinate transform
67if ~isfield(Calib,'fx_fy')
68    Calib.fx_fy=[1 1];
69end
70if ~isfield(Calib,'Tx_Ty_Tz')
71    Calib.Tx_Ty_Tz=[0 0 1];
72end
73if ~isfield(Calib,'Cx_Cy')
74    Calib.Cx_Cy=[0 0];
75end
76kc=[0 0];
77if isfield(Calib,'kc')
78    kc(1:numel(Calib.kc))=Calib.kc;
79end
80% if ~isfield(Calib,'kc2')
81%     Calib.kc2=0;
82% end
83if isfield(Calib,'R')
84    R=(Calib.R)';
85    c=Z0virt;
86    cvirt=Z0virt;
87    if testangle
88        % equation of the illumination plane: z=ax+by+c
89        a=-norm_plane(1)/norm_plane(3);
90        b=-norm_plane(2)/norm_plane(3);
91        if test_refraction
92            avirt=a/Slice.RefractionIndex;
93            bvirt=b/Slice.RefractionIndex;
94        else
95            avirt=a;
96            bvirt=b;
97        end
98        cvirt=Z0virt-avirt*Slice.SliceCoord(Zindex,1)-bvirt*Slice.SliceCoord(Zindex,2);% Z0 = (virtual) z coordinate on the rotation axis (assumed horizontal)
99                               % c=z coordinate at (x,y)=(0,0)
100        c=Z0-a*Slice.SliceCoord(Zindex,1)-b*Slice.SliceCoord(Zindex,2);
101        R(1)=R(1)+avirt*R(3);
102        R(2)=R(2)+bvirt*R(3);
103        R(4)=R(4)+avirt*R(6);
104        R(5)=R(5)+bvirt*R(6);
105        R(7)=R(7)+avirt*R(9);
106        R(8)=R(8)+bvirt*R(9);         
107    end
108    Tx=Calib.Tx_Ty_Tz(1);
109    Ty=Calib.Tx_Ty_Tz(2);
110    Tz=Calib.Tx_Ty_Tz(3);
111    Dx=R(5)*R(7)-R(4)*R(8);
112    Dy=R(1)*R(8)-R(2)*R(7);
113    D0=(R(2)*R(4)-R(1)*R(5));
114    Z11=R(6)*R(8)-R(5)*R(9);
115    Z12=R(2)*R(9)-R(3)*R(8);
116    Z21=R(4)*R(9)-R(6)*R(7);
117    Z22=R(3)*R(7)-R(1)*R(9);
118     Zx0=R(3)*R(5)-R(2)*R(6);
119     Zy0=R(1)*R(6)-R(3)*R(4);
120    B11=R(8)*Ty-R(5)*Tz+Z11*cvirt;
121    B12=R(2)*Tz-R(8)*Tx+Z12*cvirt;
122    B21=-R(7)*Ty+R(4)*Tz+Z21*cvirt;
123    B22=-R(1)*Tz+R(7)*Tx+Z22*cvirt;
124    X0=R(5)*Tx-R(2)*Ty+Zx0*cvirt;
125    Y0=-R(4)*Tx+R(1)*Ty+Zy0*cvirt;
126    %px to camera:
127    Xd=(X-Calib.Cx_Cy(1))/Calib.fx_fy(1); % sensor coordinates
128    Yd=(Y-Calib.Cx_Cy(2))/Calib.fx_fy(2);
129    dist_fact=1+kc(1)*(Xd.*Xd+Yd.*Yd);% distortion factor, first approximation Xu,Yu=Xd,Yd
130    test=0;
131    niter=0;
132    while test==0 && niter<10
133        dist_fact_old=dist_fact;     
134        Xu=Xd./dist_fact;%undistorted sensor coordinates, second iteration
135        Yu=Yd./dist_fact;
136        R2=Xu.*Xu+Yu.*Yu;
137        dist_fact=1+kc(1)*R2+kc(2)*R2.*R2;% distortion factor,next approximation
138        test=max(max(abs(dist_fact-dist_fact_old)))<0.00001; % reducing the relative error to 10^-5 forthe inversion of the quadraticcorrection
139        niter=niter+1;
140    end
141    denom=Dx*Xu+Dy*Yu+D0;
142    Xphys=(B11.*Xu+B12.*Yu+X0)./denom;%world coordinates
143    Yphys=(B21.*Xu+B22.*Yu+Y0)./denom;
144    if testangle
145        Zphys=c+a*Xphys+b*Yphys;
146    else
147        Zphys=Z0;
148    end
149else
150    Xphys=-Calib.Tx_Ty_Tz(1)+X/Calib.fx_fy(1);
151    Yphys=-Calib.Tx_Ty_Tz(2)+Y/Calib.fx_fy(2);
152end
153
Note: See TracBrowser for help on using the repository browser.