Определение положения объектов на изображении: как найти пространственные координаты объекта, используя OpenCV / forpes.ru

Главная
Определение положения объектов на изображении: как найти пространственные координаты объекта, используя OpenCV

Определение положения объектов на изображении: как найти пространственные координаты объекта, используя OpenCV +22

28.08.2025 21:37

lb357 2 1300 Источник

Привет, Хабр!

Меня зовут Брискиндов Леонид, я школьник, участник соревнований и олимпиад по робототехнике и программированию. Нередко для решения задач таких олимпиад предоставляется доступ к единственной камере, снимающей тестовое поле, на котором расположены роботы. Таким образом появляется задача определения положения объектов в пространстве по изображению, полученному с камеры. В данной статье мы и разберём, как решать такую задачу, используя библиотеку OpenCV.

OpenCV — кроссплатформенная open‑source библиотека для работы с алгоритмами машинного зрения. Данная библиотека поддерживает различные языки программирования (Python, C++, Java и другие). Статья будет актуальна для любого поддерживаемого языка программирования, однако демонстративный код будет написан на Python в связке с библиотекой NumPy.

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

Краткая памятка по линейной алгебре
1. Скаляр
2. Матрица
3. Вектор
4. Произведение матрицы и скаляра (числа)
5. Произведение матрицы и матрицы
6. Нахождение обратной матрицы
7. Однородная система координат
Формирование изображения
1. Камера‑обскура
Математика камеры
1. Виртуальная (фронтальная) плоскость
2. Системы координат и координатные пространства
3. Внутренние параметры камеры и калибровка
4. Модель камеры‑обскуры
5. Определение объекта (координатной модели)
6. Задача Perspective‑n‑Point (PnP)
Нахождение пространственных координат объекта в системе координат камеры
Частные случаи задачи
1. Определение пространственных координат точки, при известной Zc
2. Преобразование с заданной Zw для искомой точки
3. Определение позиции объекта A в системе координат объекта B
4. Результаты решения частных случаев

Краткая памятка по линейной алгебре

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

В данном разделе будет представлена минимальная необходимая информация о работе с матрицами и векторами в упрощённой форме. Линейную алгебру не выучить по небольшой памятке в статье на Хабре, но описанные методы нам пригодятся в дальнейшем. Подробнее можно узнать в учебниках по линейной алгебре, а также рекомендую серию роликов по основам линейной алгебры от 3Blue1Brown.

Скаляр

Скаляр — это величина, характеризующаяся одним численным значением:

$\displaystyle x \in \mathbb{R}$

Например:

Матрица

Матрица — это математический объект, записываемый в виде прямоугольной таблицы чисел (количество строк и столбцов задаёт размер таблицы — $m \times n$ ), описывающих элементы некоторого множества:

$A=\begin{bmatrix}a_{ij}\end{bmatrix} \text{, или } A=\begin{Vmatrix}a_{ij}\end{Vmatrix} \text{, или } A=\begin{pmatrix}a_{ij}\end{pmatrix} \text {; где:} \\ A \text { — матрица (с указанием размерности }\underset{m \times n}{A} \text{), } \\ a_{ij} \text { — элемент мтрицы} \text{, } \\ i \text { — номер строки, } j \text {— номер столбца}$

Например:

$A=\begin{bmatrix}\ 7\ 4\ 0\ \\ 0\ 3\ 4\\ 0\ 0\ 5\end{bmatrix}$

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

Наиболее распространённые преобразования описываются: матрицей масштабирования, матрицей поворота и вектором сдвига (перемещения) — см. вики по матрицам перехода.

Для квадратных (m=n) матриц существует скалярная величина, которая характеризует саму матрицу и ориентированное растяжение/сжатие пространства, описываемое ей — определитель (детерминант):

$\det A \text { или } |A|$

Детерминант можно посчитать по определённым правилам, но в контексте данной статьи, его будем рассчитывать, используя библиотеку NumPy:

np.linalg.det(a)    # a - матрица, записанная в массив

Вектор

Вектор — это математический объект, характеризующийся величиной и направлением. Вектор в n‑мерном пространстве может быть представлен координатами или матрицей, содержащей один столбец/строку:

$\displaystyle x \in \mathbb{R}^n$

Например:

$\overrightarrow{x}(7; 4; 3) \text{ или } x=\begin{bmatrix}7\\4\\3\end{bmatrix}$

Вектор, как матрица, может описывать различные преобразования, а также положение точки в пространстве. Как отмечалось выше поворот может быть описан (квадратной) матрицей, однако в контексте данной статьи также будет использоваться термин «вектор поворота», используемый в документации OpenCV, связанный с формулой поворота Родрига, и описывающий более компактное представление матрицы поворота в виде вектора. Для перевода вектора поворота в матрицу вращения (и наоборот) будем использовать метод OpenCV:

cv2.Rodrigues(src) # src - вектор/матрица поворота, записанная в массив

Далее разберём некоторые из действий (только тех, которые понадобятся в дальнейшей статье) над данными математическими объектами и преобразования, которые они описывают.

Произведение матрицы и скаляра (числа)

$B=k \cdot A \text{, } A=[a_{ij}] \text{; тогда:} \\B = [k \cdot a_{ij}]$

Например:

$2 \cdot \begin{bmatrix}7\\4\\3\end{bmatrix} = \begin {bmatrix} 14 \\ 8 \\ 6 \end {bmatrix}$

Такое произведение описывает равномерное масштабирование.

Произведение матрицы и матрицы

$C=A \cdot B \text{, }\ \underset{m \times k}{A} \text{, }\ \underset{k \times n}{B} \text{; тогда:} \\ \underset{m \times n}{C} \text {, причём } c_{ij} = \sum_{s=1}^{k} {a_{is} b_{sj}}$

Другими словами каждый элемент матрицы произведения двух матриц равен сумме произведений соответственных элементов i‑ой строки первой матрицы и j‑го столбца второй матрицы.

Например:

$\begin{bmatrix}1 \ 2 \ 3\\4 \ 5\ 6\\7 \ 8\ 9\end{bmatrix} \cdot \begin{bmatrix}1\ 2\\3 \ 4\\5\ 6\end{bmatrix} = \begin{bmatrix}1 \cdot 1 + 2 \cdot 3 + 3 \cdot 5 \ \space 1 \cdot 2 + 2\cdot4+3\cdot6 \\4\cdot1+5\cdot3+6\cdot5 \ \space 4\cdot2+5\cdot4+6\cdot6\\7\cdot1+8\cdot3+9\cdot5 \ \space 7\cdot2+8\cdot4+9\cdot6\end{bmatrix} = \begin{bmatrix} 22 \ 28 \\49 \ 64 \\ 76 \ 100 \end{bmatrix}$

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

В коде будем использовать функцию умножения из библиотеки NumPy:

np.dot(a, b)    # a, b - матрицы, записанные в массив

Нахождение обратной матрицы

Обратная матрица — это такая матрица, произведение которой с исходной матрицей в результате даёт единичную матрицу (все элементы которой равны 0, кроме элементов диагонали от верхнего левого угла к нижнему правому, которые равны 1):

$A^{-1} \text {, при} \det A \neq 0$

Обратная матрица описывает обратное преобразование.

Обратную матрицу можно вычислить по определённым правилам, но в контексте данной статьи, её будем вычислять, используя библиотеку NumPy:

np.linalg.inv(a)    # a - матрица, записанная в массив

Однородная система координат

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

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

Для примера рассмотрим результат применения преобразования, представленного матрицей $4\times4$ , к вектор‑столбцу $4\times1$ (в трёхмерном пространстве) в однородных координатах, при (без проецирования/масштабирования). Матрица и вектор :

$A = \begin{bmatrix}r_{11} \ r_{12} \ r_{13} \ t_{x} \\ r_{21} \ r_{22} \ r_{23} \ t_{y} \\ r_{31} \ r_{32} \ r_{33} \ t_{z} \\ a \space\space\space \ b \space\space\space \ c \space\space\ W \end{bmatrix} \text {; }\ B = \begin{bmatrix}X\\Y\\Z\\W \end{bmatrix}\\$

В преобразовании из матрицы : $r_{11}−r_{33}$ матрица поворота‑масштабирования; вектор перемещения. В обычных матрицах перехода, при расчётах преобразованиях трёхмерного пространства: и .

В векторе : определяют исходный вектор.

Таким образом результат данного преобразования:

$C = A \cdot B$

Формирование изображения

В данном разделе рассмотрим принцип формирования изображения в камере. Подробнее можно прочитать в статье или на вики.

Для решения поставленной задачи (и в целом в машинном зрении) используются проективные преобразования, описываемые моделью камеры с точечной диафрагмой, она же модель камеры‑обскуры, она же модель пинхол камеры (pinhole camera). Математическая модель камеры‑обскуры рассматривается в следующем разделе, а пока рассмотрим принцип работы самой камеры‑обскуры.

Камера-обскура

Принцип работы камеры‑обскуры заключается в том, что свет, отражённый от внешних объектов, проникает внутрь камеры через узкое отверстие, формируя на матрице камеры изображение. В результате на матрице проецируется отзеркаленное изображение внешних объектов.

Внутри демонстрационного стенда: отверстие, через которое проходит свет, формирующий изображение; белый лист бумаги, выполняющий функции матрицы камеры, на нём формируется изображение; USB‑камера для получения изображения с листа бумаги.

Демонстрация: в круге изображение с листа (матрицы камеры), зелёными линиями показаны пути света от объекта через отверстие.

В видеоаппаратуре, построенной на данном принципе, часто используются системы линз и зеркал, выполняющих различные функции: улучшение сбора света, регулировка фокуса и т. д, — однако также они могут добавлять искажение (дисторсию) к изображению.

Математика камеры

Виртуальная (фронтальная) плоскость

Для анализа изображений без отзеркаливания используется условное проецирование окружающих объектов не через диафрагму (пинхол) на плоскость изображения (матрицы камеры), а на виртуальную (фронтальную) плоскость изображения перед диафрагмой, причём и виртуальная плоскость изображения, и плоскость изображения на матрице камеры находятся на равном фокусном расстоянии от диафрагмы, а оптическая ось пересекает их в проекциях одной и той же точки:

Виртуальная плоскость изображения с искомой проекцией

Таким образом в дальнейшей статье будет фигурировать только виртуальная плоскость изображения.

Системы координат и координатные пространства

Далее в статье будут использоваться сокращения:

px — пиксель изображения
м — метр. Примечание: данная единица измерения выбрана в контексте данной статьи, так как является основной единицей длины в СИ, однако далее данная единица используется в формулах с масштабными коэффициентами, так что вместо метров могут использоваться другие единицы измерения (мм, футы и др.)

В OpenCV в целом и в нижеизложенной математической модели в частности, в отличие от некоторых других библиотек/фреймворков, связанных с обработкой изображений (например OpenGL), используется следующие системы координат:

Где:

$O_{c}$ — начало координат в системе координат камеры (в некоторых источниках используется обозначение $F_{c}$ )
$O_{w}$ — начало координат в мировой (объектной) системе координат (в некоторых источниках используются обозначения )
— оси системы координат камеры / координаты (м) в системе координат камеры по соответственным осям
$X_{w},Y_{w},Z_{w}$ — оси мировой системы координат / координаты (м) в мировой системе координат по соответственным осям
— оси системы координат изображения / координаты (px) в системе координат изображения
— соответствующие проекции/продолжения осей системы координат камеры на плоскость изображения ()
— координаты (px) главной точки системе координат изображения. Главная точка, она же оптический центр изображения, она же principal point — точка пересечения оптической оси и плоскости изображения.
— фокусное расстояние камеры (px) (далее будут использоваться обозначения и для фокусного расстояния, т. к. технические свойства, оптические эффекты объектива / матрицы камеры и разбиение изображения на пиксели могут приводить к несоразмерности фокусного расстояния по осям; но в идеальных условиях )
$P_{c}$ и $P_{w}$ — искомая точка () в системе координат камеры и мировой системе координат соответственно
— проекция искомой точки () на плоскость изображения (представляет из себя некоторую точку на изображении)
$^{c}T_{w}$ — матрица перехода $4\times4$ из мировой системы координат в систему координат камеры (в некоторых источниках используется обозначение $^{c}T_{o}$ )
— матрица поворота $3\times3$ и вектор перемещения (сдвига), описывающие преобразование из мировой системы координат в систему координат камеры, соответственно. Эти значения являются внешними параметрами камеры.

Причём $^{c}T_{w}$ является однородным представлением преобразования, описываемого конкатенацией (склеивания и в одну матрицу $3\times4$ ), т. е. верхний левый угол $\underset{4\times4}{^{c}T_{w}}$ занимает, правый верхний угол занимает , нижний правый угол занимает, а остальные значения равны . Также далее будут фигурировать обозначения и , где — вектор поворота, описывающий преобразование , а — то же самое, что и .

Внутренние параметры камеры и калибровка

По итогу на полученное изображение, влияют различные факторы: фокусное расстояние, расположение главной точки, различные виды искажения (дисторсии) изображения — всё это описывается внутренней матрицей (внутренними параметрами) камеры и коэффициентами (параметрами) дисторсии:

$A = \begin{bmatrix}f_x \ \ 0 \ \ c_x \\ 0 \ \ f_y \ c_y \\ 0 \ \ \ 0 \ \ \ 1 \end{bmatrix} \text{; где: } \\ A\text{ — внутренняя матрица камеры (в некоторых источниках используются обозначения }K\text{,}\\\text{cameraMatrix)} \\ f_x \text{ и } f_y \ \text{ — фокусное расстояние по осям (px)} \\ c_x \ \text{и} \ c_y \text{— координаты главной точки (px)}$

Также $A_{12}$ может быть $A_{12}\neq0$ и обозначать скос ( $\gamma$ ), однако данный случай в статье мы не рассматриваем.

В свою очередь коэффициенты дисторсии представляют из себя набор скаляров, описывающий искажения изображения, который позволяет произвести преобразование («undostorting») изображения, убирающее искажения, по определённым правилам (подробнее можно почитать в статье, вики и документации OpenCV). Массив чисел, состоящий из коэффициентов дисторсии, может обозначаться как , а отдельные коэффициенты могут обозначаться как $k_{i}, p_{i}, s_{i}$ и т. д. в зависимости от вида дисторсии, причём часть из них может равняться или отсутствовать. В рамках данной статьи, избавляться от искажения изображения будем с помощью методов OpenCV:

cv2.undistort(src, cameraMatrix, distCoeffs)
# Функция для удаления искажения изображения
# src - входное изображение NumPy ArrayLike / OpenCV MatLike
# cameraMatrix - внутренняя матрица камеры, записанная в массив
# distCoeffs - массив коэффициентов дисторсии

cv2.fisheye.undistortImage(distorted, K, D, Knew=K)
# Аналогичная функция для камер типа "рыбий глаз" (fisheye)
# distorted - входное изображение, K - матрица камеры, D - коэффициенты дисторсии
# В целом fisheye считается отдельной моделью камеры, отличной от pinhole, однако
# после удаления дисторсии к ней применимы правила для pinhole модели.

Альтернативно могут использоваться комбинации методов:

cv2.initUndistortRectifyMap()
cv2.remap()
cv2.getOptimalNewCameraMatrix()
cv2.fisheye.estimateNewCameraMatrixForUndistortRectify()
# и другие

Для нахождения внутренней матрицы камеры и коэффициентов дисторсии, необходимо провести калибровку камеры. В данной статье мы не будем рассматривать процесс калибровки, так как по этой теме существует много информации, включая стандартный скрипт в руководстве по OpenCV. Тем не менее подробнее о калибровке можно узнать в статье, вики, документации OpenCV. Результатом калибровки должны стать , а также при калибровке камеры с помощью калибровочного паттерна (например шахматной доски), массивы из и для калибровочного паттерна. Получается, что уже на этом этапе можно определить пространственные координаты камеры относительно калибровочного паттерна, но мы ищем координаты искомой точки (объекта), так что продолжим.

Модель камеры-обскуры

Для изображения, полученного в результате удаления искажений, будет применима модель камеры обскура с нулевыми коэффициентами дисторсии (без искажений):

$s\ p = AP{_c}\text{ , где: } \\ s\text{ — масштабный множитель (используются обозначения}\ W \text{, } \lambda \text{;}\ s=Z_{c} \text {)} \\ p \ \text{ представлена в однородных координатах}$

Если $P_{c}$ представлена в однородных координатах, то для соразмерности и $P_{c}$ , при перспективном проецировании, может использоваться преобразование

$\Pi = \begin{bmatrix} 1\ 0\ 0\ 0\\ 0\ 1\ 0\ 0\\ 0\ 0\ 1\ 0 \end{bmatrix}$ , тогда выражение будет иметь вид:

$s\ p = A\Pi P_{c} \\ \text{(в } \underset{4\times1}{P_{c}} \text{ с помощью } \Pi \text{ убрали нижнюю строку, содержащую } 1 \text{ и получили } \underset{3\times1}{P_{c}} \text{ )}$

Как повествовалось выше $P_{c}$ и $\underset{4\times1}{P_{w}}$ определяют координаты искомой точки в системе координат камеры и мировой системе координат соответственно, а перевод из в осуществляется преобразованием, описываемым матрицей $\underset{3\times4}{[R|t]}$ или матрицей $\underset{4\times4}{^{c}T_{w}}$ (в однородных координатах):

${P_{c}} = [R|t]P_{w} \ \text{ или } \ {P_{c}} =\ ^{c}T_{w}P_{w}$

Тогда в общем виде модель будет:

$s\ p = A[R|t]P_{w} \\ \text{или} \\ s \ p = A \Pi\ ^{c}T_{w} P_{w}$

С помощью данной модели мы сможем получить искомую в той или иной системе координат.

Определение объекта (координатной модели)

Заметим из модели камеры‑обскуры, что в общем случае невозможно определить пространственные координаты в системе координат камеры одной отдельно взятой точки, имея лишь и :

Проецирование различных точек пространства в одну точку изображения (GIF анимация)

В дальнейшей статье разберём некоторые частные случаи, когда это можно сделать, но пока будем считать, что для того, чтобы найти пространственные координаты объекта в некоторой системе координат, отличной от системы координат самого объекта (мировой системы координат), нам требуется набор нескольких пар таких, что и будут нам известны.

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

Но вернёмся к маркерам, их существует огромное множество, например:

Маркеры в машинном зрении и робототехнике

Далее мы будем использовать ArUco маркеры (чёрные квадраты с белыми пикселями внутри, кодирующими номер маркера), так как в OpenCV представлен обширный набор инструментов для детекции и работы с ними, в частности класс cv2.aruco.ArucoDetector. Не будем вдаваться в подробности работы с ArUco маркерами в OpenCV, так как по этой теме существует много информации, включая FAQ от OpenCV и документацию OpenCV.

Нам необходимо получить corners — позиции (px) вершин (углов) ArUco маркеров на изображении — с помощью функции detectMarkers. Это и будет наш набор из четырёх для каждого маркера.

Теперь определим . У ArUco маркеров вершины определяются в строгом порядке: в начальном положении маркера (без поворота) по часовой стрелке, начиная от верхней левой, — в таком же порядке расположим соответствующие каждой точки в мировой системе координат, расположив начало координат в центре маркера (тем же определив направления осей мировой системы координат относительно маркера), тогда набор из четырёх будет:

[
    [-marker_size / 2, marker_size  / 2, 0],
    [marker_size  / 2, marker_size  / 2, 0],
    [marker_size  / 2, -marker_size / 2, 0],
    [-marker_size / 2, -marker_size / 2, 0]
]
# где marker_size размер стороны маркера (м)

{0; 1; 2; 3} — индексы точек (по часовой стрелки от верхней левой), (0; 0; 0) — начало мировой (объектной) системы координат расположено в центре маркера

В результате мы получили набор из четырёх пар, в котором находится по изображению с камеры, а мы задали самостоятельно.

Задача Perspective-n-Point (PnP)

Perspective‑n‑Point (PnP) — это задача оценки положения камеры (обычно откалиброванной) в мировой системе координат (поворота и сдвига относительно начала системы отсчёта) по набору из $n\in\mathbb{N}$ точек с известной позицией в мировой системе координат и их проекциям на изображении. Другими словами зная пар необходимо найти .

В литературе встречается термин «camera 3d pose estimation», как обозначение процесса решения данной задачи.

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

P3P — классический метод решения данной задачи, при $n\in\{3;4\}$ , использовавшийся ещё в XIX веке в аналоговой фотограмметрии.
DLT (Direct linear transformation) метод и его итерационная оптимизация Левенберга‑Марквардта может использоваться как для калибровки камеры, так и для решения задачи PnP, при $n\in[4;+\infty)$ для компланарных точек и $n\in[6;+\infty)$ для остальных случаев.
EPnP (Efficient PnP) — метод решения, при $n\in[4;+\infty)$ , имеющий наименьшую сложность алгоритма, но невысокую устойчивость к ошибкам.
IPPE (Infinitesimal Plane‑based Pose Estimation) — метод решения, при $n\in[4;+\infty)$ для компланарных точек. Данный метод довольно устойчив к ошибкам с фиксированными точками, находящимися далеко от камеры и довольно близко друг к другу, поэтому его часто используют при оценке позиции относительно всевозможных маркеров (например ArUco).
SQPNP — метод решения, при $n\in[3;+\infty)$ ; обобщённый и достаточно оптимизированный.

Стоит отметить, что у данной задачи обычно имеется несколько возможных решений, и могут быть случаи, при которых погрешность (см. ошибка перепроицирования) решений будет примерно равна, из‑за чего нельзя будет однозначно утверждать о однозначной правильности того или иного решения. Например IPPE метод может иметь 2 вероятных решения, и иногда это проявляется в неоднозначности определения направления оси Z мировой системы координат:

Начало мировой системы координат находится в центре квадратного маркера. (Фото взято из проблемы от catree, указанной в репозитории OpenCV на GitHub. Изначально данное фото является демонстрацией того, что применение различных методов может давать различные результаты, но этот эффект может происходить и при использовании одного и того же метода.)

Избавление от неоднозначности решения не имеет универсального подхода, но для этого могут использоваться различные методы: фильтрация, увеличение количества некомпланарных точек (для методов, поддерживающих их), «жёсткая» фиксация оси и другие.

Также на точность результата может влиять разрешение камеры и точность нахождения координат (px) на изображении (см. cv2.cornerSubPix).

Так или иначе в OpenCV есть целый набор функций, реализующих различные методы решения задачи PnP:

cv2.solvePnP(
             objectPoints,
             imagePoints,
             cameraMatrix,
             distCoeffs,
             rvec = np.array([]),
             tvec = np.array([]),
             useExtrinsicGuess = false,
             flags = cv2.SOLVEPNP_ITERATIVE
)

# Функция решения задачи PnP
#
# ВВОД:
# - objectPoints - массив (Nx1x3/1xNx3/Nx3x1) координат (м) точек в мировой системе
# координат
# - imagePoints - массив (Nx1x2/1xNx2/Nx2x1) координат (px) соответствующих проекций
# точек из objectPoints на изображении
# - cameraMatrix - матрица камеры, записанная в массив
# - distCoeffs - массив, содержащий коэффициенты дисторсии (если на изображении уже
# удалены искажения, то массив должен быть пустой)
# - rvec и
# - tvec - выходные векторы поворота/свдига (наиболее вероятное решение), 
# записанные в массивы,  (могут использоваться в качестве начальных
# значений, при useExtrinsicGuess = true, для их уточнения/оптимизации, но в остальных
# случаях в Python могут не указываться)
# - useExtrinsicGuess - bool, включающая алгоритма уточнения/оптимизации, при
# flags = cv2.SOLVEPNP_ITERATIVE
# - flags - число (константа из cv2), устанавливающая метод
# решения задачи PnP
#
# ВЫВОД:
# - retval - bool, показывающая, получилось ли выполнить решение задачи PnP
# - rvec и 
# - tvec - искомые векторы поворота/сдвига, записанные в массивы
# 
# Доступные параметры (константы из cv2) для flags:
# - cv2.SOLVEPNP_ITERATIVE - метод DLT (Левенберга-Марквардта)
# - cv2.SOLVEPNP_EPNP - метод EPnP
# - cv2.SOLVEPNP_P3P - метод P3P (для четырёх точек)
# - cv2.SOLVEPNP_IPPE - метод IPPE
# - cv2.SOLVEPNP_IPPE_SQUARE - метод IPPE для четырёх точек, образующих квадрат с
# центром в мировой системе координат
# - cv2.SOLVEPNP_SQPNP - метод SQPNP 
# и другие (указаны в документации OpenCV)


cv2.solveP3P(
             objectPoints,
             imagePoints,
             cameraMatrix,
             distCoeffs,
             rvecs,
             tvecs,
             flags 
)
# Аналогичная cv2.solvePnP() функция, только для метода P3P с тремя точками и без
# аргумента useExtrinsicGuess

cv2.solvePnPRansac(
                   objectPoints,
                   imagePoints,
                   cameraMatrix,
                   distCoeffs,
                   rvec,
                   tvec,
                   useExtrinsicGuess,
                   iterationsCount,
                   reprojectionError,
                   confidence,
                   inliers,
                   flags
)
# Аналогичная cv2.solvePnP() функция, с использованием схемы  RANSAC для фильтрации
# ошибок


cv2.solvePnPGeneric()
# Аналогичная cv2.solvePnPRansac() функция, возвращающая всевозможные решения
# задачи PnP


cv2.aruco.estimatePoseSingleMarkers(
                                    corners,
                                    markerLength,
                                    cameraMatrix,
                                    distCoeffs,
                                    rvecs,
                                    tvecs,
                                    objPoints,
                                    estimateParameters
)
# !!! DEPRECATED !!! ФУНКЦИЯ УСТАРЕЛА И ОТСУТСТВУЕТ В НОВЕЙШИХ ВЕРСИЯХ OPENCV !!!
# Аналогичная cv2.solvePnP() функция специально для рассчётов относительно
# ArUco маркера. Некоторые аргументы отличаются.

# и другие

Результатом решения задачи PnP будут и , с чем сможем приступать непосредственно к нахождению координат.

Нахождение пространственных координат объекта в системе координат камеры

Как отмечалось выше $P_c=\ ^{c}T_{w}P_w$ , соответственно координаты любой точки в мировой (объектной) системе координат можно перевести в систему координат камеры умножением на матрицу перехода $\underset{4\times4}{^{c}T_{w}}$ , полученную из $\underset{3\times3}{R}$ и $\underset{3\times1}{t}$ которые в свою очередь получены из $\underset{3\times1}{rvec}$ и $\underset{3\times1}{tvec}$ .

Таким образом если , то . Тогда в случае с ArUco маркером, описанном выше (с началом системы координат в центре маркера), можно без расчётов утверждать, что координаты центра ArUco маркера в системе координат камеры равны . И в этом же случае можно судить о том, что матрица поворота описывает поворот самого ArUco маркера. При этом в коде ниже поворот ArUco маркера для удобства будем выражать в углах Эйлера (см. статью о преобразовании матрицы поворота в углы Эйлера).

Итак, реализуем данный подход в коде.

re3d.py

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

"""re3d / 2025 Leonid Briskindov"""
import cv2
import numpy as np
import numpy.typing as npt


def getCTW(rvec: cv2.typing.MatLike, tvec: cv2.typing.MatLike) -> npt.ArrayLike:
    """
    Функция преобразования rvec (векора поворота) и tvec (вектора сдвига) в cTw (матрицу перехода)
    """
    
    rot_mat, jacobian_mat = cv2.Rodrigues(rvec)
    mat = np.array([
        [rot_mat[0][0], rot_mat[0][1], rot_mat[0][2], tvec[0][0]],
        [rot_mat[1][0], rot_mat[1][1], rot_mat[1][2], tvec[1][0]],
        [rot_mat[2][0], rot_mat[2][1], rot_mat[2][2], tvec[2][0]],
        [0, 0, 0, 1]
    ])
    return mat


def estimatePoseSingleMarkers(marker_points: cv2.typing.MatLike,
                              marker_size: float,
                              cameraMatrix: cv2.typing.MatLike,
                              distCoeffs: cv2.typing.MatLike,
                              useEPNP: bool = False) -> tuple[bool, cv2.typing.MatLike, cv2.typing.MatLike]:
    """
    Функция по образу устаревшей cv2.aruco.estimatePoseSingleMarkers с немного отличающимися аргументами:
    ! marker_points - позиции (px) вершин (углов) маркера на изображении
    ! marker_size - размер маркера в мировой системе координат (реальный размер, например в метрах)
    ! cameraMatrix - внутренняя матрица камеры
    ! distCoeffs - коэффициенты дисторсии камеры (на неискажённых изображениях ожидается пустой массив)
    ! useEPNP - использовать EPNP метод решения задачи Perspective-n-Point вместо IPPE (SQUARE)
    """
    marker_world_points = np.array([[-marker_size / 2, marker_size / 2, 0],
                                    [marker_size / 2, marker_size / 2, 0],
                                    [marker_size / 2, -marker_size / 2, 0],
                                    [-marker_size / 2, -marker_size / 2, 0]], dtype=np.float32)
    if useEPNP:
        return cv2.solvePnP(marker_world_points, marker_points, cameraMatrix, distCoeffs, flags=cv2.SOLVEPNP_EPNP)
    else:
        return cv2.solvePnP(marker_world_points, marker_points, cameraMatrix, distCoeffs, flags=cv2.SOLVEPNP_IPPE_SQUARE)


def get3D4Points(points: list, rvec: cv2.typing.MatLike, tvec: cv2.typing.MatLike) -> npt.ArrayLike:
    """
    Функция применения преобразования, описываемого векторами rvec и tvec, к четырём точкам (из входного массива points)
    """
    mat = getCTW(rvec, tvec)
    camera_points = np.array([
        np.dot(mat, points[0]),
        np.dot(mat, points[1]),
        np.dot(mat, points[2]),
        np.dot(mat, points[3])
    ])
    return camera_points[:, :-1]


def get3DMarkerCorners(marker_size: float, rvec: cv2.typing.MatLike, tvec: cv2.typing.MatLike) -> npt.ArrayLike:
    """
    Функция применения преобразования, описываемого векторами rvec и tvec, к вершинам (углам) ArUco маркера c размером в мировой системе координат (реальным размером) = marker_size (например в метрах)
    """
    marker_world_points = np.array([[-marker_size / 2, marker_size / 2, 0, 1],
                                    [marker_size / 2, marker_size / 2, 0, 1],
                                    [marker_size / 2, -marker_size / 2, 0, 1],
                                    [-marker_size / 2, -marker_size / 2, 0, 1]], dtype=np.float32)
    return get3D4Points(marker_world_points, rvec, tvec)


def getKnew(K: cv2.typing.MatLike, c: float) -> npt.ArrayLike:
    """
    Функция пропорционального изменения фокусного расстояния для нахождения новой внутренней матрицы (Knew)
    Аналогичного результата можно добиться функцией cv2.fisheye.estimateNewCameraMatrixForUndistortRectify, хотя изначально она создана для других целей
    """
    Knew = K.copy()
    Knew[(0, 1), (0, 1)] = c * Knew[(0, 1), (0, 1)]
    return Knew


def getFixedZWPosAll(src: [tuple, list, npt.ArrayLike], Zw: float, cameraMatrix: cv2.typing.MatLike, wTc: npt.ArrayLike) -> (npt.ArrayLike, npt.ArrayLike):
    """
    Функция нахождения позиции точки в мировой системе координат и системе координат камеры с известной координатой Z в мировой системе координат и матрицей перехода из системы координат камеры в мировую систему коодринат:
    ! src - позиция (px) искомой точки на изображении
    ! Zw - координата Z точки в мировой системе координат
    ! cameraMatrix - внутренняя матрица камеры
    ! wTc - матрица перехода из системы координат камеры в мировую систему коодринат (обратная матрица к cTw)
    """
    fx, fy = cameraMatrix[0][0], cameraMatrix[1][1]
    cx, cy = cameraMatrix[0][2], cameraMatrix[1][2]

    r11, r12, r13, tx = wTc[0]
    r21, r22, r23, ty = wTc[1]
    r31, r32, r33, tz = wTc[2]

    u, v = src

    Zc = (Zw - tz) / (r31 * (u - cx) / fx + r32 * (v - cy) / fy + r33)

    Xc = (u - cx) * Zc / fx
    Yc = (v - cy) * Zc / fy

    Xw = r11 * Xc + r12 * Yc + r13 * Zc + tx
    Yw = r21 * Xc + r22 * Yc + r23 * Zc + ty

    return np.array([Xw, Yw, Zw], dtype=np.float32), np.array([Xc, Yc, Zc], dtype=np.float32)


def getFixedZWPos(src: [tuple, list, npt.ArrayLike], Zw: float, cameraMatrix: cv2.typing.MatLike, wTc: npt.ArrayLike) -> npt.ArrayLike:
    """
    Функция аналогична getFixedZWPosAll, но возвращает только позицию в мировой системе координат
    """
    return getFixedZWPosAll(src, Zw, cameraMatrix, wTc)[0]


def positionMarker(
        marker_corners: cv2.typing.MatLike, marker_size: float, cameraMatrix: cv2.typing.MatLike,
        distCoeffs: cv2.typing.MatLike = np.array([],dtype=np.float32)
    ) -> ([npt.ArrayLike, npt.ArrayLike], [npt.ArrayLike, npt.ArrayLike]):
    """
    Функция нахождения позиции и углов Эйлера ArUco маркера в системе координат камеры.
    ! marker_corners - позиции (px) вершин (углов) маркера на изображении
    ! marker_size - размер маркера в мировой системе координат (реальный размер, например в метрах)
    ! cameraMatrix - внутренняя матрица камеры
    ! distCoeffs - коэффициенты дисторсии камеры (на неискажённых изображениях ожидается пустой массив)
    """
    marker_points = np.array(
        [[-marker_size / 2, marker_size / 2, 0], [marker_size / 2, marker_size / 2, 0],
         [marker_size / 2, -marker_size / 2, 0], [-marker_size / 2, -marker_size / 2, 0]], dtype=np.float32) # Определение объектной модели ArUco маркера
                                                                                                             # Вершины (углы) ArUco маркера описывают квадрат с длиной стороны marker_size и с центром в начале системы координат
    ret, rvec, tvec = cv2.solvePnP(
        marker_points, marker_corners, cameraMatrix, distCoeffs, flags=cv2.SOLVEPNP_IPPE_SQUARE
    )                                                                                                        # Получение вектора поворота и сдвига rvec и tvec, описывающих преобразование из мировой системы координат в систему
                                                                                                             # координат камеры

    assert ret                                                                                               # Проверка на успешность выполнения cv2.solvePnP на предыдущем шаге

    x, y, z = tvec.ravel()                                                                                   # Разложение tvec на x, y, z для удобства формирования вывода (tvec / t -> x, y, z)
    rot_mat, jacobian_mat = cv2.Rodrigues(rvec)                                                              # Нахождение матрицы поворота из вектора поворота (rvec -> R)
    ax = np.arctan2(rot_mat[2][1], rot_mat[2][2])                                                            # Нахождение угла Эйлера из матрицы поворота относительно OX
    ay = np.arctan2(-1 * rvec[2][0], np.sqrt((rot_mat[2][1]) ** 2 + (rot_mat[2][2]) ** 2))                   # Нахождение угла Эйлера из матрицы поворота относительно OY
    az = np.arctan2(rot_mat[1][0], rot_mat[0][0])                                                            # Нахождение угла Эйлера из матрицы поворота относительно OZ
    return np.array([[x, y, z], [ax, ay, az]], dtype=np.float32), np.array([rvec, tvec], dtype=np.float32)   # Возврат функции ([Координаты, Углы], [rvec, tvec])

Для краткости в дальнейшей статье будут использоваться функции из данного скрипта. Конкретно текущее решение для ArUco маркера будет опираться на функцию:

re3d.positionMarker(
                     marker_corners,
                     marker_size,
                     cameraMatrix,
                     distCoeffs=np.array([],dtype=np.float32)
 ) # Для нахождения позиции и вращения ArUco маркера

Альтернативно могут применяться:

re3d.estimatePoseSingleMarkers()
re3d.get3DMarkerCorners()
# и другие

Но опять же, всё решение изначально основывается на функции:

cv2.solvePnP(
             marker_points,
             marker_corners,
             cameraMatrix,
             distCoeffs
) # Для нахождения rvec, tvec

import cv2
import numpy as np
import re3d
# Импорт библиотек

CAM_RESOLUTION = (1920, 1080) # Разрешение камеры
ARUCO_SIZE = 0.0585           # Размер ArUco маркера около 6 см
                              # (ArUco маркер распечатан, вырезан и замерен линейкой)
CAM_ID = 1                    # ID камеры (при подключении по USB)

with open('calibration/param.txt') as f: 
    cameraMatrix = eval(f.readline())
    distCoeffs = eval(f.readline())
# Загрузка из 'calibration/param.txt' матрицы камеры и коэффициентов дисторсии

w, h = CAM_RESOLUTION # Разбиение разрешения камеры на длину и высоту для удобства
cap = cv2.VideoCapture(CAM_ID, cv2.CAP_DSHOW) # Создание объекта камеры
cap.set(cv2.CAP_PROP_FRAME_WIDTH, w)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, h)
cap.set(cv2.CAP_PROP_FPS, 30)
cap.set(cv2.CAP_PROP_AUTOFOCUS, 0)
cap.set(cv2.CAP_PROP_FOCUS, 250)
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc(*'MJPG'))
# Настройка камеры

aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_250)
# Загрузка стандартного словаря ArUco маркеров 4x4
parameters = cv2.aruco.DetectorParameters()
# Создание объекта параметров детектора ArUco маркеров
detector = cv2.aruco.ArucoDetector(aruco_dict, parameters)
# Создание объекта детектора ArUco маркеров

if __name__ == "__main__":
    while cv2.waitKey(1000 // 60) != ord("q"): 
        # Обновление изображения до нажатия на клавишу "q"  
        ret, frame = cap.read()
        assert ret
        # Получение изображения с камеры
        img = cv2.fisheye.undistortImage(frame, cameraMatrix, D=distCoeffs, Knew=cameraMatrix)
        # Удаление искажения с камеры     
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        # Создание чёрно-белого изображения камеры
        corners, ids, rejected = detector.detectMarkers(gray)
        # Поиск ArUco маркеров
        if ids is not None:
            # Если найден(-ы) ArUco маркер(-ы):
            for marker in range(len(ids)):
                # Выполнить для каждого найденного ArUco маркера
                idx = int(ids[marker][0]) # Код (номер) ArUco маркера
                cornersx = corners[marker] # Вершины (углы) ArUco маркера
                position, mat = re3d.positionMarker(cornersx, ARUCO_SIZE, cameraMatrix)
                # Рассчёт позиции ArUco маркера (смотри re3d.py)
                x, y, z = position[0] # Координаты
                rx, ry, rz = map(np.degrees, position[1]) # Углы Эйлера
                rvec, tvec = mat # rvec, tvec (для отрисовки осей ArUco маркера)
                img = cv2.drawFrameAxes(img, cameraMatrix, np.array([]), rvec, tvec, 0.1, 5)
                #Отрисовка осей ArUco маркера
                img_pos = np.array(cornersx[0][0]).astype(np.int16)
                #Позиция (px) вершины (угла) ArUco маркера на изображении
                img = cv2.putText(
                    img,
                    f"x:{x:.2f}/y:{y:.2f}/z:{z:.2f}",
                    img_pos, cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 0, 255), 2
                )
                # Отрисовка координат ArUco маркера на изображении
                img = cv2.putText(
                    img,
                    f"rx:{rx:.2f}/ry:{ry:.2f}/rz:{rz:.2f}",
                    [img_pos[0], img_pos[1]+20], cv2.FONT_HERSHEY_SIMPLEX,
                    0.4, (255, 0, 255), 2
                )
                # Отрисовка углов Эйлера ArUco маркера на изображении

        cv2.imshow("Display", img) # Вывод на экран изображения
        
    cap.release()
    cv2.destroyAllWindows()
    # Закрытие окон после завершения работы

В результате имеем:

Демонстрация результата: координаты определяются в метрах, углы определяются в градусах (GIF анимация)

Результат (по кадрам)

rz показывает угол прецессии/рыскания
rx близок к 180° и ry близок к 0°, когда плоскость ArUco маркера параллельна плоскости стола, т. к. ось направлена из камеры, в данном случае примерно вниз, а осьArUco маркера направлена примерно вверх.

**x: 0.09 / y: -0.14 / z: 0.66 | rx: 172.49 / ry: 6.07 / rz: -0.22**

**x:0.09 / y: -0.04 / z:0.66 | rx: -158.49 / ry: -17.19 / rz: 91.55**

**x: 0.08 / y: -0.24 / z: 0.66 | rx: -144.15 / ry: -4.74 / rz: -5.96**

**x: -0.39 / y: 0.51 / z: 1.49 | rx: 179.96 / ry: 0.04 / rz: -4.91**

x: 0.03 / y: 0.02 / z: 0.66 | rx: 173.16 / ry: 10.55 / rz: 178.47; x и y близки к 0, так как проекция центра маркера близка к главной точке (оптическому центру)

**x: 0.16 / y: -0.11 / z: 0.66 | rx: -177.00 / ry: -6.45 / rz: -131.44**

На этом задача определения пространственных координат объектов решена. Далее разберём некоторые частные случаи.

Частные случаи задачи

Определение пространственных координат точки, при известной Zc

Как было написано выше, нельзя «в лоб» определить пространственную позицию единственной точки по изображению, однако зная это можно сделать (формула для и ниже). Ввиду неадаптивности данный метод в таком виде малоприменим, хотя имеет место быть, особенно в случаях невозможности определения модели искомого объекта и ограниченности движения точки только плоскостью, перпендикулярной оси .

Выведение

$s\begin{bmatrix}u\\v\\1\end{bmatrix} = \begin{bmatrix}f_{x}\ 0 \ \ c_{x}\\0 \ \ f_{y} \ c_{y} \\ 0 \ \ \ 0\ \ \ 1\end{bmatrix} \begin{bmatrix}X_{c} \\ Y_{c} \\ Z_{c} \end{bmatrix} \Leftrightarrow \begin{bmatrix}su\\sv\\1\cdot s \end{bmatrix}=\begin{bmatrix}f_{x}X_{c}+0\cdot Y_{c}+c_{x}Z_{c} \\ 0\cdot X_{c} + f_{y}Y_{c}+c_{y}Z_{c} \\ 0 \cdot X_{c} + 0 \cdot Y_{c} + 1 \cdot Z_{c}\end{bmatrix} \Leftrightarrow \\ \Leftrightarrow \begin{bmatrix} su \\ sv \\ s \end{bmatrix}= \begin{bmatrix} f_{x}X_{c}+c_{x}Z_{c}\\f_{y}Y_{c}+c_{y}Z_{c} \\ Z_{c} \end{bmatrix} \Leftrightarrow \begin{bmatrix}Z_{c}u\\Z_{c}v\\Z_{c} \end{bmatrix}=\begin{bmatrix}f_{x}X_{c}+c_{x}Z_{c}\\f_{y}Y_{c}+c_{y}Z_{c}\\Z_{c} \end{bmatrix} \Leftrightarrow \\ \Leftrightarrow \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}=\begin{bmatrix} \frac{f_{x}X_{c}+c_{x}Z_{c}}{Z_c} \\ \frac{f_{y}Y_{c}+c_{y}Z_{c}}{Z_{c}} \\1\end{bmatrix}$

$\begin{cases}u=\frac{f_{x}X_{c}}{Z_{c}}+c_{x} \\ v = \frac{f_{y}Y_{c}}{Z_{c}} + c_{y} \end{cases} \space \ \Leftrightarrow \ \space \begin{cases}X_{c}=\frac{(u-c_{x})Z_{c}}{f_{x}} \\ Y_{c} = \frac{(v-c_{y})Z_{c}}{f_{y}}\end{cases}$

Преобразование с заданной Zw для искомой точки

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

Решим задачу PnP для объекта, описывающего мировую систему координат, представив пару $rvec,\ tvec$ в виде матрицы перехода $^{c}T_{w}$ . $^{c}T_{w}$ описывает преобразование из мировой (объектной) системы координат в систему координат камеры, а обратная матрица перехода $^{w}T_{c} = (^{c}T_{w})^{-1} \text{, при } \det\ ^{c}T_{w} \neq 0$ описывает преобразование из системы координат камеры в мировую систему координат (обратное преобразование), тогда $P_{w}\ =\ ^{w}T_{c}P_{c}$ .

Представим

$^{w}T_{c} = \begin{bmatrix} r_{11} \ r_{12} \ r_{13} \ t_{x} \\ r_{21} \ r_{22} \ r_{23} \ t_{y} \\ r_{31} \ r_{32} \ r_{33} \ t_{z} \\ 0 \ \ \ \ 0 \ \ \ \ 0 \ \ \ \ 1\end{bmatrix} \text{, }\ P_{w}=\begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w} \\ 1 \end{bmatrix} \text{, }\ P_{c}=\begin{bmatrix} X_{c} \\ Y_{c} \\ Z_{c} \\ 1 \end{bmatrix}$

(в однородных координатах), тогда:

Выведение

$\begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w} \\ 1 \end{bmatrix} = \begin{bmatrix} r_{11} \ r_{12} \ r_{13} \ t_{x} \\ r_{21} \ r_{22} \ r_{23} \ t_{y} \\ r_{31} \ r_{32} \ r_{33} \ t_{z} \\ 0 \ \ \ \ 0 \ \ \ \ 0 \ \ \ \ 1\end{bmatrix}\begin{bmatrix} X_{c} \\ Y_{c} \\ Z_{c} \\ 1 \end{bmatrix} \Leftrightarrow \\ \Leftrightarrow \begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w} \\ 1 \end{bmatrix} = \begin{bmatrix} r_{11}X_{c}+r_{12}Y_{c}+r_{13}Z_{c}+t_{x} \\ r_{21}X_{c} + r_{22}Y_{c} + r_{23}Z_{c} + t_{y} \\ r_{31}X_{c}+r_{32}Y_{c}+r_{33}Z_{c} + t_{z} \\ 0\ \ +\ \ 0\ \ +\ \ 0\ \ +\ \ 1 \end{bmatrix}$

Таким образом и выражено через и $^{w}T_{c}$ . Выведение и было выше. Выразим :

$Z_{w} = r_{31}X_{c} + r_{32}Y_{c} +r_{33}Z_{c}+t_{z}$ (из выражения выше)
$Z_{w} = \frac{r_{31}(u-c_{x})Z_{c}}{f_{x}}+\frac{r_{32}(v-c_{y})Z_{c}}{f_{y}}+r_{33}Z_{c} + t_{z}$

(выразили и через )
$Z_{w} - t_{z} = Z_{c} ( \frac{r_{31}(u-c_{x})}{f_{x}} + \frac{r_{32}(v-c_{y})}{f_{y}} + r_{33} )$

(вынесли общий множитель)
$Z_{c} = \frac{(Z_{w} - t_{z})}{(\frac{r_{31} (u - c_{x})}{f_{x}}+ \frac{r_{32}(v-c_{y})}{f_{y}}+r_{33})}$

(выразили )

$\begin{cases}Z_{c} = \frac{(Z_{w} - t_{z})}{(\frac{r_{31} (u - c_{x})}{f_{x}}+ \frac{r_{32}(v-c_{y})}{f_{y}}+r_{33})} \\ X_{c}=\frac{(u-c_{x})Z_{c}}{f_{x}} \\ Y_{c} = \frac{(v-c_{y})Z_{c}}{f_{y}} \\ X_{w} = r_{11}X_{c} + r_{12}Y_{c} + r_{13}Z_{c} + t_{x} \\ Y_{w} = r_{21}X_{c} + r_{22}Y_{c} + r_{23}Z_{c} + t_{y} \end{cases}$

Для данного решения могут пригодиться следующие методы:

re3d.getCTW(rvec, tvec)                        # Для получения матрицы перехода (cTw)
np.linalg.det(a)                               # Для проверки корректности матрицы
np.linalg.inv(a)                               # Для нахождения обратной марицы (wTc)
re3d.getFixedZWPos(src, Zw, cameraMatrix, wTc) # Для нахождения координат точки

Определение позиции объекта A в системе координат объекта B

Разберём случай, схожий с предыдущим, но когда необходимо найти координаты некоторого объекта в координатной системе другого объекта .

Так как у нас имеется две мировые (объектные) системы координат для объектов и соответственно, далее вместо будем использовать и . То есть координаты точки в системе объекта обозначим за , а в системе объекта обозначим за .

Решим задачу PnP для каждого объекта, представив пары $rvec,\ tvec$ виде матриц перехода $^{c}T_{A}$ и $^{c}T_{B}$ соответственно. Затем для $^{c}T_{B}$ найдём обратное преобразование $^{B}T_{c} = (^{c}T_{B})^{-1} \text{, при } \det\ ^{c}T_{B} \neq 0$ , тогда $P_{B}\ =\ ^{B}T_{c}P_{c}$ , при этом $P_{c}\ =\ ^{c}T_{A}P_{A}$ (из модели камеры‑обскуры). Таким образом: $P_{B}\ =\ ^{B}T_{c}\ ^{c}T_{A}\ P_{A}$

Схема преобразований: $O_{A} \stackrel{^{c}T_{A}}{\rightarrow} O_{c} \stackrel{^{B}T_{c}}{\rightarrow} O_{B}$

Схема преобразований: $O_{A} \stackrel{^{c}T_{A}}{\rightarrow} O_{c} \stackrel{^{B}T_{c}}{\rightarrow} O_{B}$

Для данного решения могут пригодиться следующие методы:

re3d.getCTW(rvec, tvec)                        # Для получения матрицы перехода (cTw)
np.linalg.det(a)                               # Для проверки корректности матрицы
np.linalg.inv(a)                               # Для нахождения обратной марицы (wTc)
np.dot(a, b)                                   # Для применения преобразований

Результаты решения частных случаев

Результат

Изображение демонстрационного стенда (фанерная подложка 0.35×0.35м) с удалённым эффектом (undistorted) «рыбий глаз» (fisheye); фанерная подложка с изображёнными ArUco маркерами является объектом (), задающим систему координат в которой рассчитываются пространственные координаты центра ArUco маркера () на фанерной коробке; целевая позиция (0.15; 0.15; -0.208)

Красная точка — главная точка (оптический центр изображения); «Fixed Z» — преобразование с заданной для искомой точки (0.15; 0.154; -0.22); «SolvePnP» — определение позиции объекта в системе координат объекта (0.15; 0.15; -0.208)

В области $1\text{m}$ , проявляется невысокая погрешность (до $\pm1.2\text{cm}$ ) с использованием решения задачи PnP (при использовании IPPE метода) для обоих объектов. Преобразования с заданной для искомой точки даёт ещё меньшую погрешность, но стоит учитывать, что как раз из‑за фиксированногоданный метод не универсален и не рассчитан на изменение высоты точки.

Это справедливо и в других случаях:

Изображение поля робототехнических соревнований Eurobot 2025; Соревновательное поле с 4×4 ArUco маркерами является объектом (), задающим систему координат в которой рассчитываются пространственные координаты центра 3×3 ArUco маркера () на фанерной коробке; целевая позиция (1.5; 1.0; -0.45)

Красная точка — главная точка (оптический центр изображения); «Fixed Z» — преобразование с заданной для искомой точки (1.492; 0.982; -0.409); «SolvePnP» — определение позиции объекта в системе координат объекта (1.5; 1.01; -0.45)

Изображение с камеры без эффекта «рыбий глаз»; на иллюстрации показано определение позиции объекта в системе координат объекта

Заключение

В результате мы научились определять пространственные координаты объектов по камере. Весь код, используемый для подготовки статьи опубликован на GitHub. Но нет предела совершенству!

По теме существует множество учебного материала (в частности по фотограмметрии), а также технологии, которые потенциально могут улучшить результат:

Спасибо за внимание!

Комментарии (2)

Daddy_Cool
28.08.2025 21:58
#28769060
Очень интересно!
Есть ли простой способ определять скорость объектов?

ret77876
28.08.2025 21:58
#28769146
Отличная статья! А проводились/планируются ли эксперименты по исследованию погрешности определения координат объекта? И на самом деле кроме погрешности ещё интересны отклонения/шумы при разном нахождении маркера относительно оптического центра камеры.

Определение положения объектов на изображении: как найти пространственные координаты объекта, используя OpenCV +22

Оглавление

Краткая памятка по линейной алгебре

Скаляр

Матрица

Вектор

Произведение матрицы и скаляра (числа)

Произведение матрицы и матрицы

Нахождение обратной матрицы

Однородная система координат

Формирование изображения

Камера-обскура

Математика камеры

Виртуальная (фронтальная) плоскость

Системы координат и координатные пространства

Внутренние параметры камеры и калибровка

Модель камеры-обскуры

Определение объекта (координатной модели)

Задача Perspective-n-Point (PnP)

Нахождение пространственных координат объекта в системе координат камеры

Частные случаи задачи

Определение пространственных координат точки, при известной Zc

Преобразование с заданной Zw для искомой точки

Определение позиции объекта A в системе координат объекта B

Результаты решения частных случаев

Заключение

Комментарии (2)

Daddy_Cool

ret77876