Lens distortion correction (image rectification） on Matlab

I made a code to correct lens distortions with less computational cost on Matlab.

On matlab, image processing methods that are defined by user tend to be very slow. 'meshgrid' must be used for pixel iterations instead of 'for loop'.

-------------------------------------------------------------------------------------------------------------------------

  % Read an input image.
  im=imread('test.jpg');
  if size(im, 3) > 1
      im=rgb2gray(im);
  end
 
  % image size
  width = size(im,2); height = size(im,1);  
 
  % Intrinsic matrix (Input image)
  P_bf = [ 880 0   width/2-10;
           0   880 height/2+10;
           0   0   1];
  % Intrinsic matrix (Corrected image)
  P_af = [ 880 0   width/2;
           0   880 height/2;
           0   0   1];
  % Lens distortion parameter
  k1=-0.3; k2=0.075; k3=0; p1=-0.0004; p2=0.0002;
             
  % Generate pixel positions in ROI.
  [xim,yim] = meshgrid(1:width, 1:height);
  points = [xim(:)';yim(:)';ones(1, size(xim(:),1))];

  % Calculate corrected pixel positions. ** Don't use for loop **
  points1 = inv(P_af)*points;
  r2 = points1(1,:).^2 + points1(2,:).^2;
  k_radial =  1 + k1 * r2 + k2 * r2.^2 + k3 * r2.^3;
  delta_x = [2*p1*points1(1,:).*points1(2,:) + p2*(r2 + 2*points1(1,:).^2);
  p1 * (r2 + 2*points1(2,:).^2)+2*p2*points1(1,:).*points1(2,:)];
  points2(1,:) = points1(1,:).*k_radial + delta_x(1,:);
  points2(2,:) = points1(2,:).*k_radial + delta_x(2,:);
  points2(3,:) = ones(1, size(xim(:),1));
  points3 = P_bf*points2;
 
  % Convert them to 2D image coordinate.
  x_warped = reshape(points3(1,:),[height width]);
  y_warped = reshape(points3(2,:),[height width]);

  % Create pixel positions on the original image coordinate by using meshgrid
  [xim,yim] = meshgrid(1:size(im,2),1:size(im,1));

  % Compute pixel values by 'linear' method.
  im_warped = interp2(xim,yim,double(im),x_warped,y_warped,'linear');

  % Show the result image
  imshow(uint8(im_warped));

-------------------------------------------------------------------------------------------------------------------------

*  Keywords: Stereo camera, stereo vision, rectification, Matlab, fast, rapid, computational cost, effective