source: trunk/src/phys_XYZ.m @ 965

Last change on this file since 965 was 924, checked in by g7moreau, 8 years ago
  • Update Copyright Copyright 2008-2016, LEGI UMR 5519 / CNRS UGA G-INP, Grenoble, France
File size: 5.0 KB
Line 
1%------------------------------------------------------------------------
2%'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters
3% function [Xphys,Yphys]=phys_XYZ(Calib,X,Y,Z)
4%
5%OUTPUT:
6%
7%INPUT:
8%Z: index of plane
9
10%=======================================================================
11% Copyright 2008-2016, 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
25%     GNU General Public License (see LICENSE.txt) for more details.
26%=======================================================================
27
28function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Zindex)
29%------------------------------------------------------------------------
30testangle=0;
31test_refraction=0;
32if exist('Zindex','var')&& isequal(Zindex,round(Zindex))&& Zindex>0 && isfield(Calib,'SliceCoord')&&length(Calib.SliceCoord)>=Zindex
33    if isfield(Calib, 'SliceAngle') && ~isequal(Calib.SliceAngle,[0 0 0]) && ~isequal(Calib.SliceAngle(Zindex,:),[0 0 0])
34        testangle=1;
35        om=norm(Calib.SliceAngle(Zindex,:));%norm of rotation angle in radians
36        OmAxis=Calib.SliceAngle(Zindex,:)/om; %unit vector marking the rotation axis
37        cos_om=cos(pi*om/180);
38        sin_om=sin(pi*om/180);
39        coeff=OmAxis(3)*(1-cos_om);
40        norm_plane(1)=OmAxis(1)*coeff+OmAxis(2)*sin_om;
41        norm_plane(2)=OmAxis(2)*coeff-OmAxis(1)*sin_om;
42        norm_plane(3)=OmAxis(3)*coeff+cos_om;
43        Z0=norm_plane*Calib.SliceCoord(Zindex,:)'/norm_plane(3);
44    else
45        Z0=Calib.SliceCoord(Zindex,3);%horizontal plane z=cte
46    end
47    Z0virt=Z0;
48    if isfield(Calib,'InterfaceCoord') && isfield(Calib,'RefractionIndex')
49        H=Calib.InterfaceCoord(3);
50        if H>Z0
51            Z0virt=H-(H-Z0)/Calib.RefractionIndex; %corrected z (virtual object)
52            test_refraction=1;
53        end
54    end
55else
56    Z0=0;
57    Z0virt=0;
58end
59if ~exist('X','var')||~exist('Y','var')
60    Xphys=[];
61    Yphys=[];%default
62    return
63end
64%coordinate transform
65if ~isfield(Calib,'fx_fy')
66    Calib.fx_fy=[1 1];
67end
68if ~isfield(Calib,'Tx_Ty_Tz')
69    Calib.Tx_Ty_Tz=[0 0 1];
70end
71if ~isfield(Calib,'Cx_Cy')
72    Calib.Cx_Cy=[0 0];
73end
74if ~isfield(Calib,'kc')
75    Calib.kc=0;
76end
77if isfield(Calib,'R')
78    R=(Calib.R)';
79    if testangle
80        a=-norm_plane(1)/norm_plane(3);
81        b=-norm_plane(2)/norm_plane(3);
82        if test_refraction
83            a=a/Calib.RefractionIndex;
84            b=b/Calib.RefractionIndex;
85        end
86        R(1)=R(1)+a*R(3);
87        R(2)=R(2)+b*R(3);
88        R(4)=R(4)+a*R(6);
89        R(5)=R(5)+b*R(6);
90        R(7)=R(7)+a*R(9);
91        R(8)=R(8)+b*R(9);
92    end
93    Tx=Calib.Tx_Ty_Tz(1);
94    Ty=Calib.Tx_Ty_Tz(2);
95    Tz=Calib.Tx_Ty_Tz(3);
96    f=Calib.fx_fy(1);%dpy=1; sx=1
97    %dpx=Calib.fx_fy(2)/Calib.fx_fy(1);
98    Dx=R(5)*R(7)-R(4)*R(8);
99    Dy=R(1)*R(8)-R(2)*R(7);
100    D0=(R(2)*R(4)-R(1)*R(5));
101    Z11=R(6)*R(8)-R(5)*R(9);
102    Z12=R(2)*R(9)-R(3)*R(8);
103    Z21=R(4)*R(9)-R(6)*R(7);
104    Z22=R(3)*R(7)-R(1)*R(9);
105    Zx0=R(3)*R(5)-R(2)*R(6);
106    Zy0=R(1)*R(6)-R(3)*R(4);
107    A11=R(8)*Ty-R(5)*Tz+Z11*Z0virt;
108    A12=R(2)*Tz-R(8)*Tx+Z12*Z0virt;
109    A21=-R(7)*Ty+R(4)*Tz+Z21*Z0virt;
110    A22=-R(1)*Tz+R(7)*Tx+Z22*Z0virt;
111    %     X0=Calib.fx_fy(1)*(R(5)*Tx-R(2)*Ty+Zx0*Z0virt);
112    %     Y0=Calib.fx_fy(2)*(-R(4)*Tx+R(1)*Ty+Zy0*Z0virt);
113    X0=(R(5)*Tx-R(2)*Ty+Zx0*Z0virt);
114    Y0=(-R(4)*Tx+R(1)*Ty+Zy0*Z0virt);
115    %px to camera:
116    %     Xd=dpx*(X-Calib.Cx_Cy(1)); % sensor coordinates
117    %     Yd=(Y-Calib.Cx_Cy(2));
118    Xd=(X-Calib.Cx_Cy(1))/Calib.fx_fy(1); % sensor coordinates
119    Yd=(Y-Calib.Cx_Cy(2))/Calib.fx_fy(2);
120    dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd);%/(f*f); %distortion factor
121    Xu=Xd./dist_fact;%undistorted sensor coordinates
122    Yu=Yd./dist_fact;
123    denom=Dx*Xu+Dy*Yu+D0;
124    Xphys=(A11.*Xu+A12.*Yu+X0)./denom;%world coordinates
125    Yphys=(A21.*Xu+A22.*Yu+Y0)./denom;
126    if testangle
127        Zphys=Z0+a*Xphys+b*Yphys;
128    else
129        Zphys=Z0;
130    end
131else
132    Xphys=-Calib.Tx_Ty_Tz(1)+X/Calib.fx_fy(1);
133    Yphys=-Calib.Tx_Ty_Tz(2)+Y/Calib.fx_fy(2);
134end
135
136%'px_XYZ': transform phys coordinates to image coordinates (px)
137%
138% OUTPUT:
139% X,Y: array of coordinates in the image cooresponding to the input physical positions
140%                    (origin at lower leftcorner, unit=pixel)
141
142% INPUT:
143% Calib: structure containing the calibration parameters (read from the ImaDoc .xml file)
144% Xphys, Yphys: array of x,y physical coordinates
145% [Z0]: corresponding array of z physical coordinates (0 by default)
Note: See TracBrowser for help on using the repository browser.