function [ out_points, distances ] = rok_fisheye_camera_undistort_points (ocam_model, points, fc)
    % out_points = ROK_FISHEYE_CAMERA_UNDISTORT_POINTS (ocam_model, points, fc)
    %
    % Projects the points from original (distorted) image into undistorted
    % image.
    %
    % Input:
    %  - ocam_model = calibrated camera model, produced by OCamCalib
    %  - points = 2xN matrix of points in original (distorted) image (x and 
    %    y coordinates)
    %  - fc = factor proportional to the distance of the camera to the 
    %    plane (must be same as the value used for image undistortion)
    %
    % Output:
    %  - out_points = 2xN matrix of "distorted" points (x and y
    %    coordinates)
    %
    % NOTE: this function requires OCamCalib's functions in path!
    %
    % Cpyright (C) 2012, Rok Mandeljc <rok.mandeljc@fe.uni-lj.si>
    
    % Create LUT by creating image matrix and distorting it
    [ y, x ] = ndgrid(1:ocam_model.height, 1:ocam_model.width);
    lut = rok_fisheye_camera_distort_points(ocam_model, [ x(:)'; y(:)' ], fc);
        
    % For each input point, find its closest counterpart in LUT
    for p = size(points, 2):-1:1,
        [ dist, idx ] = min( sum(( bsxfun(@minus, lut, points(:,p)) ).^2) );
        out_points(1,p) = x(idx);
        out_points(2,p) = y(idx);
        distances(p) = dist;
    end
    
    % Filter out points that have very large distance (greater than 5
    % pixel)
    invalid = find( distances >= 5 );
    
    out_points(:, invalid) = NaN;
    distances(invalid) = NaN;
end
