# source:trunk/src/phys_XYZ.m@1080

Last change on this file since 1080 was 1080, checked in by sommeria, 4 years ago

ima2temperature.m added and various bug repairs

File size: 4.2 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%
7%INPUT:
8%Z: index of plane
9
10%=======================================================================
11% Copyright 2008-2020, LEGI UMR 5519 / CNRS UGA G-INP, Grenoble, France
12%   http://www.legi.grenoble-inp.fr
13%   Joel.Sommeria - Joel.Sommeria (A) legi.cnrs.fr
14%
15%     This file is part of the toolbox UVMAT.
16%
17%     UVMAT is free software; you can redistribute it and/or modify
18%     it under the terms of the GNU General Public License as published
19%     by the Free Software Foundation; either version 2 of the license,
20%     or (at your option) any later version.
21%
22%     UVMAT is distributed in the hope that it will be useful,
23%     but WITHOUT ANY WARRANTY; without even the implied warranty of
24%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
26%=======================================================================
27
28function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Zindex)
29%------------------------------------------------------------------------
30testangle=0;
31test_refraction=0;
32Zphys=0; %default output
33if exist('Zindex','var')&& isequal(Zindex,round(Zindex))&& Zindex>0 && isfield(Calib,'SliceCoord')&&size(Calib.SliceCoord,1)>=Zindex
34    if isfield(Calib, 'SliceAngle') && size(Calib.SliceAngle,1)>=Zindex && ~isequal(Calib.SliceAngle(Zindex,:),[0 0 0])
35        testangle=1;
36        norm_plane=angle2normal(Calib.SliceAngle(Zindex,:));
37    end
38    Z0=Calib.SliceCoord(Zindex,3);%horizontal plane z=cte
39    Z0virt=Z0;
40    if isfield(Calib,'InterfaceCoord') && isfield(Calib,'RefractionIndex')
41        H=Calib.InterfaceCoord(3);
42        if H>Z0
43            Z0virt=H-(H-Z0)/Calib.RefractionIndex; %corrected z (virtual object)
44            test_refraction=1;
45        end
46    end
47else
48    Z0=0;
49    Z0virt=0;
50end
51if ~exist('X','var')||~exist('Y','var')
52    Xphys=[];
53    Yphys=[];%default
54    return
55end
56%coordinate transform
57if ~isfield(Calib,'fx_fy')
58    Calib.fx_fy=[1 1];
59end
60if ~isfield(Calib,'Tx_Ty_Tz')
61    Calib.Tx_Ty_Tz=[0 0 1];
62end
63if ~isfield(Calib,'Cx_Cy')
64    Calib.Cx_Cy=[0 0];
65end
66if ~isfield(Calib,'kc')
67    Calib.kc=0;
68end
69if isfield(Calib,'R')
70    R=(Calib.R)';
71    if testangle
72        a=-norm_plane(1)/norm_plane(3);
73        b=-norm_plane(2)/norm_plane(3);
74        if test_refraction
75            a=a/Calib.RefractionIndex;
76            b=b/Calib.RefractionIndex;
77        end
78        R(1)=R(1)+a*R(3);
79        R(2)=R(2)+b*R(3);
80        R(4)=R(4)+a*R(6);
81        R(5)=R(5)+b*R(6);
82        R(7)=R(7)+a*R(9);
83        R(8)=R(8)+b*R(9);
84    end
85    Tx=Calib.Tx_Ty_Tz(1);
86    Ty=Calib.Tx_Ty_Tz(2);
87    Tz=Calib.Tx_Ty_Tz(3);
88    Dx=R(5)*R(7)-R(4)*R(8);
89    Dy=R(1)*R(8)-R(2)*R(7);
90    D0=(R(2)*R(4)-R(1)*R(5));
91    Z11=R(6)*R(8)-R(5)*R(9);
92    Z12=R(2)*R(9)-R(3)*R(8);
93    Z21=R(4)*R(9)-R(6)*R(7);
94    Z22=R(3)*R(7)-R(1)*R(9);
95    Zx0=R(3)*R(5)-R(2)*R(6);
96    Zy0=R(1)*R(6)-R(3)*R(4);
97    A11=R(8)*Ty-R(5)*Tz+Z11*Z0virt;
98    A12=R(2)*Tz-R(8)*Tx+Z12*Z0virt;
99    A21=-R(7)*Ty+R(4)*Tz+Z21*Z0virt;
100    A22=-R(1)*Tz+R(7)*Tx+Z22*Z0virt;
101    X0=(R(5)*Tx-R(2)*Ty+Zx0*Z0virt);
102    Y0=(-R(4)*Tx+R(1)*Ty+Zy0*Z0virt);
103    %px to camera:
104    Xd=(X-Calib.Cx_Cy(1))/Calib.fx_fy(1); % sensor coordinates
105    Yd=(Y-Calib.Cx_Cy(2))/Calib.fx_fy(2);
106    dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd);% distortion factor, first approximation Xu,Yu=Xd,Yd
107    test=0;
108    niter=0;
109    while test==0 && niter<10
110        dist_fact_old=dist_fact;
111        Xu=Xd./dist_fact;%undistorted sensor coordinates, second iteration
112        Yu=Yd./dist_fact;
113        dist_fact=1+Calib.kc*(Xu.*Xu+Yu.*Yu);% distortion factor,next approximation
114        test=max(max(abs(dist_fact-dist_fact_old)))<0.00001; % reducing the relative error to 10^-5 forthe inversion of the quadraticcorrection
115        niter=niter+1;
116    end
117    denom=Dx*Xu+Dy*Yu+D0;
118    Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates
119    Yphys=(A21.*Xu+A22.*Yu+Y0)./denom;
120    if testangle
121        Zphys=Z0+a*Xphys+b*Yphys;
122    else
123        Zphys=Z0;
124    end
125else
126    Xphys=-Calib.Tx_Ty_Tz(1)+X/Calib.fx_fy(1);
127    Yphys=-Calib.Tx_Ty_Tz(2)+Y/Calib.fx_fy(2);
128end
129
Note: See TracBrowser for help on using the repository browser.