function [px,py,L]=KLTHarris(img,Metodo,Qsize,q,Tmin)
% Algoritmo de  Kanade&Lucas&Tomasi e de Harris para detecção de cantos.
%
% ENTRADAS:
% img - string definindo o nome do arquivo de entrada
% Metodo - 'KLT' ou 'Harris'
% Qsize - escalar definindo o tamanho da janela 
% q - metade da distancia minima entre cantos
% Tmin  - limiar mínimo para considerar canto
%           para Harris corresponde ao valor mínimo do parametro R (default=0.0001)
%           para KLT corresponde ao valor mínimo do menor autovalor (default=0.01)
%
% SAIDAS:
% py - coordenadas y dos pontos encontrados em ordem decrescente
% px - coordenadas x dos pontos encontrados em ordem decrescente
% L  - score dos pontos encontrados em ordem decrescente
%
% Raul Queiroz Feitosa

%-------------------------------------------------------------------------
% Le image de entrada
%-------------------------------------------------------------------------
[J,map]=imread(img);
%-------------------------------------------------------------------------

%-------------------------------------------------------------------------
% transforma para tons de cinza tipo double
%-------------------------------------------------------------------------
if ~isempty(map),
    error('Imagem de entrada deve ser RGB!');   
end
 J=im2double(J);
 I=rgb2gray(J);
%-------------------------------------------------------------------------

%-------------------------------------------------------------------------
% Cria janela de suavização circular
%-------------------------------------------------------------------------
Q2=floor(Qsize/2);
Qsize=2*Q2+1;

switch Metodo,
    case('Harris')
        w=fspecial('gaussian',Qsize, Qsize/5);
        k=0.05;
        if nargin<5 | isempty(Tmin),
            Tmin=0.0001;
        end
    case('KLT')
        w=ones(Qsize)/(Qsize^2);
        if nargin<5 | isempty(Tmin),
            Tmin=0.01;
        end
    otherwise
        error('O método deve ser Harris ou KLT!');
end
%-------------------------------------------------------------------------


%-------------------------------------------------------------------------
% Calculo dos Gradientes - Montagem da Matriz M
%-------------------------------------------------------------------------
h=fspecial('sobel')/2;
Gy=imfilter(I,h,'replicate');
Gx=imfilter(I,h','replicate');

Gx2=Gx.^2;
Gy2=Gy.^2;
GxGy=Gx.*Gy;

% suavização
SGx2=imfilter(Gx2,w,'replicate');
SGy2=imfilter(Gy2,w,'replicate');
SGxGy=imfilter(GxGy,w,'replicate');
%-------------------------------------------------------------------------

%-------------------------------------------------------------------------
% Calculo da "cornerness"
%-------------------------------------------------------------------------
DetC=SGx2.*SGy2-SGxGy.^2;
TraceC=SGx2+SGy2;

switch Metodo,
    case('Harris')
        L=DetC-k*(TraceC).^2;
    case('KLT')
        L1=(TraceC-sqrt((TraceC.^2)-(4*DetC)))/2;        
        L2=(TraceC+sqrt((TraceC.^2)-(4*DetC)))/2;
        L=min(cat(3,abs(L1),abs(L2)),[],3);
    otherwise
        error('O método deve ser Harris ou KLT!');
end

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

%-------------------------------------------------------------------------
% Seleciona apenas os maximos regionais de L
%-------------------------------------------------------------------------
L(~imregionalmax(L))=0;
%-------------------------------------------------------------------------


%-------------------------------------------------------------------------
% Seleciona os cantos, eliminando detecções muito próximas
%-------------------------------------------------------------------------
while Tmin>0,
    Laux=L;
    Mask=false(size(L));
    
    % Zera cantos numa vizinhança em torno de cada ponto encontrado
    h=waitbar(0,'Procurando os cantos...');
    Ltotal=sum(L(:)>Tmin);
    while sum(Laux(:)>Tmin)>0,
        
        % Marca posição do maior valor de R na iteração
        [py,px]=find(Laux==max(Laux(:)));
        Mask(py,px)=1;   
        
        % Elimina detecções numa janela em torno dele
        ymin=max(1,(py-q));
        ymax=min(size(L,1),py+q);
        xmin=max(1,(px-q));
        xmax=min(size(L,2),px+q);
        Laux(ymin:ymax,xmin:xmax)=0;
        
        waitbar((Ltotal-(sum(Laux(:)>Tmin)))/Ltotal,h,'Procurando os cantos ...');
 
    end
    close(h)
%-------------------------------------------------------------------------

%-------------------------------------------------------------------------
    %Visualização
%-------------------------------------------------------------------------
    figure(1);
    imshow(I);
    [r,c]=find(Mask);
    hold on
    plot(c,r,'r+')

    switch Metodo,
        case('Harris')
            title(['Rmin=' num2str(Tmin) '  Qsize=' num2str(Qsize)]);
            Tmin=input('Entre com novo valor de Rmin ou 0 (zero) para terminar = ');
        case('KLT')  
            title(['L_2min=' num2str(Tmin) '  Qsize=' num2str(Qsize)]);
            Tmin=input('Entre com novo valor de L_2min ou 0 (zero) para terminar = ');
    end    
%-------------------------------------------------------------------------

end
%-------------------------------------------------------------------------


%-------------------------------------------------------------------------
% Criar resposta da função
%-------------------------------------------------------------------------
[py,px]=find(L>Tmin);
L=L(py+(px-1)*size(L,1));
%-------------------------------------------------------------------------

return


