gusucode.com > vision工具箱matlab源码程序 > vision/+vision/+internal/+calibration/distortPoints.m

    function distortedPoints = distortPoints(points, intrinsicMatrix, ...
    radialDistortion, tangentialDistortion)

%#codegen

% unpack the intrinisc matrix
cx = intrinsicMatrix(3, 1);
cy = intrinsicMatrix(3, 2);
fx = intrinsicMatrix(1, 1);
fy = intrinsicMatrix(2, 2);
skew = intrinsicMatrix(2, 1);

% center the points
center = [cx, cy];
centeredPoints = bsxfun(@minus, points, center);

% normalize the points
yNorm = centeredPoints(:, 2, :) ./ fy;
xNorm = (centeredPoints(:, 1, :) - skew * yNorm) ./ fx;

% compute radial distortion
r2 = xNorm .^ 2 + yNorm .^ 2;
r4 = r2 .* r2;
r6 = r2 .* r4;

k = zeros(1, 3, 'like', radialDistortion);
k(1:2) = radialDistortion(1:2);
if numel(radialDistortion) < 3
    k(3) = 0;
else
    k(3) = radialDistortion(3);
end

alpha = k(1) * r2 + k(2) * r4 + k(3) * r6;

% compute tangential distortion
p = tangentialDistortion;
xyProduct = xNorm .* yNorm;
dxTangential = 2 * p(1) * xyProduct + p(2) * (r2 + 2 * xNorm .^ 2);
dyTangential = p(1) * (r2 + 2 * yNorm .^ 2) + 2 * p(2) * xyProduct;

% apply the distortion to the points
normalizedPoints = [xNorm, yNorm];
distortedNormalizedPoints = normalizedPoints + normalizedPoints .* [alpha, alpha] + ...
    [dxTangential, dyTangential];

% convert back to pixels
distortedPointsX = distortedNormalizedPoints(:, 1) * fx + cx + ...
    skew * distortedNormalizedPoints(:,2);
distortedPointsY = distortedNormalizedPoints(:, 2) * fy + cy;


distortedPoints = [distortedPointsX, distortedPointsY];