gusucode.com > vision工具箱matlab源码程序 > vision/cvalgEstimateUncalibratedRectification.m
function [t1, t2] = cvalgEstimateUncalibratedRectification(... f, pts1, pts2, imageSize) % Algorithm for computing rectification transformations. % This function supports the estimateUncalibratedRectification function. % Copyright 2010-2014 The MathWorks, Inc. %#codegen outputClass = class(f); %-------------------------------------------------------------------------- % Compute the projective transformation for the second camera. %-------------------------------------------------------------------------- % Find the epipole [u, d, v] = svd(f); epipole = u(:, 3); % Translate the epipole to put the origin at the center of the image imageOrigin = cast(0.5, outputClass); t = [1, 0, -(cast(imageSize(2),outputClass)/2 + imageOrigin); ... 0, 1, -(cast(imageSize(1),outputClass)/2 + imageOrigin); ... 0, 0, 1]; epipole = t * epipole; % Move the epipole to the line at y=0 by rotating the image with the % minimum angle. % Ensure the homogeneous coordinates have positive sign. if epipole(3) < 0 epipole = -epipole; end % If the x coordinate is positive, rotate clockwise; otherwise, rotate % counter-clockwise. if epipole(1) >= 0 keepOrientation = -ones(1, outputClass); else keepOrientation = ones(1, outputClass); end r = [-epipole(1), -epipole(2), 0; epipole(2), -epipole(1), 0; 0, 0, sqrt(epipole(2)^2+epipole(1)^2) * keepOrientation]; epipole = r * epipole; % Move the epipole to the position at infinity. if epipole(1) ~= 0 g = [1, 0, 0; 0, 1, 0; -epipole(3)/epipole(1), 0, 1]; else g = eye(3, outputClass); end % Compute the overall transformation for the second image. t2 = g * r * t; t([1,2], 3) = -t([1,2], 3); t2 = t * t2; if(t2(end)) ~= 0 t2 = t2 / t2(end); end %-------------------------------------------------------------------------- % Compute the projective transformation for the first camera. %-------------------------------------------------------------------------- % Compute a (partial) synthetic camera matrix for the second camera. % This is matrix M, such that F=SM, where S is a skew-symmetric matrix. % Unfortunately, M cannot be computed using the usual factorization, where % S = [e]_x and M = [e]_x * F, because then M will be singular. Instead, M % must be computed using the SVD of F. % See http://www.robots.ox.ac.uk/~vgg/hzbook/hzbook2/clarification_rectification.pdf z = cast([ 0, -1, 0; 1, 0, 0; 0, 0, 1], outputClass); d(3,3) = (d(1,1) + d(2,2)) / 2; m = u * z * d * v'; % Compute the transformation for the first camera so the rows in the first % camera correspond the rows at the same location in the second camera. h1r = t2 * m; % Compute the transformation for the first camera so the points have % (approximately) the same column locations in the first and second images. numPoints = size(pts1, 1); p1 = h1r * [cast(pts1', outputClass); ones(1, numPoints, outputClass)]; p2 = t2 * [cast(pts2', outputClass); ones(1, numPoints, outputClass)]; p2InfinityIdx = abs(p2(3,:)) < eps(outputClass); p2(3, p2InfinityIdx) = eps(outputClass); b = p2(1,:) ./ p2(3,:) .* p1(3,:); y = p1' \ b'; h1c = [y(1), y(2), y(3); 0, 1, 0; 0, 0, 1]; % Compute the overall transformation for the first camera. t1 = h1c * h1r; if(t1(end)) ~= 0 t1 = t1 / t1(end); end t1 = t1'; t2 = t2'; %-------------------------------------------------------------------------- % In MATLAB execution, if either epipole is inside I2, issue warning. %-------------------------------------------------------------------------- if isempty(coder.target) %MATLAB execution if isPointInImage(imageSize, u(:,3)) || isPointInImage(imageSize, v(:, 3)); epipole1 = v([1,2],3) / v(3,3); epipole2 = u([1,2],3) / u(3,3); coder.internal.warning(... 'vision:estimateUncalibratedRectification:epipoleInImage', ... num2str(epipole1(1)), num2str(epipole1(2)), ... num2str(epipole2(1)), num2str(epipole2(2)), ... imageSize(1), imageSize(2)); end end %========================================================================== function isIn = isPointInImage(imageSize, ptHomogeneous) if ptHomogeneous(3) == 0 isIn = false; else pt = ptHomogeneous(1:2) / ptHomogeneous(3); imageOrigin = [-0.5; -0.5]; imageEnd = imageSize([2,1]) - 0.5; isIn = pt(1, :) >= imageOrigin(1) && pt(1, :) <= imageEnd(1) ... && pt(2, :) >= imageOrigin(2) && pt(2, :) <= imageEnd(2); end