Разбор: aruco_examples

← Все разборы примеров

Источник: репозиторий pioneer-team/pioneer-sdk2-example (GitFlic). Справочник по классам API — на странице «Pioneer SDK 2».

Порядок работы

Примеры образуют цепочку: калибровка → детекция → координаты → полёт. Перед полётом нужен корректный data.yml — его делает camera_calibration.py (порядок съёмки подробно разобран на странице camera_examples):

python3 camera_calibration.py
python3 detect_aruco.py
python3 detect_aruco_coordinates.py
python3 aruco_flight.py

detect_aruco.py — базовая детекция

Самый компактный пример знакомит с ArUco-детекцией OpenCV поверх кадров бортовой камеры:

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

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 = 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-метки на изображении

        cv2.aruco.drawDetectedMarkers(frame, corners, ids) # рисуем найденные ArUco-метки на изображении

        viewer.imshow("aruco", frame, fps=30) # запускаем трансляцию изображения
                                              # aruco - название трансляции
                                              # frame - изображение с камеры
                                              # fps=30 - количество кадров в секунду

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

Ключевые места:

  • Словарь DICT_ARUCO_ORIGINAL один на всю группу — ваши метки должны быть напечатаны из того же словаря.
  • В OpenCV 4.7+ детектор — это объект ArucoDetector; detectMarkers(frame) возвращает углы меток, ID и отбракованные области.
  • Найденные метки рисуются на кадре cv2.aruco.drawDetectedMarkers, и кадр публикуется в поток aruco: rtsp://10.42.0.1:8889/aruco/.

detect_aruco_coordinates.py — координаты метки (solvePnP)

Следующий шаг — превратить 2D-обнаружение в 3D-положение метки относительно камеры. Для этого нужны калибровка и известный реальный размер метки:

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
  • load_coefficients() читает из data.yml матрицу камеры mtx и коэффициенты искажений dist; файл ищется рядом со скриптом.
  • size_of_marker = 0.05 — сторона метки в метрах. Четыре угла метки задаются в 3D координатами points_of_marker (центр метки — начало координат).

Основной цикл отличается тем, что после детекции решается задача PnP — «как нужно расположить метку в 3D, чтобы её углы совпали с найденными на кадре»:

        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)  # запускаем трансляцию изображения

Вектор tvecs — положение метки относительно камеры: x и y лежат в плоскости кадра, z — вглубь (расстояние до метки). Оси метки рисуются командой cv2.drawFrameAxes; поток aruco_coordinates — rtsp://10.42.0.1:8889/aruco_coordinates/.

aruco_flight.py — полёт с удержанием метки

Крупный пример (~375 строк): дрон по клавише взлетает, а затем визуально удерживает метку в кадре, корректируя дистанцию и высоту. Код разделён на два потока: VideoProcessingThread непрерывно детектирует метку и публикует кадр, а главный цикл принимает решения и отправляет скорости. Константы задают «правила игры»:

DATA_PATH = Path(__file__).with_name("data.yml")       # путь к файлу калибровки рядом со скриптом
DEBUG_PRINT_INTERVAL = 0.5                             # пауза между диагностическими сообщениями в терминал
CAMERA_HORIZONTAL_ANGLE = 25                           # угол камеры, при котором она смотрит горизонтально
FORWARD_SPEED = 0.4                                    # скорость движения к метке и от метки в м/с
VERTICAL_SPEED = 0.25                                  # скорость коррекции высоты по метке в м/с
YAW_RATE = 0.4                                         # скорость поворота к метке в рад/с
COMMAND_INTERVAL = 0.3                                 # время действия команды скорости в секундах
SEND_PERIOD = 0.2                                      # как часто повторять команду скорости, включая нулевую
ZERO_SPEED = (0.0, 0.0, 0.0, 0.0)                      # нулевая команда: vx, vy, vz, yaw_rate

Кадр разбивается на три колонки и три строки — так дрон понимает, куда сместилась метка:

def get_marker_zone(x_center, frame_width):            # функция определяет, в какой зоне кадра находится метка
    if x_center < frame_width / 3:                     # проверяем, что центр метки слева от центральной зоны
        return "left"                                  # возвращаем левую зону
    if x_center > frame_width * 2 / 3:                 # проверяем, что центр метки справа от центральной зоны
        return "right"                                 # возвращаем правую зону
    return "center"                                    # возвращаем центральную зону


def get_vertical_zone(y_center, frame_height):          # функция определяет, выше или ниже центра находится метка
    if y_center < frame_height / 3:                    # проверяем, что центр метки выше центральной зоны
        return "top"                                   # возвращаем верхнюю зону
    if y_center > frame_height * 2 / 3:                # проверяем, что центр метки ниже центральной зоны
        return "bottom"                                # возвращаем нижнюю зону
    return "center"                                    # возвращаем центральную зону

Основное решение главного цикла — три простых порога: по дистанции, по горизонтальной зоне и по вертикальной зоне кадра:

        if in_sky and coordinates is not None and x_center is not None and y_center is not None and frame_width is not None and frame_height is not None: # проверяем, что дрон в воздухе и метка найдена
            distance = float(np.linalg.norm(coordinates)) # вычисляем расстояние до ArUco-метки
            zone = get_marker_zone(x_center, frame_width) # определяем зону кадра с найденной меткой
            vertical_zone = get_vertical_zone(y_center, frame_height) # определяем вертикальную зону кадра с найденной меткой
            status_parts = []                       # создаем список действий, выбранных алгоритмом

            if distance > 1.5:                      # проверяем, что дрон далеко от метки
                vy = FORWARD_SPEED                  # задаем скорость движения вперед
                status_parts.append("forward")      # сохраняем действие по дистанции
            elif distance < 1.0:                    # проверяем, что дрон слишком близко к метке
                vy = -FORWARD_SPEED                 # задаем скорость движения назад
                status_parts.append("backward")     # сохраняем действие по дистанции
            else:                                    # если дистанция находится в рабочем диапазоне
                status_parts.append("hold distance") # сохраняем действие по дистанции

            if x_center < frame_width / 3:          # проверяем, что метка находится слева на изображении
                yaw_rate = YAW_RATE                 # задаем поворот влево
                status_parts.append("turn left")    # сохраняем действие по курсу
            elif x_center > frame_width * 2 / 3:    # проверяем, что метка находится справа на изображении
                yaw_rate = -YAW_RATE                # задаем поворот вправо
                status_parts.append("turn right")   # сохраняем действие по курсу
            else:                                    # если метка находится в центральной зоне
                status_parts.append("hold yaw")     # сохраняем действие по курсу

            if y_center < frame_height / 3:          # проверяем, что метка находится выше центральной зоны кадра
                vz = VERTICAL_SPEED                  # задаем набор высоты
                status_parts.append("up")            # сохраняем действие по высоте
            elif y_center > frame_height * 2 / 3:    # проверяем, что метка находится ниже центральной зоны кадра
                vz = -VERTICAL_SPEED                 # задаем снижение
                status_parts.append("down")          # сохраняем действие по высоте
            else:                                    # если метка находится по центру по вертикали
                status_parts.append("hold height")   # сохраняем действие по высоте

            status = ", ".join(status_parts)         # собираем текстовое описание действия

Разбор решения:

  • distance = np.linalg.norm(coordinates) — расстояние до метки по 3D-вектору, который дал cv2.solvePnP.
  • Дистанция 1,0–1,5 м — рабочий диапазон: ближе 1 метра дрон отъезжает, дальше 1,5 метра подъезжает со скоростью 0,4 м/с.
  • Метка в левой трети кадра → yaw_rate положительный (поворот к метке влево); в правой трети → вправо. Верхняя треть кадра → набор высоты, нижняя → снижение.
  • Если метка пропала из кадра, состояние marker lost, hover: нулевые скорости, никакого «слепого» поиска.

Управление запуск и посадка — с клавиатуры: s — взлёт и набор 1,5 м, q — посадка и выход. Команда скорости повторяется каждые SEND_PERIOD секунд, даже когда нужно отправить нулевую:

        speed_command = (0.0, vy, vz, yaw_rate)      # собираем команду скорости: vx, vy, vz, yaw_rate
        send_due = time.monotonic() - last_speed_send_time >= SEND_PERIOD # проверяем, пора ли повторить команду
        speed_changed = speed_command != last_sent_speed # новую скорость отправляем сразу

        if in_sky and (speed_changed or send_due):   # проверяем, нужно ли отправить команду скорости
            drone.set_manual_speed_body_fixed(*speed_command, COMMAND_INTERVAL) # отправляем команду скорости
            last_speed_send_time = time.monotonic()  # запоминаем время отправки команды
            last_sent_speed = speed_command          # запоминаем последнюю отправленную скорость

        time.sleep(0.05)                            # ставим небольшую паузу, чтобы не нагружать программу

Размер метки здесь уже 0,1 м, а сервокамера перед стартом ставится на 25° — камера смотрит горизонтально. Гарантию безопасного завершения даёт finally: нулевая скорость и посадка, если дрон всё ещё в воздухе:

finally:                                            # блок finally выполнится при завершении программы
    restore_terminal(terminal_settings)             # восстанавливаем настройки терминала
    video_thread.stop()                             # останавливаем поток обработки видео
    video_thread.join()                             # ждем завершения потока обработки видео

    if drone.get_fly_state().name == "IN_SKY":      # проверяем, находится ли дрон в воздухе
        drone.set_manual_speed_body_fixed(*ZERO_SPEED, COMMAND_INTERVAL) # отправляем нулевую скорость перед посадкой
        drone.land()                                # производим посадку

Безопасность

  • Запускайте полётный пример только после проверки калибровки и распознавания метки.
  • Убедитесь, что data.yml лежит рядом со скриптом, метка хорошо освещена, вокруг свободно.
  • Потоки группы: calibration, calibration_result, aruco, aruco_coordinates, aruco_flight — все на порту 8889.