diff --git a/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/IMAGES/calib.jpg b/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/IMAGES/calib.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..2ee1a2add14b6a56aa6664418b9c60952c162702
Binary files /dev/null and b/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/IMAGES/calib.jpg differ
diff --git a/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/IMAGES/cone_detect.jpg b/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/IMAGES/cone_detect.jpg
deleted file mode 100644
index eb9b0793d8b024737cb172cb3f46ce16e70bce3b..0000000000000000000000000000000000000000
Binary files a/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/IMAGES/cone_detect.jpg and /dev/null differ
diff --git a/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/MCL.py b/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/MCL.py
index 76924772568317137ffe7d47bda9e5368e450b34..4838cc52ca89390ea7c9f9e00d1e35aa4414baf2 100644
--- a/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/MCL.py
+++ b/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/MCL.py
@@ -1,9 +1,17 @@
-import random as rd
-from math import cos, sin
+import os
+os.environ['CUDA_VISIBLE_DEVICES'] = '0'
+import cv2
import numpy as np
+import tensorflow as tf
+from yolov3.utils import detect_image, detect_realtime, detect_video, Load_Yolo_model, detect_video_realtime_mp
+from yolov3.configs import *
import matplotlib.pyplot as plt
-from yolov3.utils import detect_image, detect_realtime, detect_video, Load_Yolo_model, detect_video_realtime_mp
+import random as rd
+from math import cos, sin, tan, pi
+
+
+yolo = Load_Yolo_model()
### Carte ###
@@ -22,13 +30,16 @@ sigma_position = 0.02
sigma_direction = 0.05
-dep_x, dep_y, dep_theta = 0,0,0
+dep_x, dep_y, dep_theta = 1.314, 1.162, 3.14/3.6
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
+F = 3423 #Focale camera
+h_reel = 0.234 #Hauteur d'un plot
+y_0 = 1500 #Milieu de l'image
+
+dist_roue = 0.2 #Distance entre les roues avant et les roues arrieres
+dt = 1 #Frequence de calcul
# class position:
@@ -47,12 +58,21 @@ def distance(x_1, y_1, x_2, y_2):
def boite2coord(x1,y1,x2,y2):
d = F*h_reel/abs(y1-y2)
- y_r = -((x1+x2)/2 - y0)*d/F
+ y_r = -((x1+x2)/2 - y_0)*d/F
sin_theta= y_r/d
x_r = d*(1-sin_theta**2)**0.5
return x_r,y_r
+def rotation(point, centre, angle):
+
+ x1,y1 = point
+ x2,y2 = centre
+
+ new_x = (x1-x2)*cos(angle) - (y1-y2)*sin(angle) + x2
+ new_y = (x1-x2)*sin(angle) + (y1-y2)*cos(angle) + y2
+
+ return new_x, new_y
def distance_Chamfer(A_x, A_y, B_x, B_y):
m = len(A_x)
@@ -76,9 +96,30 @@ 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+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)
+
+ if direction ==0:
+ new_x = x + dt*vitesse*cos(theta)
+ new_y = y + dt*vitesse*sin(theta)
+ new_theta = theta
+
+ else:
+ R = dist_roue/tan(direction)
+ angle_rotation = dt*vitesse/R
+
+ if direction>0:
+ centre_rotation_x = x + R*sin(theta)
+ centre_rotation_y = y - R*cos(theta)
+ else:
+ centre_rotation_x = x - R*sin(theta)
+ centre_rotation_y = y + R*cos(theta)
+
+ new_x, new_y = rotation((x,y),(centre_rotation_x, centre_rotation_y), angle_rotation)
+ new_theta = theta + angle_rotation
+
+ new_x += rd.gauss(0, sigma_position)
+ new_y += rd.gauss(0, sigma_position)
+ new_theta += rd.gauss(0, sigma_direction)
+
return (new_x, new_y, new_theta)
@@ -153,18 +194,23 @@ def low_variance_resampling(X,W):
return X_t
-def get_position(pos, u_t):
+def get_position(u_t,pos):
+
+
+ ### Récupération de la photo
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)
+ # Positionnement des plots à partir des boites
+
liste_x =[]
liste_y =[]
for i in boxes:
@@ -177,48 +223,58 @@ def get_position(pos, u_t):
z_t.append((liste_x[i], liste_y[i]))
+ #Positionnement
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]
+ for i in range(len(pos)):
+ pos_calc[0] += pos[i][0]*W[i]
+ pos_calc[1] += pos[i][1]*W[i]
+ pos_calc[2] += pos[i][2]*W[i]
- return pos_calc
+ return pos_calc, pos
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,
- 0.23060853761333963,
- 0.5278583908503303,
- 0.14161368355256793,
- 0.5134652832573952]
-
- liste_y = [0.14021924676581576,
- -0.3119493901540909,
- -0.3464004029844368,
- 0.01390277627039628,
- -0.2754514724880131,
- -0.5902545559074325]
-
- observation = []
- for i in range(len(liste_x)):
- observation.append((liste_x[i], liste_y[i]))
+ # 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,
+ # 0.23060853761333963,
+ # 0.5278583908503303,
+ # 0.14161368355256793,
+ # 0.5134652832573952]
+
+ # liste_y = [0.14021924676581576,
+ # -0.3119493901540909,
+ # -0.3464004029844368,
+ # 0.01390277627039628,
+ # -0.2754514724880131,
+ # -0.5902545559074325]
+
+ # observation = []
+ # for i in range(len(liste_x)):
+ # observation.append((liste_x[i], liste_y[i]))
+
+ commande = (0.138841, -pi/6)
+
+ # a,W = particle_filter(pos, commande, observation)
+ #print(a,W)
+
+
+
- commande = (0.138841, -0.)
- a,W = particle_filter(pos, commande, observation)
- print(a,W)
pos_initiale = (1.314, 1.162, 3.14/3.6)
pos_finale = (1.4, 1.271, 3.14/5.5)
+ pos = [pos_initiale for i in range(30)]
+
+ pos_calc,pos = get_position(commande,pos)
+
plt.figure(1)
plt.plot(pos_initiale[0], pos_initiale[1], 'o', label='Position initale')
@@ -231,20 +287,18 @@ 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]])
+ for k in range(len(pos)):
+ i = pos[k]
+ pos_x.append(i[0])
+ pos_y.append(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]
+ # 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]))
diff --git a/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/yolov3/__pycache__/pid_controller.cpython-38.pyc b/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/yolov3/__pycache__/pid_controller.cpython-38.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..37d12133a8990982f60a45e2d1061b6c78bd8d7b
Binary files /dev/null and b/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/yolov3/__pycache__/pid_controller.cpython-38.pyc differ
diff --git a/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/yolov3/__pycache__/track_2_generator.cpython-38.pyc b/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/yolov3/__pycache__/track_2_generator.cpython-38.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..62af523bf58ddf013f236695e14d99fc2abd4ca9
Binary files /dev/null and b/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/yolov3/__pycache__/track_2_generator.cpython-38.pyc differ
diff --git a/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/yolov3/__pycache__/utils.cpython-38.pyc b/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/yolov3/__pycache__/utils.cpython-38.pyc
index 316235f8d585a76cfff84349b6803a28d8781c6a..3609578e8506cb257ed1eb0623381c424e3a9f97 100644
Binary files a/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/yolov3/__pycache__/utils.cpython-38.pyc and b/PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/yolov3/__pycache__/utils.cpython-38.pyc differ
diff --git a/PAR 152/carte_cones.py b/PAR 152/carte_cones.py
index f584b93fd0c3f559e577d31d13c39fa52c04af8d..b3bc1512939e17d085cca5f1421d6dc37e4b786a 100644
--- a/PAR 152/carte_cones.py
+++ b/PAR 152/carte_cones.py
@@ -6,7 +6,51 @@ Created on Fri Jan 27 11:53:00 2023
"""
import matplotlib.pyplot as plt
-from math import cos,sin
+from math import cos,sin,tan
+import numpy as np
+
+dist_roue = 0.2 #Distance entre les roues avant et les roues arrieres
+dt = 1 #Frequence de calcul
+
+
+def rotation(point, centre, angle):
+
+ x1,y1 = point
+ x2,y2 = centre
+
+ new_x = (x1-x2)*cos(angle) - (y1-y2)*sin(angle) + x2
+ new_y = (x1-x2)*sin(angle) + (y1-y2)*cos(angle) + y2
+
+ return new_x, new_y
+
+def motion_update(commande, position):
+ vitesse, direction = commande
+ x,y,theta = position
+
+ if direction ==0:
+ new_x = x + dt*vitesse*cos(theta)
+ new_y = y + dt*vitesse*sin(theta)
+ new_theta = theta
+
+ else:
+ R = dist_roue/tan(direction)
+ angle_rotation = dt*vitesse/R
+
+ if direction>0:
+ centre_rotation_x = x + R*sin(theta)
+ centre_rotation_y = y - R*cos(theta)
+ else:
+ centre_rotation_x = x - R*sin(theta)
+ centre_rotation_y = y + R*cos(theta)
+
+ circle = np.linspace(0,2*np.pi, 150)
+ a = R*np.cos(circle) + centre_rotation_x
+ b = R*np.sin(circle) + centre_rotation_y
+ plt.plot(a,b)
+ new_x, new_y = rotation((x,y),(centre_rotation_x, centre_rotation_y), angle_rotation)
+ new_theta = theta + angle_rotation
+
+ return [(new_x, new_y, new_theta), (centre_rotation_x, centre_rotation_y)]
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]
@@ -39,22 +83,31 @@ plt.figure(1)
plt.plot(coord_x_ext, coord_y_ext,'+')
plt.plot(coord_x_int, coord_y_int,'+')
plt.arrow(x,y,0.1*cos(theta),0.1*sin(theta), width=0.01)
-plt.show()
+tab = motion_update((0.1,-3.14/6), (x,y,theta))
+x2,y2,theta2 = tab[0]
+x_r, y_r = tab[1]
-cones_vu_x = []
-cones_vu_y = []
-for i in range(len(vision_x)):
- if vision_x[i]>0 and abs(vision_y[i])<vision_x[i]:
- cones_vu_x.append(vision_x[i])
- cones_vu_y.append(vision_y[i])
+plt.arrow(x2,y2,0.1*cos(theta2),0.1*sin(theta2), width=0.01)
+plt.plot(x_r,y_r,'o')
-for i in range(len(vision_x_ext)):
- if vision_x_ext[i]>0 and abs(vision_y_ext[i])<vision_x_ext[i]:
- cones_vu_x.append(vision_x_ext[i])
- cones_vu_y.append(vision_y_ext[i])
-
-plt.figure(2)
-plt.plot(cones_vu_x, cones_vu_y, '+')
-plt.arrow(0,0,0.1,0,width=0.01)
-plt.show()
\ No newline at end of file
+plt.show()
+
+# =============================================================================
+# cones_vu_x = []
+# cones_vu_y = []
+# for i in range(len(vision_x)):
+# if vision_x[i]>0 and abs(vision_y[i])<vision_x[i]:
+# cones_vu_x.append(vision_x[i])
+# cones_vu_y.append(vision_y[i])
+#
+# for i in range(len(vision_x_ext)):
+# if vision_x_ext[i]>0 and abs(vision_y_ext[i])<vision_x_ext[i]:
+# cones_vu_x.append(vision_x_ext[i])
+# cones_vu_y.append(vision_y_ext[i])
+#
+# plt.figure(2)
+# plt.plot(cones_vu_x, cones_vu_y, '+')
+# plt.arrow(0,0,0.1,0,width=0.01)
+# plt.show()
+# =============================================================================