% Unwarps the input image 'in' given a vector of lens distortion parameters
% 'k' and the vector 'center' of row, column coordinates of the image
% center.
% Mode can be one of 'neighbor', 'linear', 'cubic', 'spline'. Defaults to
% 'neighbor'.

function out = unwarp(in, k, center, mode)

if nargin < 4
    mode = 'linear';
end

% Undistorted image coordinates
r = (1:size(in, 1))' * ones(1, size(in, 2));
c = ones(size(in, 1), 1) * (1:size(in, 2));

% Centered versions
r = r - center(1);
c = c - center(2);

% Even powers of the distance from image center
radius2 = r .* r + c .* c;
radius4 = radius2 .^ 2;

distortion = 1 + k(1) + k(2) * radius2 + k(3) * radius4;

% Centered, distorted coordinates
rd = r .* distortion;
cd = c .* distortion;

% Un-centered versions
rd = rd + center(1);
cd = cd + center(2);

% Do the actual work
out = interp2(in, cd, rd, mode);