diff --git a/PAR 152/MCL.py b/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/MCL.py
similarity index 61%
rename from PAR 152/MCL.py
rename to PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/MCL.py
index 8e8302c38a55c19586d1c1ca4541aff11fda8983..76924772568317137ffe7d47bda9e5368e450b34 100644
--- a/PAR 152/MCL.py
+++ b/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/MCL.py
@@ -3,6 +3,8 @@ from math import cos, sin
import numpy as np
import matplotlib.pyplot as plt
+from yolov3.utils import detect_image, detect_realtime, detect_video, Load_Yolo_model, detect_video_realtime_mp
+
### Carte ###
pixel_x_ext, pixel_y_ext = [242, 220, 165, 110, 63, 33, 22, 34, 63, 110, 165, 220, 243, 310, 334, 388, 443, 490, 521, 531, 520, 489, 443, 388, 333, 310], [76, 64, 52, 64, 95, 141, 196, 252, 298, 330, 340, 328, 318, 316, 328, 339, 329, 298, 251, 196, 142, 95, 64, 53, 64, 77]
@@ -20,6 +22,20 @@ sigma_position = 0.02
sigma_direction = 0.05
+dep_x, dep_y, dep_theta = 0,0,0
+nb_particule = 30
+pos = [[dep_x, dep_y, dep_theta] for i in range(nb_particule)]
+
+F = 1000 #Focale camera
+h_reel = 0.2 #Hauteur d'un plot
+y_0 = 2048/2 #Milieu de l'image
+
+
+# class position:
+
+# def __init__(self, pos_x, pos_y, nb_particule):
+# self.__pos = [pos_x, pos_y]*nb_particule
+
def normalisation(W):
a = sum(W)
@@ -28,10 +44,23 @@ def normalisation(W):
def distance(x_1, y_1, x_2, y_2):
return np.sqrt((x_1-x_2)**2 + (y_1-y_2)**2)
+def boite2coord(x1,y1,x2,y2):
+
+ d = F*h_reel/abs(y1-y2)
+ y_r = -((x1+x2)/2 - y0)*d/F
+ sin_theta= y_r/d
+ x_r = d*(1-sin_theta**2)**0.5
+
+ return x_r,y_r
+
+
def distance_Chamfer(A_x, A_y, B_x, B_y):
m = len(A_x)
n = len(B_x)
+ if m==0 or n==0:
+ return 0.0001
+
res = 0
tab = [[distance(A_x[i], A_y[i], B_x[j], B_y[j]) for i in range(m)] for j in range(n)]
@@ -47,9 +76,9 @@ def distance_Chamfer(A_x, A_y, B_x, B_y):
def motion_update(commande, position):
vitesse, direction = commande
x,y,theta = position
- new_x = x + vitesse*cos(direction) + rd.gauss(0, sigma_position)
- new_y = y + vitesse*sin(direction) + rd.gauss(0, sigma_position)
- new_theta = theta - direction + rd.gauss(0, sigma_direction)
+ new_x = x + vitesse*cos(direction+theta) + rd.gauss(0, sigma_position)
+ new_y = y + vitesse*sin(direction+theta) + rd.gauss(0, sigma_position)
+ new_theta = theta + direction + rd.gauss(0, sigma_direction)
return (new_x, new_y, new_theta)
@@ -123,8 +152,46 @@ def low_variance_resampling(X,W):
X_t.append(X[i])
return X_t
+
+def get_position(pos, u_t):
+
+ image_path = "./IMAGES/IMG_2345.JPEG"
+ video_path = "./IMAGES/test.mp4"
+
+ yolo = Load_Yolo_model()
+ boxes = detect_image(yolo, image_path, "./IMAGES/111.jpg", input_size=YOLO_INPUT_SIZE, show=True, CLASSES=TRAIN_CLASSES, rectangle_colors=(255,0,0))
+ #detect_video(yolo, video_path, './IMAGES/detected.mp4', input_size=YOLO_INPUT_SIZE, show=False, CLASSES=TRAIN_CLASSES, rectangle_colors=(255,0,0))
+ #detect_realtime(yolo, '', input_size=YOLO_INPUT_SIZE, show=True, CLASSES=TRAIN_CLASSES, rectangle_colors=(255, 0, 0))
+ #detect_video_realtime_mp(video_path, "Output.mp4", input_size=YOLO_INPUT_SIZE, show=True, CLASSES=TRAIN_CLASSES, rectangle_colors=(255,0,0), realtime=False)
+
+
+ liste_x =[]
+ liste_y =[]
+ for i in boxes:
+ x,y = boite2coord(i[0],i[1],i[2],i[3])
+ liste_x.append(x)
+ liste_y.append(y)
+
+ z_t = []
+ for i in range(len(liste_x)):
+ z_t.append((liste_x[i], liste_y[i]))
+
+
+ pos, W = particle_filter(pos, u_t, z_t)
+
+ pos_calc = [0,0,0]
+ for i in range(len(a)):
+ pos_calc[0] += a[i][0]*W[i]
+ pos_calc[1] += a[i][1]*W[i]
+ pos_calc[2] += a[i][2]*W[i]
+
+ return pos_calc
+
+
+
if __name__ == "__main__":
pos = [(1.314, 1.162, 3.14/3.6) for i in range(10)]
+ #pos = [(2.5*rd.random(), 1.5*rd.random(), 6.28*rd.random()) for i in range(50)]
liste_x = [0.37409758380473157,
0.6517064494114153,
@@ -144,7 +211,7 @@ if __name__ == "__main__":
for i in range(len(liste_x)):
observation.append((liste_x[i], liste_y[i]))
- commande = (0.138841, 0.3)
+ commande = (0.138841, -0.)
a,W = particle_filter(pos, commande, observation)
print(a,W)
@@ -154,9 +221,9 @@ if __name__ == "__main__":
plt.figure(1)
- plt.plot(pos_initiale[0], pos_initiale[1], 'o')
+ plt.plot(pos_initiale[0], pos_initiale[1], 'o', label='Position initale')
plt.arrow(pos_initiale[0], pos_initiale[1], 0.07*cos(pos_initiale[2]), 0.07*sin(pos_initiale[2]))
- plt.plot(pos_finale[0], pos_finale[1], 'o')
+ plt.plot(pos_finale[0], pos_finale[1], 'o', label='Position finale')
plt.arrow(pos_finale[0], pos_finale[1], 0.07*cos(pos_finale[2]), 0.07*sin(pos_finale[2]))
plt.plot(coord_x_ext, coord_y_ext,'+')
@@ -164,14 +231,24 @@ if __name__ == "__main__":
pos_x = []
pos_y = []
- for k in range(len(a)):
- i = a[k]
- pos_x.append(i[0])
- pos_y.append(i[1])
- plt.arrow(i[0], i[1], 0.07*cos(i[2]), 0.07*sin(i[2]))
- plt.annotate(W[k], [i[0], i[1]])
-
- plt.plot(pos_x,pos_y,'.')
+ #for k in range(len(a)):
+ #i = a[k]
+ #pos_x.append(i[0])
+ #pos_y.append(i[1])
+ #plt.arrow(i[0], i[1], 0.07*cos(i[2]), 0.07*sin(i[2]))
+ #plt.annotate(W[k], [i[0], i[1]])
+
+ plt.plot(pos_x,pos_y,'x', label='Positions possibles')
+
+ pos_calc = [0,0,0]
+ for i in range(len(a)):
+ pos_calc[0] += a[i][0]*W[i]
+ pos_calc[1] += a[i][1]*W[i]
+ pos_calc[2] += a[i][2]*W[i]
+
+ plt.plot(pos_calc[0], pos_calc[1], 'o', label='Moyenne pondérée des positions calculées')
+ plt.arrow(pos_calc[0], pos_calc[1], 0.07*cos(pos_calc[2]), 0.07*sin(pos_calc[2]))
+ plt.legend()
plt.show()
\ No newline at end of file