%
%  Computes the association between points in the image plane and 
% points in the LRM floor.
%
%  Nelson Gon\c calves
%  01Oct08
% 

%%----------------------------------------------
%%
%%  Put here where the points on the floor for each
%% camera are
%%
%%                X    Y     1
% FloorCamera1 = [  1.0  2.0   1] ; 

FloorCamera8 = [  
    1.0     13.65   1; 
    -3.55   14.95   1;
    -3.55   15.97   1;
    -3.55   16.96   1;
    -3.55   18.27   1;
    1.0     18.27   1;
    1.0     17.46   1;
    1.0     14.46   1;
    -0.05   14.46   1;
    -1.31   14.85   1 %10
    -1.31   17.03   1 %11
    -0.05   17.46   1;
    ] ; 




%ImageFiles = ['FloorCamera1.jpg', 'FloorCamera2.jpg', 'FloorCamera3.jpg',
%              'FloorCamera4.jpg', 'FloorCamera5.jpg', 'FloorCamera6.jpg'] ; 
ImageFiles = ['camera8_.jpg'; 'camera8_.jpg'; 'camera8_.jpg'; 'camera8_.jpg'; 'camera8_.jpg'; 'camera8_.jpg'; 'camera8_.jpg'; 'camera8_.jpg'];

%%----------------------------------------------
%%
%%  Set the camera to use
%%
%%
ImageFile      = ImageFiles(1,:) ; 
WorldPoints    = FloorCamera8 ; 
HomographyName = 'camera8_new_hom.dat'

%%----------------------------------------------
%% 
%% Ask the user to click on the image points
%%
imshow(ImageFile) ;

imgPoints = [] ; 

for i=1:size(WorldPoints,1)

    imgPoints(i,:) = [round(ginput(1)) 1] ;
    
    % Mark the point on the image
    hold on ;
    plot(imgPoints(i,1),imgPoints(i,2),'g.') ;
    hold off ;
end

%%-----------------------------------------------
%%
%% Compute the camera homography (code from Luis Tavares)
%%
M = [] ; 

for i = 1:size(WorldPoints,1)
    M(i*2-1,:) = [-WorldPoints(i,:)   0 0 0	        imgPoints(i,1)*WorldPoints(i,:)] ;
    M(i*2,:)   = [0 0 0	             -WorldPoints(i,:)	imgPoints(i,2)*WorldPoints(i,:)] ;
end

[vect val]      = eig(M'*M);
[minimo indice]	= min(dFloorCamera8 = [  1.0  2.0   1] ; iag(val));
res             = vect(:,indice)/vect(end,indice);
P               = [res(1:3)'; res(4:6)'; res(7:9)'];

disp(P) ;

save HomographyName P -ASCII -DOUBLE ; 

%%-----------------------------------------------
%%
%%   Show on the image where the world points are
%%  projected, given the homography
%%
hold on ;

for i = 1:size(WorldPoints,1)
    
    retro	= P*WorldPoints(i,:)' ;
    retro	= round(retro/retro(3)) ;
    plot(retro(1),retro(2),'b.') ;
end

hold off ;

%%-----------------------------------------------
%%
%%   Compute the homography error
%%

error = 0 ;

for i = 1:size(WorldPoints,1)
    directo	= inv(P)*imgPoints(i,:)' ;
    directo	= directo/directo(3) ;
    erro = erro + sqrt(sum((directo-WorldPoint(i,:)').^2))/size(WorldPoint,1) ;
end

disp('--: Average projection error: ') ; 
error