tjygnhfgtr

PAR 152/Yolo V3/TensorFlow-2.x-YOLOv3-master/MCL.py

@@ -98,6 +98,18 @@ def distance_Chamfer(A_x, A_y, B_x, B_y):
     return res
 
 
+def orientation_voiture(x,y):
+    x1,y1 = x-centre_x, y-centre_y
+    if x<0:
+        x2,y2 = x1+diametre/2, y1
+        theta = atan(y2/x2)
+        return theta-3.1415/2
+    else:
+        x2,y2 = x1-diametre/2, y1
+        theta = atan(y2/x2)
+        return theta-3.1415/2
+    
+    
 def motion_update(commande, position):
     vitesse, direction, FPS = commande
     x,y,theta = position
@@ -121,7 +133,7 @@ def motion_update(commande, position):
             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_theta = orientation_voiture(new_x, new_y)
     
     new_x += rd.gauss(0, sigma_position)
     new_y += rd.gauss(0, sigma_position)
@@ -243,16 +255,6 @@ def get_position(boxes,commande,pos):
     
     return pos_calc, pos, cone_x, cone_y
 
-def orientation_voiture(x,y):
-    x1,y1 = x-centre_x, y-centre_y
-    if x<0:
-        x2,y2 = x1+diametre/2, y1
-        theta = atan(y2/x2)
-        return theta-3.1415/2
-    else:
-        x2,y2 = x1-diametre/2, y1
-        theta = atan(y2/x2)
-        return theta-3.1415/2
         
 
 if __name__ == "__main__":