Координаты ArUco-метки

from pioneer_sdk2 import Camera, ImageViewer # импортируем классы Camera и ImageViewer из библиотеки pioneer_sdk2
import cv2                                  # библиотека cv2 содержит функции для работы с изображениями
import numpy as np                          # библиотека numpy нужна для работы с массивами
from pathlib import Path                    # класс Path нужен для работы с путем к файлу калибровки


DATA_PATH = Path(__file__).with_name("data.yml") # путь к файлу калибровки рядом со скриптом


def load_coefficients(path):               # функция для загрузки коэффициентов калибровки камеры
    cv_file = cv2.FileStorage(str(path), cv2.FILE_STORAGE_READ) # открываем файл для чтения данных калибровки

    if not cv_file.isOpened():             # проверяем, что файл калибровки удалось открыть
        raise FileNotFoundError("Не удалось открыть файл калибровки: " + str(path)) # сообщаем, какой файл не найден

    camera_matrix = cv_file.getNode("mtx").mat() # считываем матрицу камеры из файла
    dist_coeffs = cv_file.getNode("dist").mat()  # считываем коэффициенты искажений из файла

    cv_file.release()                      # закрываем файл после чтения

    if camera_matrix is None or dist_coeffs is None: # проверяем, что файл содержит нужные данные
        raise ValueError("Файл калибровки не содержит mtx или dist") # сообщаем о неверном файле калибровки

    return camera_matrix, dist_coeffs      # возвращаем матрицу камеры и коэффициенты искажений


aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_ARUCO_ORIGINAL) # выбираем словарь ArUco-меток
aruco_params = cv2.aruco.DetectorParameters()                         # создаем параметры для поиска ArUco-меток
aruco_detector = cv2.aruco.ArucoDetector(aruco_dict, aruco_params)    # создаем детектор ArUco-меток

camera_matrix, dist_coeffs = load_coefficients(DATA_PATH) # загружаем коэффициенты калибровки камеры

size_of_marker = 0.05                    # задаем размер стороны ArUco-метки в метрах

points_of_marker = np.array([            # задаем координаты углов ArUco-метки
    (size_of_marker / 2, -size_of_marker / 2, 0),
    (-size_of_marker / 2, -size_of_marker / 2, 0),
    (-size_of_marker / 2, size_of_marker / 2, 0),
    (size_of_marker / 2, size_of_marker / 2, 0)
], dtype=np.float32)                      # используем тип float32, который подходит для функций OpenCV

camera = Camera()                         # создаем экземпляр класса Camera для получения кадров с камеры
viewer = ImageViewer()                    # создаем экземпляр класса ImageViewer для трансляции изображения

try:                                      # основной код находится внутри блока try
    while True:                           # запускаем бесконечный цикл
        frame = camera.get_cv_frame(timeout=1.0) # получаем один кадр с камеры
                                                 # timeout=1.0 - время ожидания кадра в секундах

        if frame is None:                 # проверяем, что кадр не был получен
            continue                      # пропускаем текущую итерацию цикла

        corners, ids, rejected = aruco_detector.detectMarkers(frame) # ищем ArUco-метки на изображении

        if ids is not None and len(corners) > 0: # проверяем, что хотя бы одна ArUco-метка найдена
            image_points = corners[0].reshape(-1, 2) # получаем координаты углов первой найденной метки

            success, rvecs, tvecs = cv2.solvePnP(points_of_marker, image_points, camera_matrix, dist_coeffs) # считаем положение метки

            if success:                   # проверяем, что положение метки успешно рассчитано
                print(
                    f"x={tvecs.item(0):.2f},", # координата метки по оси X
                    f"y={tvecs.item(1):.2f},", # координата метки по оси Y
                    f"z={tvecs.item(2):.2f}"   # координата метки по оси Z
                )                         # выводим координаты ArUco-метки в терминал

                cv2.drawFrameAxes(frame, camera_matrix, dist_coeffs, rvecs, tvecs, 0.1) # рисуем оси координат метки

        cv2.aruco.drawDetectedMarkers(frame, corners, ids) # рисуем найденные ArUco-метки на изображении
        viewer.imshow("aruco_coordinates", frame, fps=30)  # запускаем трансляцию изображения

finally:                                  # блок finally выполнится при завершении программы
    camera.stop()                         # останавливаем получение кадров с камеры
    viewer.close()                        # останавливаем видеопоток