%Tae Soo Kim
%tkim60@jhu.edu
%Computer Vision, 600.361
%Assignment 3
%Question 4

%The following piece of work is my own work but many ideas 
%and technical implementation details were referenced from 
%lecture notes of Dr.Vidal and the work of Minwoong Kim of Purdue
%University and his work on homography by Levenberg-Marquardt method and 
%image mosaicing.

function H = RANSAC_Homography(p1, p2, iter, thr, mininlier)

    count = size(p1,1);
    nc = 4; % Number of correspondences used to find a homography

    M = mininlier; % # of min inlier set
    for n = 1:iter
         lcv = 1; % Loop control variable
         while lcv % To avoid repeated selection
             r = randi(count,nc,1);
             r = sort(r); 
                 for k = 1:nc-1
                    lcv = lcv*(r(k+1)-r(k));
                 end
             lcv = ~lcv;
         end
         A = zeros(2*nc,9);
         for k = 1:nc
             A(2*k-1:2*k,:)=...
             [0,0,0,-[p1(r(k),:),1],p2(r(k),2)*[p1(r(k),:),1];
             [p1(r(k),:),1],0,0,0,-p2(r(k),1)*[p1(r(k),:),1]];
         end
         [U,D,V] = svd(A);
         h = V(:,9);
         H = [h(1),h(2),h(3);h(4),h(5),h(6);h(7),h(8),h(9)];

         d2 = zeros(count,1); % d^2(x_measured, x_true)
         for k = 1:count
             x_true = H*[p1(k,:),1]'; % x_true in HC
             temp = x_true/x_true(3);
             x_true = temp(1:2); % x_true in image plane
             d = p2(k,:)-x_true';
             d2(k) = d(1)^2+d(2)^2;
         end
         [Y I] = sort(d2);
         if sum(Y(1:M)) < thr
             inliers = I(1:M);
         end 
    end
    
    A = zeros(2*M,9);
    for k = 1:M
         A(2*k-1:2*k,:)=...
         [0,0,0,-[p1(inliers(k),:),1],p2(inliers(k),2)*[p1(inliers(k),:),1];
         [p1(inliers(k),:),1],0,0,0,-p2(inliers(k),1)*[p1(inliers(k),:),1]];
    end
    [dontneed,dontneed,V] = svd(A);
    h1 = V(:,9); % Homography estimated with all inliers 
    H = [h1(1),h1(2),h1(3);h1(4),h1(5),h1(6);h1(7),h1(8),h1(9)];

end