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);