function [t1,t2] = cvalgEstimateUncalibratedRectification(...
    f,pts1,pts2,imageSize,isXY)
% Algorithm for computing rectification transformations.
%   This function supports the estimateUncalibratedRectification function.

% Copyright 2010 The MathWorks, Inc.
% $Revision: 1.1.6.3 $  $Date: 2011/04/02 01:28:56 $
%#codegen

outputClass = class(f);

%--------------------------------------------------------------------------
% Compute the projective transformation for the second camera.
%--------------------------------------------------------------------------

if isXY
  f = f([2,1,3], :);
  f = f(:, [2,1,3]);
  imageOrigin = cast(0.5, outputClass);
else
  imageOrigin = cast(-0.5, outputClass);
end

% Move the image center to the origin.
t = [1, 0, -(cast(imageSize(1),outputClass)/2+imageOrigin); ...
     0, 1, -(cast(imageSize(2),outputClass)/2+imageOrigin); ...
     0, 0,                                               1];

% Decompose the fundamental matrix.
[u, d, v] = svd(f);
pt = u(:, 3);

pt = t * pt;

% Move the epipole to the line at row=0 by rotating the image with the
% minimum angle.
% Ensure the homogeneous coordinates have positive sign.
if pt(3) < 0
  pt = -pt;
end

% If the column coordinate is positive, rotate clockwise; otherwise, rotate
% counter-clockwise.
if pt(2) >= 0
  keepOrientation = ones(1, outputClass);
else
  keepOrientation = -ones(1, outputClass);
end
r = [pt(2), -pt(1),                                       0;
     pt(1),  pt(2),                                       0;
         0,      0, sqrt(pt(1)^2+pt(2)^2) * keepOrientation];
pt = r * pt;

% Move the epipole to the position at infinity.
if pt(2) ~= 0
  g = [1,            0, 0; 
       0,            1, 0; 
       0, -pt(3)/pt(2), 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.
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.
if isXY
  p1 = bsxfun(@plus,h1r(:,[1,2])*cast(flipud(pts1'),outputClass),h1r(:,3));
  p2 = bsxfun(@plus,t2(:,[1,2])*cast(flipud(pts2'),outputClass),t2(:,3));
else
  p1 = bsxfun(@plus, h1r(:,[1,2])*cast(pts1,outputClass), h1r(:,3));
  p2 = bsxfun(@plus, t2(:,[1,2])*cast(pts2,outputClass), t2(:,3));
end

p2Infinity = abs(p2(3,:)) < eps(outputClass);
p2(3,p2Infinity) = eps(outputClass);
b = p2(2,:) ./ p2(3,:) .* p1(3,:);
y = p1' \ b';
h1c = [   1,    0,    0;
       y(1), y(2), y(3);
          0,    0,    1];

% Compute the overall transformation for the first camera.
t1 = h1c * h1r;
if(t1(end)) ~= 0
  t1 = t1 / t1(end);
end

if isXY
  t1 = convertGeometricTransform(t1);
  t2 = convertGeometricTransform(t2);
end

%--------------------------------------------------------------------------
% If either epipole is inside I2, issue warning.
%--------------------------------------------------------------------------
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);
  warning('vision:estimateUncalibratedRectification:epipoleInImage', ...
    ['An epipole may be located inside of an image. The epipoles are ',...
     'located at [%f,%f] in I1 and [%f,%f] in I2, but the specified ',...
     'imageSize was [%d,%d]. Severe distortion may result if T1 or ',...
     'T2 are used to transform these images. See isEpipoleInImage for '...
     'more information.'],...
    epipole1(1), epipole1(2), epipole2(1), epipole2(2), ...
    imageSize(1), imageSize(2));
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(1:2) - 0.5;
  isIn = pt(1, :) >= imageOrigin(1) && pt(1, :) <= imageEnd(2) ...
    && pt(2, :) >= imageOrigin(2) && pt(2, :) <= imageEnd(2);
end

%==========================================================================
function T = convertGeometricTransform(t)
T = t;
T(1,1) = t(2,2);
%T(1,2) = t(1,2);
T(1,3) = t(3,2);
%T(2,1) = t(2,1);
T(2,2) = t(1,1);
T(2,3) = t(3,1);
T(3,1) = t(2,3);
T(3,2) = t(1,3);
%T(3,3) = t(3,3);
