How can I undistort an image in Matlab using the known camera parameters?

Question

This is easy to do in OpenCV however I would like a native Matlab implementation that is fairly efficient and can be easily changed. The method should be able to take the camera

Answer 1

The simplest and most common way of doing undistort (also called unwarp or compensating for lens distortion) is to do a forward distortion on a chosen output photo size and then a reverse mapping using bilinear interpolation.

Here is code I wrote for performing this:

function I = undistort(Idistorted, params)
fx = params.fx;
fy = params.fy;
cx = params.cx;
cy = params.cy;
k1 = params.k1;
k2 = params.k2;
k3 = params.k3;
p1 = params.p1;
p2 = params.p2;

K = [fx 0 cx; 0 fy cy; 0 0 1];

I = zeros(size(Idistorted));
[i j] = find(~isnan(I));

% Xp = the xyz vals of points on the z plane
Xp = inv(K)*[j i ones(length(i),1)]';

% Now we calculate how those points distort i.e forward map them through the distortion
r2 = Xp(1,:).^2+Xp(2,:).^2;
x = Xp(1,:);
y = Xp(2,:);

x = x.*(1+k1*r2 + k2*r2.^2) + 2*p1.*x.*y + p2*(r2 + 2*x.^2);
y = y.*(1+k1*r2 + k2*r2.^2) + 2*p2.*x.*y + p1*(r2 + 2*y.^2);

% u and v are now the distorted cooridnates
u = reshape(fx*x + cx,size(I));
v = reshape(fy*y + cy,size(I));

% Now we perform a backward mapping in order to undistort the warped image coordinates
I = interp2(Idistorted, u, v);

To use it one needs to know the camera parameters of the camera being used. I am currently using the PMD CamboardNano which according to the Cayim.com forums has the parameters used here:

params = struct('fx',104.119, 'fy', 103.588, 'cx', 81.9494, 'cy', 59.4392, 'k1', -0.222609, 'k2', 0.063022, 'k3', 0, 'p1', 0.002865, 'p2', -0.001446);

I = undistort(Idistorted, params);

subplot(121); imagesc(Idistorted);
subplot(122); imagesc(I);

Here is an example of the output from the Camboard Nano. Note: I artificially added border lines to see what the effect was of the distortion close to the edges (its much more pronounced):