function [x_comp]  = comp_distortion(x_dist,k2);

%       [x_comp] = comp_distortion(x_dist,k2);
%       
%       compensates the radial distortion of the camera
%       on the image plane.
%       
%       x_dist : the image points got without considering the
%                radial distortion.
%       x : The image plane points after correction for the distortion
%       
%       x and x_dist are 2xN arrays
%
%       NOTE : This compensation has to be done after the substraction
%              of the center of projection, and division by the focal
%              length.
%       
%       (do it up to a second order approximation)

[two,N] = size(x_dist);

if (two ~= 2 ), 
 error('ERROR : The dimension of the points should be 2xN');
end;

if length(k2) > 1,
   [x_comp]  = comp_distortion_oulu(x_dist,k2);
else
   
radius_2= x_dist(1,:).^2 + x_dist(2,:).^2;
radial_distortion = 1 + ones(2,1)*(k2 * radius_2);
radius_2_comp = (x_dist(1,:).^2 + x_dist(2,:).^2) ./ radial_distortion(1,:);
radial_distortion = 1 + ones(2,1)*(k2 * radius_2_comp);
x_comp = x_dist ./ radial_distortion;

end;

%% Function completely checked : It works fine !!!