function I_new = rok_fisheye_camera_undistort_image (ocam_model, I, fc)
    % I_new = ROK_FISHEYE_CAMERA_UNDISTORT_IMAGE (ocam_model, I, fc)
    %
    % Undistort image using OCamCalib's camera model. This function is very
    % similar to 'undistort' provided by OCamCalib, with slight change in
    % LUT construction (namely, xc and yc provided by the model are used
    % instead of half image width and height) within
    % ROK_FISHEYE_CAMERA_DISTORT_POINTS().
    %
    % This function uses nearest neighbour interpolation; if better is
    % required, please use OpenCV and C++ functions provided by OCamCalib;
    % although for the same output, modify the LUT construction to use xc
    % and yc instead of half image widht and height.
    %
    % Input:
    %  - ocam_model = calibrated camera model, produced by OCamCalib
    %  - I = input image (either color or grayscale)
    %  - fc = factor proportional to the distance of the camera to the 
    %    plane (must be same as the value used for image undistortion)
    %
    % Output:
    %  - I_new = undistorted image
    %
    % NOTE: this function requires OCamCalib's functions in path!
    %
    % Copyright (C) 2012, Rok Mandeljc <rok.mandeljc@fe.uni-lj.si>
    
    % Use same image size as original image...
    width = ocam_model.width;
    height = ocam_model.height;
    
    % Create LUT by creating image matrix and distorting it
    [ y, x ] = ndgrid(1:height, 1:width);
    lut = rok_fisheye_camera_distort_points(ocam_model, [ x(:)'; y(:)' ], fc);
    
    % Allocate output image
    I_new = zeros(height, width, size(I, 3), 'uint8');

    % Replicate greyscale image in all three channels
    if size(I, 3) == 1,
        I_tmp(:,:,1) = I;
        I_tmp(:,:,2) = I;
        I_tmp(:,:,3) = I;  
    else
        I_tmp = I;
    end

    % Nearest neihgbour - we use OCamCalib's function
    [ r, g, b ] = get_color_from_imagepoints( I_tmp, lut(end:-1:1, :)' );
    
    % Rearrange the channels
    I_new(:,:,1) = reshape(r, height, width);
    I_new(:,:,2) = reshape(g, height, width);
    I_new(:,:,3) = reshape(b, height, width);
end
