function [x1hat,x2hat] = correct_match_pts(x1, x2, F)

%CORRECT_MATCH_PTS corrects a list of correspondences using a given fundamental matrix.
%
%   [x1hat,x2hat] = correct_match_pts(x1, x2, F)
%
%   Given a list of measure poitn correspondences x1 <-> x2 and a
%   fundamental matrix F, compute the corrected correspondences
%   x1hat <-> x2hat that minimize the geometric error
%      d(x1,x1hat).^2 + d(x2,x2hat).^2
%      (where d(.,.) denotes the Euclidean distance between the
%       points in consideration)
%   subject to the epipolar constraint
%      x2hat' * F * x1hat = 0
%   for each pair of correspondences in the lists x1hat and x2hat.
%
%   Input arguments:
%   - x1 and x2: must both be either 3-by-n or 2-by-n matrices.
%     If they are 3-by-n matrices then the list of correspondences
%     would be converted to inhomogeneous coordinates.  Thus,
%     the last (3rd) row of both matrices must not contain 0s.
%     Here, n is the number of correspondences to be corrected.
%   - F must be a 3-by-3 rank-2 fundamental matrix.
%
%   Output arguments:
%   - x1hat and x2hat would be the list of correspondences having
%     the same dimensions as x1 and x2.
%
%   Functions required: pflat
%
%   Reference: R. Hartley and A. Zisserman, "Multiple View Geometry
%   in Computer Vision", Cambridge University Press 2000,
%   Algorithm 11.1, Steps (i)-(x), page 304.
%
%Created January 2004.
%
%Copyright Du Huynh
%The University of Western Australia
%School of Computer Science and Software Engineering

% check input arguments
[m,n] = size(x1);
if size(x2,1) ~= m | size(x2,2) ~= n
   error('correct_match_pts: x1 and x2 must be the same size');
elseif sum(size(F) == [3,3]) ~= 2
   error('correct_match_pts: F must be a 3-by-3 matrix');
end

if m == 3
   x1 = pflat(x1);
   x2 = pflat(x2);
end

x1hat = zeros(3,n);
x2hat = zeros(3,n);

T1 = eye(3);
T2 = eye(3);
for i=1:n
   T1(1:2,3) = -x1(1:2,i);
   T2(1:2,3) = -x2(1:2,i);
   Fnew = inv(T2)'*F*inv(T1);
   e1 = null(F);
   e2 = null(F');
   e1 = e1 / sqrt(e1(1)^2 + e1(2)^2);
   e2 = e2 / sqrt(e2(1)^2 + e2(2)^2);
   R1 = [e1(1:2)' 0; -e1(2) e1(1) 0; 0 0 1];
   R2 = [e2(1:2)' 0; -e2(2) e2(1) 0; 0 0 1];
   Fnew = R2*Fnew*R1';
   f1 = e1(3); f2 = e2(3);
   a = Fnew(2,2); b = Fnew(2,3); c = Fnew(3,2); d = Fnew(3,3);
   % form the polynomial g(t) as shown in Eq(11.7), page 303
   % of Hartley & Zisserman
   % Below are the coefficients generated by Matlab's symbolic
   % maths toolbox
   coeff0 = b^2*c*d-a*d^2*b;
   coeff1 = -a^2*d^2+b^2*c^2+f2^4*d^4+b^4+2*b^2*f2^2*d^2;
   coeff2 = 4*b^2*f2^2*c*d-2*a*d^2*f1^2*b+2*b^2*c*f1^2*d+...
      4*f2^4*c*d^3+4*a*b^3-a^2*d*c+b*c^2*a+4*a*b*f2^2*d^2;
   coeff3 = 6*a^2*b^2-2*a^2*d^2*f1^2+6*f2^4*c^2*d^2+2*b^2*f2^2*c^2+...
      2*b^2*c^2*f1^2+8*a*b*f2^2*c*d+2*a^2*f2^2*d^2;
   coeff4 = 4*f2^4*c^3*d-a*d^2*f1^4*b+4*a^2*f2^2*c*d+4*a*b*f2^2*c^2+...
      2*b*c^2*f1^2*a+4*a^3*b+b^2*c*f1^4*d-2*a^2*d*f1^2*c;
   coeff5 = (a^2+f2^2*c^2)^2-(a*d-b*c)*f1^4*b*c-(a*d-b*c)*f1^4*a*d;
   coeff6 = (-a*d+b*c)*f1^4*a*c;
   ts = roots([coeff6 coeff5 coeff4 coeff3 coeff2 coeff1 coeff0]);
   ts = real(ts);
   min_cost = Inf;
   for j = 1:length(ts)
      t = ts(j);
      cost = t^2 / (1+f1^2*t^2) + (c*t+d)^2/( (a*t+b)^2 + f2^2*(c*t+d)^2 );
      if cost < min_cost
         min_cost = cost;
         best_t = t;
      end
   end
   asym_cost = 1 / f1^2 + c^2/(a^2 + f2^2*c^2);
   l1 = [best_t*f1; 1; -best_t];
   l2 = [-f2*(c*best_t+d); a*best_t+b; c*best_t+d];
   x1hat(:,i) = [-l1(1)*l1(3); -l1(2)*l1(3); l1(1)^2+l1(2)^2];
   x2hat(:,i) = [-l2(1)*l2(3); -l2(2)*l2(3); l2(1)^2+l2(2)^2];
   x1hat(:,i) = inv(T1)*R1'*x1hat(:,i);
   x2hat(:,i) = inv(T2)*R2'*x2hat(:,i);
end

x1hat = pflat(x1hat);
x2hat = pflat(x2hat);
if m == 2
   x1hat = x1hat(1:2,:);
   x2hat = x2hat(1:2,:);
end
return