gusucode.com > vision工具箱matlab源码程序 > vision/+vision/+internal/+calibration/+checkerboard/subPixelLocation.m
function loc = subPixelLocation(metric, loc) %#codegen for id = 1: size(loc,1) loc(id,:) = subPixelLocationImpl(metric, loc(id,:)); end function subPixelLoc = subPixelLocationImpl(metric, loc) % subPixelLocation(metric, loc) fits a quadratic function to a patch in the % image metric around the pixel specified by loc. metric is a grayscale % 2-D image, and loc is a two-element vector [x y]. subPixelLoc is a % two-element vector [x y], corresponding to the maximum of the quadratic % function. % The size of the patch is halfPatchSize * 2 + 1 halfPatchSize = 2; % Check if the patch is outside the image if any(loc(:) < halfPatchSize + 1) || loc(1) > size(metric, 2) - halfPatchSize - 1 ... || loc(2) > size(metric, 1) - halfPatchSize -1 subPixelLoc = single(loc); return; end % Get the patch patch = metric(loc(2)-halfPatchSize:loc(2)+halfPatchSize, ... loc(1)-halfPatchSize:loc(1)+halfPatchSize); % Create the matrix for the normal equations used to solve for the % coefficients of the quadratic function. This matrix depends only on the % patch size, and thus needs to be computed only once. persistent X; if isempty(X) X = createNormalEquationsMatrix(halfPatchSize); end % Get a least-squares solution for the coefficients y = patch(:); beta = X * y; % f(x,y) = Ax^2 + By^2 + Cx + Dy + Exy + F A = beta(1); B = beta(2); C = beta(3); D = beta(4); E = beta(5); % F = beta(6), but we do not need it % Solve for the maximum of the quadratic, in patch-based coordinates x = -(2*B*C - D*E) / (4*A*B - E^2); y = -(2*A*D - C*E) / (4*A*B - E^2); if ~isfinite(x) || abs(x) > halfPatchSize || ~isfinite(y) || abs(y) > halfPatchSize x = single(0); y = single(0); end % Get the sub-pixel location subPixelLoc = single(loc) + [x y]; function X = createNormalEquationsMatrix(halfPatchSize) % X = [1 1 -1 -1 1 1; % 0 1 0 -1 0 1; % 1 1 1 -1 -1 1; % end row 1 % 1 0 -1 0 0 1; % 1 0 1 0 0 1; % 0 0 0 0 0 1; % end row 2 % 1 1 -1 1 -1 1; % 0 1 0 1 0 1; % 1 1 1 1 1 1]; v = -halfPatchSize : 1 : halfPatchSize; [x, y] = meshgrid(v, v); x = x(:); y = y(:); X = [x.^2, y.^2, x, y, x.*y, ones(size(x))]; X = (X' * X) \ X';