function parejasBuenas = ransacTransfAfin( img1,puntos1,img2,puntos2,parejas)
% img1 es el primer fotograma, donde llevaremos todos los demás
% puntos1 es un vector px2 con las coord de los p puntos caract imag1
% x1 y1
% x2 y2
%
% xp yp
% img2 es un fotograma que queremos llevar al original
% puntos2 es una matriz nx2 con las coord de los n puntos caract imag2 
% parejas contiene las parejas establecidas en el emparejamiento match.
% parejas es un vector mx2 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
% parejasBuenas son las parejas de puntos que están bien emparejadas
% la ROI con el vehículo de interés vendrá dada las coord de sus esquinas
% eliminar todos los puntos caracteristicos que caigan fuera de la ROI



% 
% -------------------------- EJECUCION------------------------------------
% puntos1 = sift(frame1);
% puntos2 = sift(frame2);
% parejas = match
% parejasBuenas = ransacTransfAfin( parejas, 10);  %10 iteraciones
% A = LMSTransfAfin(puntos1Buenos, puntos2Buenos); % calcula matriz A transf.afín min.cuad

% ------------------------------------------------------------------------

A=zeros(3);  %A contiene la transf afin
mejorA=zeros(3); %la mejor transf afin que encontramos
[filas,columnas]=size(img1);
ROI=[1,1,filas,columnas];  %lo hago en toda la imagen para quitarme complicaciones
numParejas=length(parejas);
indexSample=zeros(1,3); %3 parejas para calcular transf.afin
mejorSample=zeros(1,3); %3 parejas que proporc diferenc mas baja en la transf.afin

numMaxIter = 10;  %umbral límite de iteraciones
for numIteraciones=1:numMaxIter
    
    % Necesitamos 3 puntos para calcular t.afín. Los cogemos al azar
    for k=1:3
        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
    end
    %la muestra para establecer la transf afin estará formada por las 3
    %parejas que hemos metido en indexSample
    
    %Miramos a qué puntos corresponden las parejas seleccionadas
    %primer punto
    x11 = puntos1(parejas(indexSample(1),1),1);
    y11 = puntos1(parejas(indexSample(1),1),2);        
    x21 = puntos2(parejas(indexSample(1),2),1);
    y21 = puntos2(parejas(indexSample(1),2),2);    
    %segundo punto
    x12 = puntos1(parejas(indexSample(2),1),1);
    y12 = puntos1(parejas(indexSample(2),1),2);        
    x22 = puntos2(parejas(indexSample(2),2),1);
    y22 = puntos2(parejas(indexSample(2),2),2);    
    %tercer punto
    x13 = puntos1(parejas(indexSample(3),1),1);
    y13 = puntos1(parejas(indexSample(3),1),2);        
    x23 = puntos2(parejas(indexSample(3),2),1);
    y23 = puntos2(parejas(indexSample(3),2),2);    
 
    tresPuntos1= [x11 y11; x12 y12; x13 y13];
    tresPuntos2= [x21 y21; x22 y22; x23 y23];   
    %Calculamos ahora t.afin
    %Hallamos la matriz A de la transformación afín de 2-->1
    
    A = LMSTransfAfin(tresPuntos1,tresPuntos2);
    
    T = maketform('affine',A');  %OJO Matlab trabaja con las traspuestas!!!
    imgRegist= imtransform(img2,T,'XData',[1 500], 'YData',[1 500]);%lo representa siempre en un cuadrado 500x500.
    %imgRegist es el fotograma img2 llevado a la posicion inicial
    %para superponerle
    
    %Calculo diferencias en la ROI
    diferencia = SAD( img1,imgRegist,ROI); %cuanto mayor sea diferencia más distintas son las imag
    %busco diferencias pequeñas pues eso indica que la transform es buena
    if numIteraciones==1 %inicializo variables
        difMasPeque = diferencia;
        mejorSample = indexSample;  %apuntamos las tres parejas
        mejorA=A;
        mejorT=T;
    end
    if difMasPeque > diferencia
        difMasPeque = diferencia;
        mejorSample = indexSample;  %apuntamos las tres parejas
        mejorA=A;
        mejorT=T;
    end
end

%Ahora miramos si hay parejas que proporcionan resultados consistentes
n=length(parejas);
numParejasBuenas=0;
% parejasBuenas es un vector nx2 con las parejas que se ajustan a la
% transformación afin encontrada. 
% parejasBuenas( ordinalpuntos1,  ordinalpuntos2)
for i=1:n
    %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(mejorT, x2, y2); %a (x2,y2) le corresponde (x_1,y_1)
    umbralDistancia = 3;  %pongo un umbral de 3
    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






