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)