function inliers = ransacTransfProy( puntos1,puntos2,parejas)

% puntos1 es un vector p x 2 con las coord de los p puntos caract de imag1:
% img1 es el modelo
% x1 y1
% x2 y2
%
% xp yp

% puntos2 es una matriz n x 2 con las coord de los n puntos caract de imag2 
% img2 es la img bajo perspectiva que llevaremos al modelo img1 con homogr
% parejas contiene las parejas establecidas en el emparejamiento match.
% parejas es un vector m x 2 con los índices de los m puntos emparej.
% ej:
% 2 5
% 6 7
% .
% .
% .
% indica que el punto 2 de la img1 está emparejado con el punto 5 de img2,
% el 6 con el 7 y así sucesivamente
% numMaxIter numero máximo de iteraciones del algoritmo RANSAC
% inliers son las parejas de puntos que están bien emparejadas

% -------------------------- EJECUCION------------------------------------
% puntos1 = sift(img1);
% puntos2 = sift(img2);
% parejas = match
% parejasBuenas = ransacTransfProy( puntos1, puntos 2, parejas, 10);
% for k=1:length(parejasBuenas)
%     puntos1Buenos(k,1) = puntos1(parejasBuenas(k,1),1);
%     puntos1Buenos(k,2) = puntos1(parejasBuenas(k,1),2);
%     puntos2Buenos(k,1) = puntos2(parejasBuenas(k,2),1);
%     puntos2Buenos(k,2) = puntos2(parejasBuenas(k,2),2);
% end
%%calcula matriz H transf.proy que pasa de img2 a img1 por min.cuad
% H = LMSTransfProy(puntos1Buenos, puntos2Buenos); 
% ------------------------------------------------------------------------

%parametros del algoritmo que se pueden ajustar
numMaxIter = 10;  %num de iteraciones. 
umbralDistancia = 3;  %error máximo admitido en la distancia pto calculado

numParejas=length(parejas);

for numIteraciones=1:numMaxIter
    
    indexSample = eligeParejasHomog( numParejas );
   
    %la muestra para establecer la transf proy estará formada por las 4
    %parejas que hemos metido en indexSample
    
    cuatroPuntos1 = coordParejasElegidas(puntos1, 1, parejas, indexSample);
    cuatroPuntos2 = coordParejasElegidas(puntos2, 2, parejas, indexSample);
   
    %Calculamos ahora t.proy
    %Hallamos la matriz H de la transformación proy de 2-->1
    
    H = LMSTransfProy(cuatroPuntos1,cuatroPuntos2);
    
    T = maketform('projective',H');  %OJO Matlab trabaja con las traspuestas!!!
    
    %VERIFICACION
    %Ahora miramos si hay mas parejas consistentes con la homog hallada
    % parejasBuenas sera un vector con las parejas que se ajustan a la
    % transformación proy que estamos verificando.
    numParejasBuenas=0;  %numParejasBuenas será el soporte q tiene nuestra hipotesis
    for i=1:numParejas
        %coord x,y del punto en la imagen2
        x2 = puntos2(parejas(i,2),1);
        y2 = puntos2(parejas(i,2),2);
        %coord x,y del punto en la imagen1
        x1 = puntos1(parejas(i,1),1);
        y1 = puntos1(parejas(i,1),2);
        %Si son homólogos al aplicar la transf a x2,y2 tiene que acabar
        %cerca de x1,y1
        [x_1, y_1] = tformfwd(T, x2, y2); %a (x2,y2) le corresponde (x_1,y_1)

        if ( norm ( [x_1, y_1] - [x1, y1] ) < umbralDistancia )
            numParejasBuenas = numParejasBuenas +1;
            parejasBuenas(numParejasBuenas,1) =  parejas(i,1);
            parejasBuenas(numParejasBuenas,2) =  parejas(i,2);
        end
    end
    
    if numIteraciones==1 %inicializo variables
        mejorSoporte = numParejasBuenas;
        inliers=parejasBuenas;
    end
    if numParejasBuenas  > mejorSoporte
        mejorSoporte = numParejasBuenas;
        inliers=parejasBuenas;

    end
end
end


function muestraAleatParejas = eligeParejasHomog( numParejas )
%UNTITLED muestraAleatParejas estará formado por 4 indices de las parejas
%elegidas

indexSample=zeros(1,4); %indices de las 4 parejas para calcular transf.proy

% Necesitamos 4 parejas de puntos para calcular homografía Cogemos 4 parejas al azar
% No puedo coger la misma pareja dos o más veces
for k=1:4
    indexRand=floor(numParejas*rand())+1;
    indexSample(k)=indexRand;
    while k==2 && indexSample(1)==indexSample(2) %si coge los mismos que repita
        indexRand=floor(numParejas*rand())+1;
        indexSample(k)=indexRand;
    end
    while k==3 && (indexSample(1)==indexSample(3) || indexSample(2)==indexSample(3))  %si coge los mismos que repita
        indexRand=floor(numParejas*rand())+1;
        indexSample(k)=indexRand;
    end
    while k==4 && (indexSample(1)==indexSample(4) || indexSample(2)==indexSample(4) || indexSample(3)==indexSample(4))  %si coge los mismos que repita
        indexRand=floor(numParejas*rand())+1;
        indexSample(k)=indexRand;
    end
end

muestraAleatParejas = sort(indexSample); %Aunque realmente no hace falta lo devuelve con indices en orden. 
end


function coordCuatroPuntos = coordParejasElegidas(puntos,numImg, parejas, indexSample)
    
    %Miramos a qué puntos corresponden las parejas seleccionadas
    %primer punto 
    x1 = puntos(parejas(indexSample(1),numImg),1);
    y1 = puntos(parejas(indexSample(1),numImg),2);  
    
    %segundo punto
    x2 = puntos(parejas(indexSample(2),numImg),1);
    y2 = puntos(parejas(indexSample(2),numImg),2);        
    
    %tercer punto
    x3 = puntos(parejas(indexSample(3),numImg),1);
    y3 = puntos(parejas(indexSample(3),numImg),2);        

    
    %cuarto punto
    x4 = puntos(parejas(indexSample(4),numImg),1);
    y4 = puntos(parejas(indexSample(4),numImg),2);        

 
    coordCuatroPuntos= [x1 y1; x2 y2; x3 y3; x4 y4];
end


   


