68 lines
1.8 KiB
Python
68 lines
1.8 KiB
Python
import cv2
|
|
import pyapriltags
|
|
|
|
at_detector = pyapriltags.Detector(families="tag36h11")
|
|
|
|
|
|
def open_camera():
|
|
# cap = cv2.VideoCapture(config.usb_camera_device)
|
|
cap = cv2.VideoCapture("http://localhost:50085/video")
|
|
|
|
if not cap.isOpened():
|
|
print("Cannot open camera")
|
|
exit()
|
|
|
|
# cv2.namedWindow("Object and AprilTag Detection", cv2.WINDOW_NORMAL)
|
|
# cv2.resizeWindow("Object and AprilTag Detection", 800, 600)
|
|
|
|
while True:
|
|
print("Frame")
|
|
|
|
ret, frame = cap.read()
|
|
|
|
if not ret:
|
|
break
|
|
|
|
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
|
at_results = at_detector.detect(gray)
|
|
|
|
for i, at_result in enumerate(at_results):
|
|
# print(
|
|
# f"Erkannter AprilTag {i + 1}: ID={at_result.tag_id}, Koordinaten={at_result}")
|
|
|
|
apriltag_x = 890
|
|
apriltag_y = 500
|
|
|
|
print(
|
|
f"ID: {at_result.tag_id} Corners: {at_result.center}")
|
|
|
|
corner_x = at_result.center[0]
|
|
corner_y = at_result.center[1]
|
|
|
|
offset = 5
|
|
|
|
if corner_x > apriltag_x + offset:
|
|
print("move right")
|
|
|
|
if corner_x < apriltag_x - offset:
|
|
print("move left")
|
|
|
|
if corner_y > apriltag_y + offset:
|
|
print("move back")
|
|
|
|
if corner_y < apriltag_y - offset:
|
|
print("move forward")
|
|
|
|
# for at_result in at_results:
|
|
# corners = at_result.corners.astype(int)
|
|
# cv2.polylines(frame, [corners], isClosed=True,
|
|
# color=(0, 255, 255), thickness=2)
|
|
|
|
# cv2.imshow("Object and AprilTag Detection", frame)
|
|
|
|
# if cv2.waitKey(1) & 0xFF == ord('q'):
|
|
# break
|
|
|
|
# cap.release()
|
|
# cv2.destroyAllWindows()
|