clc

%% Minimizzazione della mtrice fondamentale

x1 = [312,260,184,144,170,225,241,255,265,220,190,138; 
      289,311,246,196,176,226,226,190,152,113,133,94;
      1,1,1,1,1,1,1,1,1,1,1,1];
x2 = [335.9,256.8,129.5,39,116.8,179.8,256.8,256.2,256.8,200.5,142.9,59.14;
      263.5,297,268.2,195.18,154.3,201.9,233.4,192.5,138.2,62.5,68.5,16.9;
      1,1,1,1,1,1,1,1,1,1,1,1];



[F e1 e2] = fundmatrix(x1,x2);


% Scelgo i primi tre pti

x2(:,1)'*F*x1(:,1);

% Cerchiamo matrici di projezione

P1 = [eye(3,3),zeros(3,1)];

A_int = [0,-e2(3,1),e2(2,1);e2(3,1),0,-e2(1,1);-e2(2,1),e2(1,1),0];
A = A_int*F;

% P2 = [A,e2]
% vett1 = [];
% vett2 = [];
% for(i =1:1)
%     
% [x1_fin,x2_fin] = triangulation(x1(:,i),x2(:,i),F);
% 
% vett1 = [vett1, x1_fin];
% vett2 = [vett2, x2_fin];
% 
% end

    [x1_norm, TX] = normalise2dpts_diff(x1(:,1));
    [x2_norm, TY] = normalise2dpts_diff(x2(:,1));
    
    % Note that it may have not been possible to normalise
    % the points if one was at infinity so the following does not
    % assume that scale parameter w = 1.
    
    %Npts = length(x1_norm);
    A = zeros(3,9);
    
    O = [0 0 0];
    %for n = 1:Npts
	X = x1_norm(:,1)';
	x_bo = x2_norm(1,1); y = x2_norm(2,1); w = x2_norm(3,1);
	A(1,:) = [  O  -w*X  y*X];
	A(2,:) = [ w*X   O  -x_bo*X];
	A(3,:) = [-y*X  x_bo*X   O ];
    %end
    
    [U,D,V] = svd(A,0);
    D = V';
    X_head = D(:,3);
