% Dima Damen
% Nov 8th 2007

% This function takes a file that contains 2 points on each vertical line
% that could be detected in the image. Each row in the file refers to one
% line that has been found. It finds the vanishing point (the intersection
% of each two lines) and averages over all of them to find the best
% intersection point. It then reads the homography found before to reflect
% this in the world coordinates.

function [cameraX, cameraY] = FindVanishingPointInWorld2 (linesFile, Hom)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% FindVanishingPointInWorld2
% Syntax:
%   [cameraX, cameraY] = FindVanishingPointInWorld2 (linesFile, Hom)
%
% Descrioption:
%   Finds the position of the camera on the ground plane. linesFile
%   contains points in the line perpendicular to the ground plane. Hom is
%   the ground-plane homography matrix 
%   
% COPYRIGHT
%{
%This source file is the copyright property of the University of Leeds
%('The University').
%
%Permission to use, copy, modify, and distribute this source file for
%educational, research, and not-for-profit purposes, without fee and
%without a signed licensing agreement, is hereby granted, provided that
%the above copyright notice, this paragraph and the following three
%paragraphs appear in all copies, modifications, and distributions.
%
%In no event shall The University be liable to any party for direct,
%indirect, special, incidental or consequential damages, including lost
%profits, arising out of the use of this software and its documentation.
%
%The software is provided without warranty. The University has no
%obligation to provide maintenance, support, updates, enhancements, or
%modifications.
%
%This software was written by Dima Damen, Vision Group,
%School of Computing, University of Leeds, UK. The code and use
%thereof should be attributed to the author where appropriate
%(including demonstrations which rely on it's use).
%}
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

   lines = load (linesFile);
   
   [m,n] = size (lines);
   ExpectedXY = zeros (m*(m-1), 2);
   count = 1;
   for i = 1:m
       X11 = lines (i,1);
       Y11 = lines (i,2);
       X12 = lines (i,3);
       Y12 = lines (i,4);
       M1 = (Y12-Y11)/(X12-X11);
       if (isinf (M1))
           continue;
       end;
       X3 = (2500-Y11)/M1 + X11;
%        plot ([X11; X12; X3], [Y11; Y12; 2500], 'b-');
       for j = i+1:m
           X21 = lines (j,1);
           Y21 = lines (j,2);
           X22 = lines (j,3);
           Y22 = lines (j,4);
           M2 = (Y22-Y21)/(X22-X21);
           if (isinf (M2))
               continue;
           end;
           X = (M2*X21-M1*X11+Y11-Y21)/(M2-M1);
           Y = M1*(X-X11)+Y11;
           ExpectedXY (count,:) = [X,Y];
           count = count + 1;
       end;
   end;
   ExpectedXY = ExpectedXY (1:count-1,:);
   XB = median (ExpectedXY(:,1));
   YB = median (ExpectedXY(:,2));
   P = [XB; YB; 1];
   P2 = Hom * P;
   X = P2(1,1);
   Y = P2(2,1);
   Z = P2(3,1);
   cameraX = X / Z;
   cameraY = Y / Z;
end