Калибровка камеры и поза ArUco с помощью OpenCV

Изображение камеры задано в пикселях, но роботу нужны лучи и координаты. Калибровка камеры оценивает внутренние параметры, которые отображают трёхмерные (3D) точки в системе координат камеры в пиксели, а также искажения, вносимые объективом. Когда эти значения известны, маркер с известной геометрией может сообщить своё положение и ориентацию относительно камеры.

Маркеры ArUco — квадратные бинарные маркеры с идентификатором (ID). Обнаружение четырёх углов определяет маркер на изображении; оценка позы объединяет эти пиксельные углы, реальный размер маркера и откалиброванные параметры камеры.

Поймите результаты калибровки

Матрица камеры содержит фокусные расстояния и оптический центр:

[ fx   0  cx ]
[  0  fy  cy ]
[  0   0   1 ]

fx и fy выражены в пикселях. cx и cy задают главную точку. Коэффициенты искажения учитывают радиальные бочкообразные и подушкообразные искажения, а также тангенциальное смещение.

Эти значения относятся к камере, объективу, фокусировке и геометрии изображения. Изменение разрешения обрезкой или масштабированием может потребовать корректировки внутренних параметров; изменение фокуса или замена объектива требует повторной калибровки.

Распечатайте и измерьте калибровочный объект

Шахматная доска — практичный первый объект, поскольку её внутренние углы легко определить. Точно измерьте размер клетки. Масштаб позы наследует эту единицу: точки объекта в метрах дают перемещения в метрах; точки объекта в миллиметрах дают миллиметры.

Держите объект плоским. Изогнутый лист нарушает плоскую геометрию, предполагаемую для точек объекта. Если важна точность, закрепите его на жёстком материале.

Определите число внутренних углов, а не напечатанных клеток. У доски 10 на 7 клеток есть 9 на 6 внутренних углов. Смешение этих чисел приводит к сбою обнаружения даже тогда, когда изображение выглядит чётким.

Соберите полезный набор изображений

Соберите как минимум десять успешных ракурсов в качестве отправной точки, а затем добавляйте разнообразие, а не дубликаты. Перемещайте доску по всему кадру, наклоняйте по обеим осям, меняйте расстояние и включайте углы рядом с краями изображения, где искажение сильнее всего. Оставляйте узор резким и полностью видимым.

Двадцать почти одинаковых фронтальных кадров дают меньше информации, чем меньшее число хорошо распределённых поз. Отбрасывайте размытие от движения и кадры, в которых обнаружение углов неоднозначно. Используйте то же разрешение и те же настройки камеры, которые запланированы для работы.

Для каждого принятого изображения сохраняйте обнаруженные углы на изображении и соответствующие плоские точки объекта (column × squareSize, row × squareSize, 0).

Откалибруйте с помощью OpenCV

Основной поток на Python выглядит так:

import cv2
import numpy as np

pattern = (9, 6)          # internal corners: columns, rows
square_size = 0.024       # metres

object_template = np.zeros((pattern[0] * pattern[1], 3), np.float32)
object_template[:, :2] = np.mgrid[0:pattern[0], 0:pattern[1]].T.reshape(-1, 2)
object_template *= square_size

object_points, image_points = [], []

for gray in calibration_images:
    found, corners = cv2.findChessboardCorners(gray, pattern)
    if not found:
        continue
    refined = cv2.cornerSubPix(
        gray, corners, (11, 11), (-1, -1),
        (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001),
    )
    object_points.append(object_template.copy())
    image_points.append(refined)

rms, camera_matrix, distortion, rvecs, tvecs = cv2.calibrateCamera(
    object_points, image_points, gray.shape[::-1], None, None
)

Перед вызовом калибровки защититесь от слишком малого числа принятых изображений. Сохраняйте вместе матрицу, коэффициенты искажения, ширину и высоту изображения, определение объекта и дату калибровки. Файл с одним только массивом легко неправильно использовать позже.

Проверяйте не только возвращённое значение RMS

Среднеквадратичная (RMS) ошибка репроекции показывает среднеквадратичное отклонение проекций калибровочных точек от обнаруженных точек изображения. Меньшее значение обычно лучше, но одно число может скрыть плохие области или слабый набор изображений.

Вычислите ошибку репроекции для каждого изображения и изучите худшие ракурсы. Наложите репроецированные точки на изображения. После устранения искажений проверьте прямые архитектурные границы, особенно возле углов. Затем оцените позу объекта на известном расстоянии и сравните перемещение с физическим измерением.

Если одно изображение даёт основной вклад в ошибку, изучите и удалите его только по обоснованной причине, например из-за размытия или неправильного порядка углов. Многократное удаление кадров с большой ошибкой до тех пор, пока число не станет хорошим, может скрыть плохую модель или объект.

Обнаружьте маркер ArUco

Выберите заранее определённый словарь и создавайте маркеры из него же. Более крупные битовые сетки дают больше идентификаторов (IDs), но при фиксированном размере изображения каждому биту достаётся меньше пикселей. Распечатайте маркер с белой немаркированной рамкой и измерьте длину стороны чёрного квадрата, используемую в модели позы.

dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
parameters = cv2.aruco.DetectorParameters()
detector = cv2.aruco.ArucoDetector(dictionary, parameters)

corners, ids, rejected = detector.detectMarkers(frame)

Создание объектов API различается в старых выпусках OpenCV, поэтому согласуйте код с официальной документацией установленной версии. Выходные данные всё равно должны содержать четыре упорядоченных угла изображения для каждого обнаруженного маркера.

Оцените позу по известному размеру маркера

Определите углы маркера в его собственной системе координат и решите задачу перспективной проекции n точек (perspective-n-point). Для стороны маркера s специальный решатель OpenCV для квадрата ожидает согласованный порядок углов:

s = 0.080  # metres
object_corners = np.array([
    [-s/2,  s/2, 0],
    [ s/2,  s/2, 0],
    [ s/2, -s/2, 0],
    [-s/2, -s/2, 0],
], dtype=np.float32)

ok, rvec, tvec = cv2.solvePnP(
    object_corners,
    corners[0].reshape(4, 2),
    camera_matrix,
    distortion,
    flags=cv2.SOLVEPNP_IPPE_SQUARE,
)

tvec — вектор положения начала маркера в системе координат камеры; его единица длины совпадает с единицей s. rvec — вектор вращения Родригеса, а не углы Эйлера. Используйте cv2.Rodrigues(), чтобы получить матрицу вращения, или преобразуйте его через систему координат робота.

Нарисуйте проекции координатных осей и проверьте их физически. Положительное перемещение с правдоподобной величиной недостаточно, если оси отражены или введён неправильный размер маркера.

Сделайте позу достаточно стабильной для робота

Поза становится шумной, когда маркер занимает мало пикселей, находится под крутым углом, размыт или приближается к краю изображения. Улучшите освещение и экспозицию, используйте больший маркер, подойдите ближе или примените доску с несколькими маркерами, если это допускает приложение.

Фильтруйте только допустимые позы и сохраняйте метки времени. Фильтр нижних частот уменьшает дрожание, но добавляет задержку управления. Отбрасывайте резкие результаты по ошибке репроекции, ограничениям расстояния и ограничениям движения, а не усредняйте слепо каждое обнаружение.

Поза маркера задана относительно камеры. Чтобы управлять манипулятором или мобильной базой, преобразуйте её через откалиброванное крепление камеры в base_link или основание манипулятора. Несоответствие миллиметров и метров здесь создаёт тысячекратную ошибку.

Диагностируйте типичные сбои

• Устранение искажений изгибает прямые линии: ракурсы калибровки не обеспечивают достаточного покрытия, объект был не плоским или изменилась геометрия изображения.
• Масштаб расстояния постоянно неверен: размер клетки или маркера указан неверно либо использована не та единица.
• Оси позы направлены не туда: неправильный порядок углов объекта или соглашение о системах координат.
• Идентификатор маркера (ID) дрожит: маркер слишком мал, размыт, плохо напечатан или взят из другого словаря.
• Поза скачет при фронтальном ракурсе: нужны лучшие точки съёмки, отслеживание или ограничения от нескольких маркеров из-за неоднозначности плоской позы и слабой геометрии пикселей.
• Калибровка работает только при одном разрешении: внутренние параметры использованы после обрезки или изменения размера без корректировки.
• Движение робота отстаёт от маркера: чрезмерное сглаживание или устаревшие метки времени скрывают текущую геометрию.

Используйте симулятор датчиков, чтобы исследовать шум, смещение, фильтрацию и задержку. Симулятор роботизированной руки показывает, почему поза цели всё ещё может быть недостижимой даже после правильной геометрии камеры.