function [x1_fin,x2_fin]= triangulation(x1,x2,F_orig)

% F fundamental matrix
% x1,x2 punto del piano immagine 3x1
% Calcolo due matrici di trasformazione ch spostano le misure x nell'origine
% troviamo x1 = (0,0,1) = x2 

T1 = [1,0,-x1(1,1); 0,1,-x1(2,1); 0,0,1];
T2 = [1,0,-x2(1,1); 0,1,-x2(2,1); 0,0,1];

% Nuova F

F = inv(T2)'*F_orig*inv(T1);

% Conto per trovare gli nuovi epipoli associati a questa matrice
% fondamentale

[U,D,V] = svd(F,0);
	e1 = hnormalise(V(:,3));
	e2 = hnormalise(U(:,3));
    
% Verifica Calcolo epipoli
 zero_epi1 = e2'*F;
 zero_epi2 = F*e1;
 
 % Normalizzazione dei due epipoli e1^2 + e2^2 = 1
 
 cost1 = sqrt(1/(e1(1,1)^2 + e1(2,1)^2)) ;
 cost2 = sqrt(1/(e2(1,1)^2 + e2(2,1)^2));
 
 e1(1,1) = e1(1,1)*cost1;
 e1(2,1) = e1(2,1)*cost1;
 e1(3,1) = e1(3,1)*cost1;
 e2(1,1) = e2(1,1)*cost2;
 e2(2,1) = e2(2,1)*cost2;
 e2(3,1) = e2(3,1)*cost2;
 
 cost1 = (e1(1,1)^2 + e1(2,1)^2);
 cost2 = (e2(1,1)^2 + e2(2,1)^2);
 
 % Con questi risultati trovo R1 e R2
 
 R1 = [e1(1,1),e1(2,1),0;-e1(2,1),e1(1,1),0;0,0,1];
 R2 = [e2(1,1),e2(2,1),0;-e2(2,1),e2(1,1),0;0,0,1];
 
 % Verifica della normalizzazione dei epipoli e delle R
 
 a = R1*e1;
 b = R2*e2;
 
 
% Setto di nuovo una nuova F

F_fin = R2*F*R1'

% Ora inizializzo i parametri del polinomio g che serve per il calcolo
% della funzxione di costo

k = e1(3,1)
k_primo = e2(3,1)
a = F_fin(2,2)
b = F_fin(2,3)
c = F_fin(3,2)
d = F_fin(3,3)

C = [k^4*a*c; (k^4*b*c + k^4*a*d); 
    (k_primo^2*c^4 + 2*k^2*a*c + k^4*b*d);
    (k_primo^2*4*c^3*d + a^2 + 2*k^2*b*c + 2*k^2*a*d);
    (2*a*b + 2*k_primo*c^2*d^2 + 4*c^2*d^2*k_primo^2 + a*c + 2*k^2*b*d);
    (b^2 + 4*c*d^3*k_primo^2 + c*b + a*d);
    (d^4 + k_primo^2  -a*d + b*c + b*d)];

radici = roots(C');

% verifico la funzione di costo s(t) con i sei valori trovati, perte reale

radici_real = real(radici);
costo = [];
for i = 1:6
    t = radici_real(i,1);    
    s = (t^2/(1 + k^2*t^2)) + ((c*t + d)^2)/(((a*t + b)^2 + k_primo^2*(c*t + d)^2));
    costo = [costo; s];
    
    for j=1:i
        if(s <= min(costo))
            t_min_tmp = [t,i];
        end
    end
    
end

%k = min(costo);

% t_min e la radice che minimizza la funzione di costo s

t_min = t_min_tmp(1,1);
% Definizione delle linee


l1 = [t_min*k,1,-t_min];
l2 = [-k_primo*(c*t_min + d), a*t_min + b, c*t_min + d];

% il pto piu vicino all'origine di ogni linea si trova con la seguente
% formula

x1_tmp =[t_min*k*t_min,t_min , (t_min*k)^2 + 1] ;
x2_tmp = [k_primo*(c*t_min + d)*(c*t_min + d), (-a*t_min + b)*(c*t_min + d),(k_primo*(c*t_min + d))^2 + (a*t_min + b)^2 ];

x1_fin = inv(T1)*R1'*x1_tmp'
x2_fin = inv(T2)*R2'*x2_tmp'

% Cerco di determinare i punti X della scena reale!

   