-
Notifications
You must be signed in to change notification settings - Fork 0
/
filtro_de_particulas_CAM.m
executable file
·164 lines (108 loc) · 4.43 KB
/
filtro_de_particulas_CAM.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
clc
clear all;
close all;
pkg load image-acquisition
pkg load image
%addpath ./funciones_viejas; %funciones de backup (no utilizadas en esta versi�n)
%% Variables
dt = 1; % [frame] Cada cuantos frames hago la predicci�n
N_particulas = 2000;
N_part_max = 10000;
desvio_color = [0.4 0.1 0.1]; % [escala de color] Desvio estandar de la
% medicion del color
desvio_pos = 5; % [pixels] Desvio estandar de la estimacion de la posicion
color_trackeo = [0; 0; 255]; % [RGB] Valor del color a trackear
K = 0.7; % Factor de multiplicaci�n de media para graficar
W_th = 0.00001; % Valor m�nimo del peso medio, debajo de este crea part�culas nuevas
m = 'R';
%R: Roulette wheel selection
%S: Stochastic universal sampling
exit = false; % flag de salida del GUI
%% Modelo de movimiento
A = [1 0 dt 0 0.5*dt^2 0 ;
0 1 0 dt 0 0.5*dt^2;
0 0 1 0 dt 0 ;
0 0 0 1 0 dt ;
0 0 0 0 1 0 ;
0 0 0 0 0 1] ;
% x = x' + vx' * dt + 1/2 * dt^2 * ax';
% y = y' + vy' * dt + 1/2 * dt^2 * ay';
% vx = vx' + ax' *dt;
% vy = vy' + ay' *dt;
% ax = ax';
% ay = ay';
% Suponemos aceleraci�n constante entre frame y frame
%% Vector de Estados del objeto trackeado
E = zeros(size(A,1),1); % vector de estado del objeto trackeado
% E = [x; y; vx; vy; ax; ay];
%% Cargamos el video
%----- CHEQUEAR CONFIGURACIONES CON IMAQTOOL -----%
% Comentar la linea si usan YUV
% video = videoinput('winvideo', 1, 'RGB24_640x480');
% Comentar las 2 lineas si usan RGB
%video = videoinput('winvideo', 1, 'YUY2_640x480');
video = videoinput("v4l2", "/dev/video0");
%video.ReturnedColorspace = 'rgb';
set(video, "VideoFormat", "RGB24");
video.FramesPerTrigger = Inf;
resolucion = get(video, "VideoResolution");
%-------------------------------------------------%
%% Inicializaci�n de Interfaz Gr�fica
% gui();
%% Inicializaci�n de Particulas
X = crear_particulas([1 1], resolucion, N_particulas); % Ancho x Alto
%% Inicializaci�n de Video
start(video)
%% Generaciones
while(exit == false)
if (kbhit(1) == 'q')
exit = true
end
% Obtenemos un frame
% Frame = getdata(video, 1); % Alto x Ancho x 3(RGB)
Frame = getsnapshot(video); % Alto x Ancho x 3(RGB)
% Calculamos los pesos de cada particula
W = evaluacion(desvio_color, color_trackeo, X(1:2, :), Frame);
% Graficamos las particulas
[X, ind] = mostrar_particulas(X, W, Frame, resolucion, K, W_th);
% Graficamos el contorno y calculamos el �rea
[Area, Cont] = mostrar_contorno(X, ind);
% --------------NO COMENTAR PARA VERSION CON GUI------------%
% % Actualizo los valores de la gui
% [N_part_nuevas, color_trackeo, ...
% desvio_color, K, desvio_pos, ...
% dt, exit] = gui(Frame, X, ind, Cont, Area);
% ----------------------------------------------------------%
% --------------COMENTAR PARA VERSION CON GUI---------------%
% Variamos el n�mero de particulas en funci�n del �rea
[X, W] = variar_N_particulas(X, W, Area, N_part_max, resolucion);
% ----------------------------------------------------------%
% Selecci�n
X = seleccion(X, W, m);
% Estimaci�n del vector de estados del objeto trackeado
E = estimacion(X)
N_p = size(X,2)
% Predicci�n para el proximo frame
X = prediccion(X, A);
% Mutaci�n
X = mutacion(X, desvio_pos, dt);
% Correcci�n por si alguna particula no tiene valor entero en x e y
% o por si queda fuera del frame
X = correccion (X, resolucion);
% --------------NO COMENTAR PARA VERSION CON GUI------------%
% % Actualizo numero de part�culas si las cambie en la GUI
% if (N_part_nuevas ~= N_particulas)
% [X, W] = variar_N_particulas(X, W, N_part_nuevas, N_part_max, resolucion);
% end
% ----------------------------------------------------------%
% Limpiar datos
% flushdata(video);
end
%% Cierre de video y limpieza de memoria
% Limpiar datos
%-EJECUTAR ESTOS COMANDOS SI PONEN CONTROL-C EN LA LINEA DE COMANDOS-%
close(figure(1));
%flushdata(video);
stop(video)
%delete(video)
%clear video