[242] | 1 | %------------------------------------------------------------------------ |
---|
| 2 | %'phys_XYZ':transforms image (px) to real world (phys) coordinates using geometric calibration parameters |
---|
[1078] | 3 | % function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Zindex) |
---|
[242] | 4 | % |
---|
| 5 | %OUTPUT: |
---|
[1108] | 6 | % Xphys,Yphys,Zphys: vector of phys coordinates corresponding to the input vector of image coordinates |
---|
[242] | 7 | %INPUT: |
---|
[1108] | 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 |
---|
[809] | 16 | |
---|
| 17 | %======================================================================= |
---|
[1107] | 18 | % Copyright 2008-2022, LEGI UMR 5519 / CNRS UGA G-INP, Grenoble, France |
---|
[809] | 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 | |
---|
[242] | 35 | function [Xphys,Yphys,Zphys]=phys_XYZ(Calib,X,Y,Zindex) |
---|
| 36 | %------------------------------------------------------------------------ |
---|
[1108] | 37 | testangle=0;% =1 if the illumination plane is tilted with respect to the horizontal plane Xphys Yphys |
---|
| 38 | test_refraction=0;% =1 if the considered points are viewed through an horizontal interface (located at z=Calib.InterfaceCoord(3)') |
---|
[1078] | 39 | Zphys=0; %default output |
---|
[1059] | 40 | if exist('Zindex','var')&& isequal(Zindex,round(Zindex))&& Zindex>0 && isfield(Calib,'SliceCoord')&&size(Calib.SliceCoord,1)>=Zindex |
---|
[1078] | 41 | if isfield(Calib, 'SliceAngle') && size(Calib.SliceAngle,1)>=Zindex && ~isequal(Calib.SliceAngle(Zindex,:),[0 0 0]) |
---|
[242] | 42 | testangle=1; |
---|
[1108] | 43 | norm_plane=angle2normal(Calib.SliceAngle(Zindex,:));% coordinates of the unit vector normal to the current illumination plane |
---|
[972] | 44 | end |
---|
[1059] | 45 | Z0=Calib.SliceCoord(Zindex,3);%horizontal plane z=cte |
---|
[242] | 46 | Z0virt=Z0; |
---|
| 47 | if isfield(Calib,'InterfaceCoord') && isfield(Calib,'RefractionIndex') |
---|
[1108] | 48 | H=Calib.InterfaceCoord(3);% z position of the water surface |
---|
[242] | 49 | if H>Z0 |
---|
| 50 | Z0virt=H-(H-Z0)/Calib.RefractionIndex; %corrected z (virtual object) |
---|
| 51 | test_refraction=1; |
---|
| 52 | end |
---|
| 53 | end |
---|
| 54 | else |
---|
| 55 | Z0=0; |
---|
| 56 | Z0virt=0; |
---|
| 57 | end |
---|
| 58 | if ~exist('X','var')||~exist('Y','var') |
---|
| 59 | Xphys=[]; |
---|
| 60 | Yphys=[];%default |
---|
| 61 | return |
---|
| 62 | end |
---|
| 63 | %coordinate transform |
---|
| 64 | if ~isfield(Calib,'fx_fy') |
---|
| 65 | Calib.fx_fy=[1 1]; |
---|
| 66 | end |
---|
| 67 | if ~isfield(Calib,'Tx_Ty_Tz') |
---|
| 68 | Calib.Tx_Ty_Tz=[0 0 1]; |
---|
| 69 | end |
---|
| 70 | if ~isfield(Calib,'Cx_Cy') |
---|
| 71 | Calib.Cx_Cy=[0 0]; |
---|
| 72 | end |
---|
| 73 | if ~isfield(Calib,'kc') |
---|
| 74 | Calib.kc=0; |
---|
| 75 | end |
---|
| 76 | if isfield(Calib,'R') |
---|
| 77 | R=(Calib.R)'; |
---|
[1108] | 78 | c=Z0virt; |
---|
[242] | 79 | if testangle |
---|
[1108] | 80 | % equation of the illumination plane: z=ax+by+c |
---|
[242] | 81 | a=-norm_plane(1)/norm_plane(3); |
---|
| 82 | b=-norm_plane(2)/norm_plane(3); |
---|
| 83 | if test_refraction |
---|
[1108] | 84 | avirt=a/Calib.RefractionIndex; |
---|
| 85 | bvirt=b/Calib.RefractionIndex; |
---|
[242] | 86 | end |
---|
[1108] | 87 | c=Z0virt-avirt*Calib.SliceCoord(Zindex,1)-bvirt*Calib.SliceCoord(Zindex,2);% Z0 = (virtual) z coordinate on the rotation axis (assumed horizontal) |
---|
| 88 | % c=z coordinate at (x,y)=(0,0) |
---|
| 89 | R(1)=R(1)+avirt*R(3); |
---|
| 90 | R(2)=R(2)+bvirt*R(3); |
---|
| 91 | R(4)=R(4)+avirt*R(6); |
---|
| 92 | R(5)=R(5)+bvirt*R(6); |
---|
| 93 | R(7)=R(7)+avirt*R(9); |
---|
| 94 | R(8)=R(8)+bvirt*R(9); |
---|
[242] | 95 | end |
---|
| 96 | Tx=Calib.Tx_Ty_Tz(1); |
---|
| 97 | Ty=Calib.Tx_Ty_Tz(2); |
---|
| 98 | Tz=Calib.Tx_Ty_Tz(3); |
---|
| 99 | Dx=R(5)*R(7)-R(4)*R(8); |
---|
| 100 | Dy=R(1)*R(8)-R(2)*R(7); |
---|
| 101 | D0=(R(2)*R(4)-R(1)*R(5)); |
---|
| 102 | Z11=R(6)*R(8)-R(5)*R(9); |
---|
| 103 | Z12=R(2)*R(9)-R(3)*R(8); |
---|
| 104 | Z21=R(4)*R(9)-R(6)*R(7); |
---|
| 105 | Z22=R(3)*R(7)-R(1)*R(9); |
---|
[1108] | 106 | Zx0=R(3)*R(5)-R(2)*R(6); |
---|
| 107 | Zy0=R(1)*R(6)-R(3)*R(4); |
---|
| 108 | B11=R(8)*Ty-R(5)*Tz+Z11*c; |
---|
| 109 | B12=R(2)*Tz-R(8)*Tx+Z12*c; |
---|
| 110 | B21=-R(7)*Ty+R(4)*Tz+Z21*c; |
---|
| 111 | B22=-R(1)*Tz+R(7)*Tx+Z22*c; |
---|
| 112 | X0=(R(5)*Tx-R(2)*Ty+Zx0*c); |
---|
| 113 | Y0=(-R(4)*Tx+R(1)*Ty+Zy0*c); |
---|
[242] | 114 | %px to camera: |
---|
| 115 | Xd=(X-Calib.Cx_Cy(1))/Calib.fx_fy(1); % sensor coordinates |
---|
| 116 | Yd=(Y-Calib.Cx_Cy(2))/Calib.fx_fy(2); |
---|
[1080] | 117 | dist_fact=1+Calib.kc*(Xd.*Xd+Yd.*Yd);% distortion factor, first approximation Xu,Yu=Xd,Yd |
---|
| 118 | test=0; |
---|
| 119 | niter=0; |
---|
| 120 | while test==0 && niter<10 |
---|
| 121 | dist_fact_old=dist_fact; |
---|
| 122 | Xu=Xd./dist_fact;%undistorted sensor coordinates, second iteration |
---|
| 123 | Yu=Yd./dist_fact; |
---|
| 124 | dist_fact=1+Calib.kc*(Xu.*Xu+Yu.*Yu);% distortion factor,next approximation |
---|
| 125 | test=max(max(abs(dist_fact-dist_fact_old)))<0.00001; % reducing the relative error to 10^-5 forthe inversion of the quadraticcorrection |
---|
| 126 | niter=niter+1; |
---|
| 127 | end |
---|
[242] | 128 | denom=Dx*Xu+Dy*Yu+D0; |
---|
[1108] | 129 | Xphys=(B11.*Xu+B12.*Yu+X0)./denom;%world coordinates |
---|
| 130 | Yphys=(B21.*Xu+B22.*Yu+Y0)./denom; |
---|
[242] | 131 | if testangle |
---|
| 132 | Zphys=Z0+a*Xphys+b*Yphys; |
---|
| 133 | else |
---|
| 134 | Zphys=Z0; |
---|
| 135 | end |
---|
| 136 | else |
---|
| 137 | Xphys=-Calib.Tx_Ty_Tz(1)+X/Calib.fx_fy(1); |
---|
| 138 | Yphys=-Calib.Tx_Ty_Tz(2)+Y/Calib.fx_fy(2); |
---|
| 139 | end |
---|
| 140 | |
---|