mirror of
https://github.com/modelec/detection_pot.git
synced 2026-01-18 16:47:33 +01:00
138 lines
5.5 KiB
Python
138 lines
5.5 KiB
Python
import cv2 as cv
|
|
import cv2.aruco as aruco
|
|
import numpy as np
|
|
|
|
# Focal de la cam
|
|
# Calculer la focal avec le fichier get_the_focal.py
|
|
# FOCAL_LENGTH = 1444 # Téléphone Maxime pixel 7 pro
|
|
FOCAL_LENGTH = 1470
|
|
# FOCAL_LENGTH = 600
|
|
|
|
arucoTagMapping = {
|
|
47: ["Panneau solaire", 3.62],
|
|
36: ["Fleur blanche", 2],
|
|
13: ["FLeur violette", 2]
|
|
}
|
|
|
|
# Charger une image depuis la caméra (0 pour la caméra par défaut, généralement la webcam)
|
|
cap = cv.VideoCapture(0)
|
|
|
|
# Définir le dictionnaire ArUco
|
|
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
|
|
|
|
# Créer le détecteur ArUco
|
|
parameters = aruco.DetectorParameters()
|
|
|
|
#
|
|
detector = aruco.ArucoDetector(aruco_dict, parameters)
|
|
|
|
while True:
|
|
|
|
# Lire une image depuis la caméra
|
|
ret, frame = cap.read()
|
|
|
|
# Convertir l'image en niveaux de gris
|
|
gray = cv.cvtColor(frame, cv.COLOR_BGR2GRAY)
|
|
|
|
# Détecter les marqueurs ArUco
|
|
corners, ids, rejectedImgPoints = detector.detectMarkers(gray)
|
|
|
|
# Dessiner les résultats
|
|
if ids is not None:
|
|
# aruco.drawDetectedMarkers(frame, corners, ids)
|
|
|
|
for i in range(len(ids)):
|
|
|
|
if len(corners) > 0:
|
|
ids = ids.flatten()
|
|
for (markerCorner, markerID) in zip(corners, ids):
|
|
|
|
if markerID not in arucoTagMapping:
|
|
continue
|
|
|
|
try:
|
|
tagCorners = markerCorner.reshape((4, 2))
|
|
except:
|
|
print("Error with the corners")
|
|
continue
|
|
|
|
(topLeft, topRight, bottomRight, bottomLeft) = tagCorners
|
|
|
|
topRight = (int(topRight[0]), int(topRight[1]))
|
|
topLeft = (int(topLeft[0]), int(topLeft[1]))
|
|
bottomRight = (int(bottomRight[0]), int(bottomRight[1]))
|
|
bottomLeft = (int(bottomLeft[0]), int(bottomLeft[1]))
|
|
|
|
center_x = int((topLeft[0] + topRight[0] + bottomRight[0] + bottomLeft[0]) / 4)
|
|
center_y = int((topLeft[1] + topRight[1] + bottomRight[1] + bottomLeft[1]) / 4)
|
|
|
|
# Calculate the apparent width in pixels
|
|
P = np.sqrt((topRight[0] - topLeft[0]) ** 2 + (topRight[1] - topLeft[1]) ** 2)
|
|
|
|
# Calculate the distance to the Aruco tag
|
|
D = (arucoTagMapping[markerID][1] * FOCAL_LENGTH) / P
|
|
|
|
tag_position_x = (topRight[0] + topLeft[0]) / 2
|
|
tag_position_y = (topRight[1] + bottomRight[1]) / 2
|
|
|
|
image_width = frame.shape[1]
|
|
image_height = frame.shape[0]
|
|
|
|
# Calcul de l'angle horizontal par rapport au centre de l'image
|
|
angle_to_tag_horizontal = np.arctan2(tag_position_x - (image_width / 2),
|
|
image_width / (2 * np.tan(np.radians(60))))
|
|
|
|
# Conversion de l'angle en degrés
|
|
angle_to_tag_horizontal_deg = np.degrees(angle_to_tag_horizontal)
|
|
|
|
# Calculate the difference in the x and y coordinates
|
|
dx = topRight[0] - topLeft[0]
|
|
dy = topRight[1] - topLeft[1]
|
|
|
|
# Calculate the angle in radians
|
|
angle_radians = np.arctan2(dy, dx)
|
|
|
|
# Convert the angle to degrees
|
|
angle_degrees = np.degrees(angle_radians)
|
|
|
|
# Print the rotation angle of the ArUco tag
|
|
sens_du_tag = ""
|
|
if -15 < angle_degrees < 15:
|
|
sens_du_tag = "debout"
|
|
elif 15 < angle_degrees < 60:
|
|
sens_du_tag = "penche a droite"
|
|
elif 60 < angle_degrees < 110:
|
|
sens_du_tag = "tombe a droite"
|
|
elif 110 < angle_degrees < 180:
|
|
sens_du_tag = "a l'envers"
|
|
elif -180 < angle_degrees < -110:
|
|
sens_du_tag = "a l'envers"
|
|
elif -110 < angle_degrees < -60:
|
|
sens_du_tag = "tombe a gauche"
|
|
elif -60 < angle_degrees < -15:
|
|
sens_du_tag = "penche a gauche"
|
|
|
|
# Affichage des informations
|
|
cv.putText(frame, f"{arucoTagMapping[markerID][0]}, id : {markerID}", (topLeft[0], topLeft[1] - 90),
|
|
cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 3)
|
|
distance_str = "{:.2f}".format(D)
|
|
cv.putText(frame, f"Distance : {distance_str} cm", (topLeft[0], topLeft[1] - 60),
|
|
cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 3)
|
|
angle_str = "{:.2f}".format(angle_to_tag_horizontal_deg)
|
|
cv.putText(frame, f"Angle horizontale : {angle_str} degrees", (topLeft[0], topLeft[1] - 30),
|
|
cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 3)
|
|
cv.putText(frame, f"Le tag est {sens_du_tag}", (topLeft[0], topLeft[1]),
|
|
cv.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 3)
|
|
|
|
# print(f"{arucoTagMapping[markerID][0]} : {angle_to_tag_horizontal_deg} degrees, {D} cm")
|
|
# Afficher l'image
|
|
cv.imshow('Frame', frame)
|
|
|
|
# Sortir de la boucle si la touche 'q' est enfoncée
|
|
if cv.waitKey(1) & 0xFF == ord('q'):
|
|
break
|
|
|
|
# Libérer la capture et fermer la fenêtre
|
|
cap.release()
|
|
cv.destroyAllWindows()
|