Merge branch 'main' of https://gitlab.ec-lyon.fr/epsa-personnal-git/epsa-pae-ai-self-driving

PAR 152/positionnement.py

+from scipy.cluster.hierarchy import linkage, fcluster, dendrogram
+from numpy import array
+import numpy as np
+from math import sqrt
+import matplotlib.pyplot as plt
+
+carte = [] #carte contenant les coordonnées (x,y) de chaque plot, l'indince i correspond au plot i
+carte = np.array(carte)
+
+
+
+boxes = [array([ 71.55448151,  48.70854568, 117.95120239, 128.82731628,
+         0.96357524,   0.        ]), array([265.82519531,  63.02380371, 317.72790527, 161.43673706,
+         0.95850408,   0.        ]), array([204.24989319, 124.79711151, 320.40344238, 312.65106201,
+         0.95731342,   0.        ]), array([392.64465332, 120.61515808, 507.        , 307.8104248 ,
+         0.95594758,   0.        ]), array([467.19827271,  83.70771027, 507.        , 204.51119995,
+         0.95114303,   0.        ]), array([183.88371277,  18.13962936, 212.38748169,  72.65458679,
+         0.94486743,   0.        ]), array([467.60531616,  21.23407555, 501.08724976,  79.89296722,
+         0.93922979,   0.        ]), array([ 39.37908936, 126.7710495 , 137.76670837, 320.09881592,
+         0.93714899,   0.        ]), array([346.33688354,  74.00056458, 411.95550537, 180.61778259,
+         0.9355219 ,   0.        ]), array([198.56393433,  60.01593781, 251.088974  , 148.07209778,
+         0.93453765,   0.        ]), array([107.80426025,  17.43326759, 135.90390015,  67.50372314,
+         0.92372549,   0.        ]), array([226.01693726,  20.78704262, 255.77336121,  71.31395721,
+         0.92241275,   0.        ]), array([  0.73022616,  43.94894028,  50.41823196, 118.34261322,
+         0.92166394,   0.        ]), array([413.37960815,  21.73137093, 447.26065063,  77.86586761,
+         0.91958499,   0.        ]), array([135.58837891,  52.80698395, 189.51263428, 134.50894165,
+         0.91850674,   0.        ]), array([366.38388062,  22.89640999, 395.97808838,  77.14238739,
+         0.91489226,   0.        ]), array([23.01332664, 17.67047882, 51.43401337, 67.86820221,  0.91442037,
+        0.        ]), array([154.2374115 ,  88.37498474, 231.43321228, 213.48825073,
+         0.9137339 ,   0.        ]), array([ 22.97482491,  88.09642029,  71.89588165, 214.76116943,
+         0.90880054,   0.        ]), array([63.18217087, 15.96899509, 91.19147491, 69.62237549,  0.90012252,
+        0.        ]), array([12.51589012, 22.42526627, 51.88586807, 94.58940887,  0.38236487,
+        0.        ])]   
+milieu = 2
+F = 500
+h_reel = 0.7
+y0 = (boxes[milieu][0] + boxes[milieu][2])/2
+
+seuil = 0.5
+
+def distance(pt1,pt2):
+    return sqrt((pt1[0]-pt2[0])**2 + (pt1[1]-pt2[1])**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 changement_referentiel(coord_voiture, coord_plot):
+    theta = coord_voiture[2]
+    x_v = coord_plot[0] - coord_voiture[0]
+    y_v = coord_plot[1] - coord_voiture[1]
+    x = x_v*np.cos(theta) + y_v*np.sin(theta)
+    y = x_v*np.sin(theta) + y_v*np.cos(theta)                       
+    return [x,y]
+
+
+
+
+def get_groupe_max(cluster):
+    occ = [0 for i in range(max(cluster)+1)]
+    print(occ)
+    for i in cluster:
+        occ[i] += 1
+    return np.argmax(occ)
+
+def association(cone, old_pos_cone):
+    dist = []
+    for pt in old_pos_cone:
+        dist.append(distance(cone,pt))
+    res = np.argmin(dist)
+    if dist[res]<seuil:
+        return res
+    else:
+        return None
+    
+    
+
+def identification_cones(old_pos, old_pos_cone, new_pos_cone, ind_cone):
+    #Regrouper les cones par deux si possible
+    
+    correspondance = [-1 for i in range(len(new_pos_cone))]
+
+    for i in range(len(new_pos_cone)):
+        cone = association(new_pos_cone[i], old_pos_cone)
+        if cone != None:
+            correspondance[i] = cone
+    
+    liste_ind_cone = [-1]*len(new_pos_cone)
+    for i in range(len(new_pos_cone)):
+            if correspondance[i] != -1:
+                liste_ind_cone[i] = ind_cone[correspondance[i]]
+    
+    new_pos = positionnement(new_pos_cone, liste_ind_cone) #permet de renvoyer ls cones
+    
+    for i in range(len(liste_ind_cone)):
+        if liste_ind_cone[i] == -1:
+            liste_ind_cone[i] = identification_new(new_pos, i)
+    
+    new_pos = positionnement(new_pos_cone, liste_ind_cone)
+    
+    return new_pos, liste_ind_cone
+
+
+
+
+def identification_new(pos, pos_cone):
+    
+    new_pos_cone = changement_referentiel(pos, pos_cone)
+
+    dist = []
+    for cone in carte:
+        dist.append(distance(cone,new_pos_cone))
+    res = np.argmin(dist)
+
+    return res
+
+
+
+def positionnement(new_pos_cone, liste_ind_cone):
+    pass
+    
+    
+    
+
+    
+
+if __name__ == '__main__':
+    
+    #initialisation
+    pos = [0,0,0]
+    old_pos_cone = []
+    ind_cone = []
+    
+    #boucle
+    pos_cone = []
+    for i in boxes:
+        pos_cone = boite2coord(i[0],i[1],i[2],i[3])
+    pos, liste_ind_cone = identification_cones(pos, old_pos_cone, pos_cone, ind_cone)
+    old_pos_cone = pos_cone
+    
+    
+            
\ No newline at end of file