% IDEALIMAGEPTS - Ideal image points with no distortion.
%
% Usage: xyideal = idealimagepts(C, xy)
%
% Arguments:
% C - Camera structure, see CAMSTRUCT for definition.
% xy - Image points specified as 2 x N array (x,y) / (col,row)
%
% Returns:
% xyideal - Ideal image points. These points correspond to the image
% locations that would be obtained if the camera had no lens
% distortion. That is, if they had been projected using an ideal
% projection matrix computed from fx, fy, ppx, ppy, skew.
%
% See also CAMSTRUCT, CAMERAPROJECT, SOLVESTEREOPT
% Copyright (c) 2015 Peter Kovesi
% pk at peterkovesi com
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% The Software is provided "as is", without warranty of any kind.
% August 2015 Adapted from imagept2plane
% February 2016 Refined inversion of distortion process slightly
function xyideal = idealimagepts(C, xy)
[dim,N] = size(xy);
if dim ~= 2
error('Image xy data must be a 2 x N array');
end
% Reverse the projection process as used by CAMERAPROJECT
% Subtract principal point and divide by focal length to get normalised,
% distorted image coordinates. Note skew represents the 2D shearing
% coefficient times fx
y_d = (xy(2,:) - C.ppy)/C.fy;
x_d = (xy(1,:) - C.ppx - y_d*C.skew)/C.fx;
% Compute the inverse of the lens distortion effect by computing the
% 'forward' direction lens distortion at this point and then subtracting
% this from the current point.
% Radial distortion factor. Here the squared radius is computed from the
% already distorted coordinates. The approximation we are making here is to
% assume that the distortion is locally constant.
rsqrd = x_d.^2 + y_d.^2;
r_d = 1 + C.k1*rsqrd + C.k2*rsqrd.^2 + C.k3*rsqrd.^3;
% Tangential distortion component, again computed from the already distorted
% coords.
dtx = 2*C.p1*x_d.*y_d + C.p2*(rsqrd + 2*x_d.^2);
dty = C.p1*(rsqrd + 2*y_d.^2) + 2*C.p2*x_d.*y_d;
% Subtract the tangential distortion components and divide by the radial
% distortion factor to get an approximation of the undistorted normalised
% image coordinates (with no skew)
x_n = (x_d - dtx)./r_d;
y_n = (y_d - dty)./r_d;
% Finally project back to pixel coordinates.
x_p = C.ppx + x_n*C.fx + y_n*C.skew;
y_p = C.ppy + y_n*C.fy;
xyideal = [x_p
y_p];