1  %=======================================================================


2  % Copyright 20082015, LEGI UMR 5519 / CNRS UJF GINP, Grenoble, France


3  % http://www.legi.grenobleinp.fr


4  % Joel.Sommeria  Joel.Sommeria (A) legi.cnrs.fr


5  %


6  % This file is part of the toolbox UVMAT.


7  %


8  % UVMAT is free software; you can redistribute it and/or modify


9  % it under the terms of the GNU General Public License as published


10  % by the Free Software Foundation; either version 2 of the license,


11  % or (at your option) any later version.


12  %


13  % UVMAT is distributed in the hope that it will be useful,


14  % but WITHOUT ANY WARRANTY; without even the implied warranty of


15  % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the


16  % GNU General Public License (see LICENSE.txt) for more details.


17  %=======================================================================


18 


19  function [x] = comp_distortion_oulu(xd,k);


20 


21  %comp_distortion_oulu.m


22  %


23  %[x] = comp_distortion_oulu(xd,k)


24  %


25  %Compensates for radial and tangential distortion. Model From Oulu university.


26  %For more informatino about the distortion model, check the forward projection mapping function:


27  %project_points.m


28  %


29  %INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix)


30  % k: Distortion coefficients (radial and tangential) (4x1 vector)


31  %


32  %OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix)


33  %


34  %Method: Iterative method for compensation.


35  %


36  %NOTE: This compensation has to be done after the subtraction


37  % of the principal point, and division by the focal length.


38 


39 


40  if length(k) == 1,


41 


42  [x] = comp_distortion(xd,k);


43 


44  else


45 


46  k1 = k(1);


47  k2 = k(2);


48  k3 = k(5);


49  p1 = k(3);


50  p2 = k(4);


51 


52  x = xd; % initial guess


53 


54  for kk=1:20,


55 


56  r_2 = sum(x.^2);


57  k_radial = 1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3;


58  delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2);


59  p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)];


60  x = (xd  delta_x)./(ones(2,1)*k_radial);


61 


62  end;


63 


64  end;


65 


66 

