Subversion Repositories seema-scanner

% undistortPointsFast2 - Fast undistortion of image points. 
%
% Performs a single inversion of the distortion model considering radial
% distortion only.
%
% Based on SIUndistortPoints by Morten Hannemose. 
% Adapted to conventions of Matlab CV Toolbox structs.

function points = undistortPointsFast(points, cameraParams)

assert(size(points,2)==2,'points must be Nx2');

pp = cameraParams.PrincipalPoint;
f = cameraParams.FocalLength;
ks = cameraParams.RadialDistortion;

points(:,1) = (points(:,1)-pp(1));
points(:,2) = (points(:,2)-pp(2));

Rd2 = (points(:,1)/f(1)).^2+(points(:,2)/f(2)).^2;
l1 = ks(1)*Rd2;
l2 = ks(2)*Rd2.^2;

% Perform 4 Newton steps

xn = 1;
% for i=1:4
% xn = xn - (xn.^5.*l2+l1.*xn.^3+xn-1)./(5*xn.^4.*l2+3*l1.*xn.^2+1);
% end
for i=1:4
    xn = (5-xn.*(4+2*l1.*xn.^2))./(5*((3*l1+5*l2.*xn.^2).*xn.^2+1))+4/5*xn;
end

points(:,1) = points(:,1).*xn+pp(1);
points(:,2) = points(:,2).*xn+pp(2);