[109] | 1 | %'px_XYZ': transform physical to image coordinates. |
---|
| 2 | %------------------------------------------------------------------------ |
---|
| 3 | %[X,Y]=px_XYZ(Calib,Xphys,Yphys,Zphys) |
---|
| 4 | %------------------------------------------------------------------------ |
---|
| 5 | % OUTPUT: |
---|
| 6 | % [X,Y]: image coordinates(in pixels) |
---|
| 7 | %------------------------------------------------------------------------ |
---|
| 8 | % INPUT: |
---|
| 9 | % Calib: structure containing calibration parameters |
---|
| 10 | % Xphys,Yphys,Zphys; vectors of physical coordinates for a set of points |
---|
[40] | 11 | |
---|
[809] | 12 | %======================================================================= |
---|
[1027] | 13 | % Copyright 2008-2018, LEGI UMR 5519 / CNRS UGA G-INP, Grenoble, France |
---|
[809] | 14 | % http://www.legi.grenoble-inp.fr |
---|
| 15 | % Joel.Sommeria - Joel.Sommeria (A) legi.cnrs.fr |
---|
| 16 | % |
---|
| 17 | % This file is part of the toolbox UVMAT. |
---|
| 18 | % |
---|
| 19 | % UVMAT is free software; you can redistribute it and/or modify |
---|
| 20 | % it under the terms of the GNU General Public License as published |
---|
| 21 | % by the Free Software Foundation; either version 2 of the license, |
---|
| 22 | % or (at your option) any later version. |
---|
| 23 | % |
---|
| 24 | % UVMAT is distributed in the hope that it will be useful, |
---|
| 25 | % but WITHOUT ANY WARRANTY; without even the implied warranty of |
---|
| 26 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
---|
| 27 | % GNU General Public License (see LICENSE.txt) for more details. |
---|
| 28 | %======================================================================= |
---|
| 29 | |
---|
[40] | 30 | function [X,Y]=px_XYZ(Calib,Xphys,Yphys,Zphys) |
---|
| 31 | if ~exist('Zphys','var') |
---|
| 32 | Zphys=0; |
---|
| 33 | end |
---|
[114] | 34 | if ~isfield(Calib,'fx_fy') |
---|
| 35 | Calib.fx_fy=[1 1]; |
---|
[84] | 36 | end |
---|
[114] | 37 | if ~isfield(Calib,'Tx_Ty_Tz') |
---|
| 38 | Calib.Tx_Ty_Tz=[0 0 1]; |
---|
[84] | 39 | end |
---|
[114] | 40 | |
---|
[40] | 41 | %%%%%%%%%%%%% |
---|
[542] | 42 | % general case |
---|
[40] | 43 | if isfield(Calib,'R') |
---|
| 44 | R=(Calib.R)'; |
---|
[542] | 45 | %correct z for refraction if needed |
---|
| 46 | if isfield(Calib,'InterfaceCoord') && isfield(Calib,'RefractionIndex') |
---|
| 47 | H=Calib.InterfaceCoord(3); |
---|
| 48 | if H>Zphys |
---|
| 49 | Zphys=H-(H-Zphys)/Calib.RefractionIndex; %corrected z (virtual object) |
---|
[892] | 50 | % test_refraction=1; |
---|
[542] | 51 | end |
---|
| 52 | end |
---|
| 53 | |
---|
[114] | 54 | %camera coordinates |
---|
| 55 | xc=R(1)*Xphys+R(2)*Yphys+R(3)*Zphys+Calib.Tx_Ty_Tz(1); |
---|
| 56 | yc=R(4)*Xphys+R(5)*Yphys+R(6)*Zphys+Calib.Tx_Ty_Tz(2); |
---|
| 57 | zc=R(7)*Xphys+R(8)*Yphys+R(9)*Zphys+Calib.Tx_Ty_Tz(3); |
---|
[542] | 58 | |
---|
| 59 | %undistorted image coordinates |
---|
[114] | 60 | Xu=xc./zc; |
---|
| 61 | Yu=yc./zc; |
---|
[542] | 62 | |
---|
| 63 | %radial quadratic correction factor |
---|
[114] | 64 | if ~isfield(Calib,'kc') |
---|
| 65 | r2=1; %no quadratic distortion |
---|
| 66 | else |
---|
| 67 | r2=1+Calib.kc*(Xu.*Xu+Yu.*Yu); |
---|
| 68 | end |
---|
[542] | 69 | |
---|
| 70 | %pixel coordinates |
---|
[114] | 71 | if ~isfield(Calib,'Cx_Cy') |
---|
| 72 | Calib.Cx_Cy=[0 0];%default value |
---|
| 73 | end |
---|
| 74 | X=Calib.fx_fy(1)*Xu.*r2+Calib.Cx_Cy(1); |
---|
[542] | 75 | Y=Calib.fx_fy(2)*Yu.*r2+Calib.Cx_Cy(2); |
---|
| 76 | |
---|
| 77 | %case 'rescale' |
---|
| 78 | else |
---|
[114] | 79 | X=Calib.fx_fy(1)*(Xphys+Calib.Tx_Ty_Tz(1)); |
---|
| 80 | Y=Calib.fx_fy(2)*(Yphys+Calib.Tx_Ty_Tz(2)); |
---|
[92] | 81 | end |
---|
| 82 | |
---|
| 83 | |
---|
| 84 | |
---|