lunes, 27 de febrero de 2017

Tracking de objetos en video mediante filtro de Kalman (MATLAB)

TRACKING DE VIDEO MEDIANTE KALMAN

En este programa se lleva a cabo el tracking de personas y objetos en un video. Para llevar a cabo esta acción se han utilizado llevado a cabo tres etapas principales.

-         -  Primero, detección de los puntos en movimiento. Para ello  se ha optado por la solución más sencilla posible, que es la de realizar la diferencia entre dos frames consecutivos del video. El mayor problema de este método es su sensibilidad al ruido, razón por la que se realiza también un filtrado.

-          - Segundo, un método de segmentación que nos permite distinguir los distintos objetos que hay en la escena. En este caso se ha optado por el método de k-means o isodata. Se podría haber utilizado en principio cualquier otro método de clusters o segmentación.

-          - Tercero, filtro de Kalman de manera que se mejora el seguimiento de los objetos, y además se solucionan problemas asociados a la superposición de objetos o desaparición temporal de los mismos. Se ha optado por una descripción del sistema de segundo orden (aceleración constante).

El funcionamiento del programa se explica con mayor detalle en el código que viene a continuación. Este es el único código necesario para la ejecución del programa.

Como video de entrada se ha utilizado el siguiente:

El video resultado es el mostrado a continuación:


El código en Matlab:

% README: Este es el único código necesario para la ejecución del programa.
% Es necesario que en el mismo directorio en el que se encuentre el
% programa exista un video en formato mp4 llamado Visor_Humains.mp4. Sino
% basta con cambiar la parte inicial del código del programa para adecuarlo
% a cada situación

clear all;
close all;
clc;

%% 0.0 Se cargan los datos correspondientes al video en una estructura
% denominada video
video = VideoReader('Visor_Humains.mp4');
frames=video.read();
for k=1:video.NumberOfFrames
    video_fr(k).cdata = squeeze(frames(:,:,:,k));
end
longueur_sequence=video.NumberOfFrames;

% 0.0.1 Se cambia de RGB a nivel de grises para facilitar el tratamiento
Width=video.Width ;
Height=video.Height;
video_fr_gray=zeros(Height,Width,longueur_sequence);
for i=1:longueur_sequence
    video_fr_gray(:,:,i) = rgb2gray(video_fr(i).cdata);
end

%% 0.1 Descripción del sistema para Kalman
num_estados=6;
num_salidas=4;
% Descripción del vector de estado
% x(1)-Position horizontal
% x(2)-Position vertical
% x(3)-Velocidad Horizontal
% x(4)-Velocidad Vertical
% x(5)-Dimensión horizontal del objeto
% x(6)-Dimensión vertical del objeto
Dt=1; % En realidad debería ser fijado a 1/25 de segundos (25 frames por s)
% Se utiliza una descripción de segundo orden (suponemos aceleración cte)
A_est=[1  0  Dt 0  0  0;
       0  1  0  Dt 0  0;
       0  0  1  0  0  0;
       0  0  0  1  0  0;
       0  0  0  0  1  0;
       0  0  0  0  0  1];
% Se han añadido al estado las dimensiones del objeto
H=[1 0 0 0 0 0;
   0 1 0 0 0 0;
   0 0 0 0 1 0;
   0 0 0 0 0 1];
sigma_v_est=1e-0;
sigma_w_est=2e-5;
% Valores que se han fijado de manera heuristica para un mejor
% comportamiento en el video del ejemplo
R=diag([0.5*sigma_v_est 1*sigma_v_est 0.1*sigma_v_est 0.1*sigma_v_est]);
Q=diag([0.3*sigma_w_est 0.01*sigma_w_est...
    0.1*sigma_w_est sigma_w_est 10*sigma_w_est 10*sigma_w_est]);

%% 0.2 PArametros del video
init_frame=2;
temps=0.04; % Tiempo entre frames
num_frames=900;
total_frames=size(video_fr_gray,3);

%% 0.3 Otros parámetros
% Tipo de filtro que se aplica para suavizar el efecto del ruido a la hora
% de detectar los puntos en movimiento
filt = fspecial('gaussian',15);
% Valor mínimo de la diferencia entre dos frames para considerar que existe
% movimiento en la escena
threshold=55;
% Nombre máximo de objetos que se pueden detectar en la escena
max_obj=5;

%% 0.4 Inicialización de la estructura objeto y de parametros del filtro de
% Kalman
for i=1:max_obj
    % 0.4.1 Inicialización de los objetos
    % .presence inica la presencia del objeto en la escena, esté visible o
    % no. El objeto no está presente al inicio
    objet(i).presence=false;
    % .visibilite indica si el objeto está visible o no, ya que puede ser
    % que este en la escena pero esté oculto por la superposición con otro
    % objeto. Inicialmente no visible.
    objet(i).visibilite=false;
    % Número de frames en los que el objeto no ha sido visualizado en los
    % últimos frames
    objet(i).non_visual=0;
    % Posición del objeto
    objet(i).position=[0 0]';
   
    % 0.4.1 Inicialización de los parámetros para el filtro de Kalman
    % Vector de estado estimado
    objet(i).x_est=zeros(num_estados,init_frame+num_frames);
    % Vector de estado proyectado
    objet(i).x_proy=squeeze(objet(i).x_est(:,1));
    objet(i).z=zeros(num_salidas,1);
    objet(i).P_proy=Q; % Proyección de la covarianza
end;
% Número de objetos presentes en la escena
num_obj_presents=0;
% Mínimo número de pixels en los que se detecta movimiento para suponer que
% de verdad hay movimiento en la escena y no es ruido.
min_point_mov=100;
% Número máximo de frames en los que el objeto no ha sido visualizado y a
% partir del cual suponemos que el objeto ha desaparecido de la escena
max_frames_non_visual=12;
% Distancia mínima (medida en pixels) a partir de la cual se supone que dos
% clusters detectados son en realidad el mismo
dist_min=80;
% Distancia mínima entre un objeto que se esta visualizando, y un la
% posición estimada de un objeto para suponer que son el mismo
dist_min_2=80;


%--------------------------------------------------------------------------
%% ---------------------------BOUCLE GENERAL-------------------------------
%--------------------------------------------------------------------------
frame(init_frame-1).num_obj_presents=0;
for i=init_frame:(init_frame+num_frames)
    % 1. Se obtiene la diferencia entre dos frames consecutivos
    diff = squeeze(video_fr_gray(:,:,i))-squeeze(video_fr_gray(:,:,i-1));
   
    % 2. Filtramos la diferencia para eliminar el ruido
    diff_filt=filter2(filt,diff,'same');
   
    % 3. Se hace un threshold
    frame(i).moving=(abs(diff_filt)>threshold);
   
    % 4. Se busca el número de objetos visibles en la escena mediante
    % técnicas de clusters (Aquí K-Means). Se supone que los objetos
    % únicamente se mueven en horizontal, de manera que consideramos
    % unicamente su proyección sobre el eje horizontal, minimizando el
    % coste computacional
    [row,col] = find(frame(i).moving==1);
    xbins1 = 0:size(diff_filt,2);
    [counts,centers]=hist(col,xbins1);
    num_objets=0;
    frame(i).num_objets_detec=0;
    frame(i).num_objets=0;
    if ((~isempty(col))&&(size(col,1)>min_point_mov))
        % 4.1 Se realiza un clusterizado para el nombre máximo de objetos
        % que puede haber en la escena
        [aux.idx,aux.C,sum_clust]=...
            kmeans(col,max_obj,'EmptyAction','singleton');
        aux_old.idx=aux.idx;
       
        % 4.2 Se fusionan los objetos que están muy próximos
        Dist_Mat=zeros(max_obj);
        for num=1:max_obj
            for j=num:max_obj
                % Se construye la matriz de distancias entre clusters
                Dist_Mat(num,j)=abs(aux.C(num)-aux.C(j));
            end;
        end;
        Dist_Mat=Dist_Mat';
        % Dist_Mat_2 contiene 1 si los objetos de la columna y de la línea
        % son el mismo objeto. Guardamos la mitad inferior puesto que es
        % simétrica.
        Dist_Mat_2=tril((Dist_Mat<dist_min).*(Dist_Mat>0));
        % La líneas de obj_agrup indican los nuevos inidices de los objetos
        % que han sido agrupados, mientras que las columnas son el
        % resultado del k-mean original.
        obj_agrup=zeros(max_obj,max_obj);
        % agrup(k) indica si los clusters de salida de k-means han sido
        % clasificados
        agrup=zeros(1,max_obj);
        num_obj=0;
        for num=1:max_obj
            if (agrup(num)==0)
                % Se marca como agrupado
                agrup(num)=1;
                % Se aumenta el número de objetos agrupados
                num_obj=num_obj+1;
                % Se indican los clusters que forman el objeto
                obj_agrup(num_obj,num)=1;
                % Se miran las columnas de Dist_Mat para ver que clusters
                % están próximos
                for j=num+1:max_obj
                    if (Dist_Mat_2(j,num)==1)
                        % Se marca como agrupado y se indica a qué grupo
                        % pertenece
                        agrup(j)=1;
                        obj_agrup(num_obj,j)=1;
                        % Se buscan los grupos que están próximos
                        for jj=1:num
                            if (Dist_Mat(j,jj)==1)
                                agrup(jj)=1;
                                obj_agrup(num_obj,jj)=1;
                            end;
                        end;
                    end;
                end;
            end;
        end;
        % Una vez agrupados todos los clusters y encontrados todos los
        % objetos, se actualiza la tabla de índices
        old=aux.idx;
        new=zeros(size(old));
        for num=1:num_obj
            for j=1:max_obj
                if (obj_agrup(num,j)==1)
                    [row_g,col_g]=find(old==j);
                    new(row_g,col_g)=num;
                end;
            end;
            % Se calculan los nuevos centros
            n_i=sum(new==num);
            aux.C(num)=sum(col.*(new==num))/n_i;
        end;
        aux.idx=new;

        % OSe actualiza el número de objetos
        frame(i).num_objets=num_obj;
       
               
        % 5. Se calcula el tamañó de cada objeto para poder dibujar un
        % rectangulo a su alrededor
        for num=1:frame(i).num_objets
            temp=row.*(aux.idx==num);
            temp=temp(find(temp~=0));
            bottom(num) = max(temp);
            top(num) = min(temp);
            temp=col.*(aux.idx==num);
            temp=temp(find(temp~=0));
            right(num) = max(temp);
            left(num) = min(temp);
            % Se guardan los centro de los objetos
            frame(i).centres(num,1)=(right(num)+left(num))/2;
            frame(i).centres(num,2)=(bottom(num)+top(num))/2;
        end;
    end;

    % 6. Hacer la relación entre los objetos observados y los objetos
    % estimados mediante Kalman
   
    %Se define una estructura objeto con los siguientes atribujots:
    % - presencia: (booleano) Indica si el objeto está en la escena incluso
    %   si este no puede ser visto porque otro objeto se encuentra delante
    %   suyo. Este sería el caso de cuando dos personas se cruzan en la
    %   escena.
    % - visibilite: (booleano) Se utiliza para inidcar si el objeto
    %   presente está visible o no.
    % - non_visual: (int) Es el número de veces que le objeto no ha sido
    %   visto de manera consecutiva. Si este valor superca cierto límite
    %   previamente establecido, el objeto se considera que ha
    %   desaparecido. Si este valor es menor que ese límite se considera
    %   que el objeto está presente pero no visible.
    % - x_est: Es el vector de estado estimado por Kalman para determinado
    %   objeto
    % - x_proy: Es el vector de estado proyectado por Kalman para
    %   determinado objeto.
    % - z: Es la observación correspondiente al objeto
    % - P_proy: Es la proyección para el próximo estado de la matriz de
    %   covarianza
   
    % Haremos por lo tanto un array de tamaño el número máximo de objetos
    % que se suponen que se pueden encontrar en la escena
   
    % 6.1 Primero se van a comparar los objetos estimados con Kalman con
    % los objetos observados. De esta manera se va a construir una matriz
    % Dist_Vis_Pres, con las distancias entre los objetos visto y los
    % objetos presentes
    frame(i).num_obj_presents=0;
    Rel_Vis_Pres=zeros(max_obj,max_obj);
    Dist_Vis_Pres=[];
    % Si no se ven objetos se ponen las visibilidades a cero
    if (frame(i).num_objets==0)
        for j=1:max_obj
            objet(j).visibilite=false;
            % Se verifica que el objeto ha sido visto últimamemente, si no
            % se hace reset del objeto
            if (objet(j).non_visual>=max_frames_non_visual)
                objet(j).presence=false;
                objet(j).visibilite=false;
                objet(j).non_visual=0;
                objet(j).position=[0 0]';
                objet(j).x_est=zeros(num_estados,init_frame+num_frames);
                objet(j).x_proy=squeeze(objet(j).x_est(:,1));
                objet(j).z=zeros(num_salidas,1);
                objet(j).P_proy=Q;
            end;
        end;
    end;
   
    for num=1:frame(i).num_objets
        for j=1:max_obj
            % Se verifica que el objeto ha sido visto últimamemente, si no
            % se hace reset del objeto
            if (objet(j).non_visual>=max_frames_non_visual)
                objet(j).presence=false;
                objet(j).visibilite=false;
                objet(j).non_visual=0;
                objet(j).position=[0 0]';
                objet(j).x_est=zeros(num_estados,init_frame+num_frames);
                objet(j).x_proy=squeeze(objet(j).x_est(:,1));
                objet(j).z=zeros(num_salidas,1);
                objet(j).P_proy=Q;
            end;
            % _Si el objeto está presente se calcula la distancia con el
            % objeto visto
            if (objet(j).presence==true)
                % MaMatriz de distancia entre los objetos vistos y
                % presentes
                Dist_Vis_Pres(num,j)=...
                    abs(frame(i).centres(num,1)-objet(j).position(1));
            else
                % MaMatriz de distancia entre los objetos vistos y
                % presentes
                Dist_Vis_Pres(num,j)=inf;
            end;
        end;
        % Una vez que hemos encontrado las distancias entre los objetos
        % vistos y estimados, hacemos la relación entre los dos, de manera
        % que al objeto presente le corresponda el objeto visto más próximo
        % a él, verificando siempre que esta distancia sea mayor que la
        % distancia límite a partir de la cual suponemos que dos objetos
        % son el mismo objeto (dist_min_2)
        if isempty(Dist_Vis_Pres)
            % Si no había objetos presentes antes, pero ha sido detectado
            % un objeto, se crea un nuevo objeto presente, fijando que la
            % distancia mínima es infinito.
            aux2=inf;
        else
            aux2=min(squeeze(Dist_Vis_Pres(num,:)));
        end;
       
      
        if (aux2<dist_min_2)
            % 6.1.1 Se ha encontrado una correspondencia:
            % Se hace la relación entr el objeto presente y visto
            Rel_Vis_Pres(num)=find(squeeze(Dist_Vis_Pres(num,:))==aux2);
            objet(Rel_Vis_Pres(num)).visibilite=true;
            % Se actualizan los atributos del objeto:
            % Outputs/Observaciones:
            objet(Rel_Vis_Pres(num)).z=[frame(i).centres(num,1)...
                frame(i).centres(num,2) abs(right(num)-left(num))...
                abs(bottom(num)-top(num))]';
            % Posción:
            objet(Rel_Vis_Pres(num)).position=[frame(i).centres(num,1)...
                frame(i).centres(num,2)]';
            % Como se acaba de ver el objeto se pone el contador de no
            % visible a cero.
            objet(Rel_Vis_Pres(num)).non_visual=0;
        else
            % 6.1.2 Si no se ha hecho ninguna correspondencia entre vistos
            % y presentes, se crea un nuevo objeto.
            trouve=false;
            cont=0;
            while (~trouve)
                cont=cont+1;
                if (objet(cont).presence==false)
                    trouve=true;
                    objet(cont).presence=true;
                    objet(cont).visibilite=true;
                    objet(cont).non_visual=0;
                    objet(cont).x_est(:,i)=[frame(i).centres(num,1)...
                        frame(i).centres(num,2)...
                        0 0 ...
                        abs(right(num)-left(num))...
                        abs(bottom(num)-top(num))]';
                    objet(cont).position=squeeze(objet(cont).x_est(1:2,i));
                    objet(cont).x_proy=squeeze(objet(cont).x_est(:,i));
                    objet(cont).z=[frame(i).centres(num,1)...
                        frame(i).centres(num,2) abs(right(num)-left(num))...
                        abs(bottom(num)-top(num))]';
                    objet(cont).P_proy=Q;
                end;
            end;
        end;
    end;
   
   
    % 7. Se utiliza el fltro de Kalman para todos los objetos una vez que
    % se ha realizado la buena correspondencia entre los objetos vistos y
    % estimados
    for num=1:max_obj
        if (objet(num).presence && objet(num).visibilite)
            % Se dá mayor o meno importancia a las observaciones
            % dependiendo de donde se ha visto el objeto (Idea es eliminar
            % los efectos de borde)
            if ((objet(num).z(1)<50)||(objet(num).z(1)>(Width-125)))
                R_k=0.1*R;
                Q_k=10*Q;
            else
                R_k=R;
                Q_k=Q;
            end;
            % 7.1 Si el objeto está presente  ha sido visualizado, se
            % procede con kalman utilizando la descripción del sistema
            % 7.1.1 Cálculo de la ganancia de Kalman
            objet(num).G=...
                objet(num).P_proy*(H')/(H*objet(num).P_proy*(H')+R_k);
            % 7.1.2 Actualización del estado
            objet(num).x_est(:,i)=objet(num).x_proy+...
                objet(num).G*(objet(num).z-H*objet(num).x_proy);
            % 7.1.3 Actualización de la matriz de covarianza del error de
            % estimación
            objet(num).P_est=...
                objet(num).P_proy*(eye(num_estados)-(H')*(objet(num).G'));
            % 7.1.4 Proyeccion del vector de estado para el proximo
            % instante
            objet(num).x_proy=A_est*objet(num).x_est(:,i);
            % 7.1.5 Proyección de la matriz de covarianza
            objet(num).P_proy=A_est*objet(num).P_est*(A_est')+Q_k;
            objet(num).position=squeeze(objet(num).x_est(1:2,i));
        elseif objet(num).presence
            % 7.2 Si el objeto esta presente pero no visible (No existen
            % observaciones en este caso). Únicamente se puede utilizar el
            % modelo para estimar la posición.
            % 7.2.1 Se aumenta el número de frames en los que no ha sido
            % visto el objeto presente
            objet(num).non_visual=objet(num).non_visual+1;
            % 7.2.2 Se aplican las ecuaciones del sistema
            objet(num).x_est(:,i)=objet(num).x_proy;
            objet(num).x_proy=A_est*objet(num).x_est(:,i);
            objet(num).position=squeeze(objet(num).x_est(1:2,i));
        else
            % 7.3 El objeto no está presente
        end;
    end;
   
    % 8. Se muestran los puntos en movimiento
    figure_1=figure(1);
    pause(0.01); % 25 frames par second
    imshow(uint8(video_fr_gray(:,:,i))); hold on;
    color=[1 0 0;
           0 0 1;
           0 1 0;
           0.5 0 0.5;
           0 0.5 0.5;
           0.5 0.5 0;
           0 0 0;
           ];
    for j=1:max_obj
        if objet(j).presence

            if ((objet(j).x_est(5,i)~=0)&&(objet(j).x_est(6,i)~=0))
            rectangle('Position',[(objet(j).x_est(1,i)-objet(j).x_est(5,i)/2)...
                (objet(j).x_est(2,i)-objet(j).x_est(6,i)/2)...
                objet(j).x_est(5,i) objet(j).x_est(6,i)],...
                'EdgeColor',squeeze(color(j,:)),...
                'LineWidth',3);
            strmax = ['Objet ',num2str(j)];
            text((objet(j).x_est(1,i)-objet(j).x_est(5,i)/2),...
                (objet(j).x_est(2,i)-objet(j).x_est(6,i)/2-20),...
                strmax,'HorizontalAlignment','left',...
                'Color',squeeze(color(j,:)));
            end;
        end;
    end;
    num_objets=sum([objet(1:end).presence]);
    title(sprintf('Nombre d objets %i',num_objets));
    axis tight manual
    set(gca,'nextplot','replacechildren');
end;

Espero que les sea útil, un saludo (Remarcar que ciertas palabras puede que se encuentren en francés)