Русский
Русский
English
Авторизация
Ip-адрес
Восстановление пароля
Регистрация
Статистика
Реклама

Slam

Одноглазый глубиномер

21.09.2020 10:09:24 | Автор: admin

Недавно вышла интересная статья от FaceBook о том как можно делать неплохой 3D с монокулярных камер. Статья не очень применимая на практике. Но по качеству картинки завораживает:

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

Начнём с основ. Существует несколько способов снять 3D:

  • Активная 3д камера (лидар, проекционная, TOF). Излучаем-измеряем.

  • Стерео камера. Оцениваем глубину из геометрии.

  • Монокулярные. Используем для оценки глубины либо опыт, либо сдвиг камеры.

В этой статье мы будем разбирать монокулярное. В чём его сложность? Да вот:

Эта картинка очень хорошо описывает основную проблему. Даже человек не может оценить глубину с одного ракурса.Что-то может, но точность будет не очень + возможна масса ошибок.

Как с этим борются? Камера должна двигаться, а место действия не должно меняться. Алгоритмы которые могут собрать из это картинку называются SLAM. Логика там более-менее одинаковая (описываю грубо):

  • На первом кадре выделяем набор фич и кладём их в общий мешок.

  • На каждом новом кадре выделяем фичи, сравниваем их с фичами из мешка.

  • Найдя пересекающиеся - можно оценить текущий сдвиг относительно прочих кадров, а для всех фич из мешка - оценить положение

Вот пример того как это работает из далёкого 2014 года (но вообще было и сильно раньше):

У алгоритмов есть одна забавная бага. Вы не можете знать абсолютные расстояния в полученной сцене. Только относительные.

Но это не сложно исправить замерив хотя бы один объект.

Хорошие SLAM-алгоритмы появились ещё лет десять назад. Сейчас вы можете найти их в AR Core или ARKit, которые доступны почти на каждом телефоне. Вот как это будет выглядеть прямо из коробки без всяких лидаров:

Эти же алгоритмы используются в:

  • Алгоритмах восстановления 3Д по серии кадров (фотограмметрия)

  • Алгоритмах дополненной реальности для редактирования видео

  • Алгоритмах ориентации в дронах

  • И прочее и прочее

Из лучших доступных OpenSource алгоритмов такого плана сейчас есть COLMAP. Он удобно упакован, много исследователей используют его для создания базового решения.
Но в чём проблем таких алгоритмов?

Все SLAM алгоритмы не дают достаточную плотность 3D:

colmapcolmapLSD-SLAMLSD-SLAM

Как с этим борются?

Подходов много. Раньше были аналитические, а сейчас более популярные - через нейронные сети. Собственно, о двух-трех самых популярных и интересных мы и поговорим.
Мы будем говорить именно про самые популярные. Почему не про самые точные? Это интересный вопрос. Если мы говорим самые точные, то мы должны, например, формализовать по какому датасету сравниваем точности. Таких датасетов много.
Но вот суровый минус - они все специфичные. И зачастую сети имеющие хорошие точности на таких датасетах - работают только на них. Хорошим примером является KITTY:

Сети оптимизированные и хорошо обученные по нему - будут не очень хорошо работать на других данных.

При этом зачастую восстановление 3Д это уже не простой констракт плана проверить на датасете глубину, а что-то более сложное, такое как проверить точность восстановления объекта, или оценить точность на всей видеопоследовательности. И тут уже датасетов будет мало, сложно по ним бенчмаркаться.

Если посмотреть на какой-нибудь сабтаск оценки глубины - Depth Estimation, то по каждому конкурсу будет всего одна-две работы.
Так что смысла говорить о самом точном нет. И я буду рассказывать, на свой взгляд о самом интересном и полезном, что показывает разные подходы.

MiDaS

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

  • Смогли одновременно обучаться на множестве датасетов с разнородными данными. На стереофильмах, на данных с датчиков глубины, на сканах местности, и.т.д.

  • Натренировали принципиально разные выходы модели для прогнозирования глубины + различные способы смешивания для получения оптимальной оценки.

  • Смогли сделать эту модель достаточно быстрой

Кроме того MiDaS доступен из коробки на разных платформах:

И то и то можно запустить минут за десять. А результат? В первом приближении как-то так:

Видно, что тут оценка глубины есть в каждой точке. Но сразу видно что:

  • Видна нестабильность глубины от кадра к кадру

  • Нет абсолютных значений размера (что типично для монокулярных задач)

  • Глубина достаточно неплохо обрабатывается, в том числе для объектов на похожем фоне

Но, естественно, доверять однозначно таким вещам нельзя:

Полный скан

Устранить дрожание во время кадра можно за счёт учёта соседних кадров и нормировки за их счёт. И тут на помочь приходят ранее упомянутые SLAM алгоритмы.С их помощью строят профиль перемещения камеры, делают привязку каждого кадра к общему плану.
Есть несколько алгоритмов которые такое реализуют. Например Атлас и DELTAS

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

Второй примерно так же, но чуть сложнее:

Есть чуть более хитрые подходы, когда нейронная сеть оценивает глубину как Мидас, и проецирует напрямую в TSDF.
Какая точность у таких методов? Конечно куда меньше чем у любой аппаратуры оснащенной нормальным 3D сканером. Например очередной сканер на iPhoneовском лидаре:

Consistent Video Depth Estimation

Можно ли добиться большего, но с монокулярной камеры? Да, можно. Но для этого надо отправляться в сторону алгоритмов которые оценивают глубину по серии кадров в последовательности. Примером такого алгоритма может быть Consistent Video Depth Estimation.
Его особенностью является интересный подход, когда берётся два снимка из последовательности, для которых через ранее упомянутый SLAM оценивают сдвиг. После чего эти два снимка служат инпутом для обучения модели под текущее видео:

Да-да, для каждого видео обучается (дообучается) отдельная модель.И как-бы основной минус такого подхода - его время. Расчет занимает 4-5 часов на 10-секундное видео. Но результат восхищает:

Почему я привожу тут видео из примеров? Банально. Потому что запустить эту конструкцию, даже при наличии colab по своему видео я не смог.

Но на текущий момент такая штука показывает State-Of-Art в распознавании глубины в видео. А больше примеров можно посмотреть тут.

В формате видео

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

Заключение

В целом, на мой взгляд, для серьёзных задач монокулярный depth не нужен. И в будущем, скорее всего, везде где нужен будет хороший depth - проще будет использовать какой-нибудь лидар, благо точность растет, а цена падает. И в лидарах и в стереокамерах вовсю используются те же самые нейросети. Только вот в лидарах и стереокамерах есть оценка реальной глубины по одному кадру - что делает их априори точнее.
Но это не отменяет того что сегодня Monocular Depth уже используется в десятках применений где он удобнее. И, возможно, в будущем, за счёт повышения качества, он станет использоваться ещё чаще.

p.s.

В последнее время делаю много мелких статей/видеороликов. Но так как это не формат хабра - то публикую их на своём канале (телега, вк). На хабре обычно публикую когда рассказ становится уже более самозамкнутым, иногда нужно 2-3 разных мини-рассказа на соседние темы.

Подробнее..
Категории: Работа с 3d-графикой , Блог компании recognitor , Машинное обучение , Обработка изображений , Ar и vr , 3d , Slam , Depth sensors , Depth estimation , 3d reconstruction , Midas

Свой AR. Маркеры

23.03.2021 14:22:27 | Автор: admin


Привет, Хабр.
По мере написания библиотеки в этой статье я хочу продолжить объяснять математику, лежащей в основе работы дополненной реальности. Результатом будет пример на игровом движке Unity, распознающий маркер и накладывающий на него трехмерную модельку. Библиотека пишется на C++ под Android, но фокус статьи будет направлен на математику. Эта статья, в отличии от предыдущей, будет ближе к практике, но если необходимо разобраться с основами векторной математики, то можно начать с нее.


Однородные координаты.


В предыдущей статье мы рассматривали преобразования в евклидовом пространстве $\mathbb{R}^2$, а сейчас ведем понятие проективного пространства $\mathbb{P}^2$ (projective space).
Чтобы перевести координаты точки из двумерного евклидова пространства в проективное, нужно умножить вектор координат на любой ненулевой скаляр $w$, а затем добавить $w$ в качестве последней компоненты: $\vec v = \begin{pmatrix} x & y \end{pmatrix}^T \in \mathbb{R}^2 \space \Rightarrow \space \vec p = \begin{pmatrix} x \cdot w & y \cdot w & w \end{pmatrix}^T \in \mathbb{P}^2$. Получаем однородные координаты точки (homogeneous coordinates). Так как $w$ свободная переменная, то одной точке евклидова пространства принадлежит бесконечное множество точек проективного пространства. Перевод обратно выполняется делением на последнюю компоненту: $\vec p = \begin{pmatrix} x & y & w \end{pmatrix}^T \in \mathbb{P}^2 \space \Rightarrow \space \vec v = \begin{pmatrix} \frac{x}{w} & \frac{y}{w} \end{pmatrix}^T \in \mathbb{R}^2$. Удобно в качестве значения $w$ брать 1: $\vec v = \begin{pmatrix} x & y \end{pmatrix}^T \in \mathbb{R}^2 \space \Rightarrow \space \vec p = \begin{pmatrix} x & y & 1 \end{pmatrix}^T \in \mathbb{P}^2$. Перевод обратно в таком случае также упрощается: $\vec p = \begin{pmatrix} x & y & 1 \end{pmatrix}^T \in \mathbb{P}^2 \space \Rightarrow \space \vec v = \begin{pmatrix} x & y \end{pmatrix}^T \in \mathbb{R}^2$.
Можно заметить, что умножение однородных координат на скаляр не меняет координат соответствующей точки в евклидовом пространстве: $\vec{p_B} = \vec{p_A} \cdot s = \begin{pmatrix} x \cdot s & y \cdot s & w \cdot s \end{pmatrix}^T \in \mathbb{P}^2 \space \Rightarrow \space \vec v = \begin{pmatrix} \frac{x \cdot s}{w \cdot s} & \frac{y \cdot s}{w \cdot s} \end{pmatrix}^T = \begin{pmatrix} \frac{x}{w} & \frac{y}{w} \end{pmatrix}^T \in \mathbb{R}^2$.
Также, если последняя компонента равна 0, то такую точку мы не сможем перевести в евклидово пространство: $\vec p = \begin{pmatrix} x & y & 0 \end{pmatrix}^T \in \mathbb{P}^2$ такая точка называется точкой на бесконечности (point at infinity или ideal point).


Трансформации в однородных координатах.


Трансформации, которые мы делали в евклидовом пространстве, можно применять и для однородных координат.
Для примера возьмем поворот точек матрицей поворота: $\vec{v'} = R \vec{v} = \begin{pmatrix} R_{11} & R_{12} \\ R_{21} & R_{22} \end{pmatrix} \begin{pmatrix} v_x \\ v_y \end{pmatrix} = \begin{pmatrix} {v'}_x \\ {v'}_y \end{pmatrix}$.
В однородных координатах это принимает такую форму: $\vec{v'} = R \vec{v} = \begin{pmatrix} R_{11} & R_{12} & 0 \\ R_{21} & R_{22} & 0 \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} v_x \\ v_y \\ 1 \end{pmatrix} = \begin{pmatrix} {v'}_x \\ {v'}_y \\ 1 \end{pmatrix}$
А смещение выполнялось следующим образом: $\vec{v'} = \vec v + \vec t$. Мы не могли преобразовать смещение объекта в матричную операцию, так как она нелинейна в евклидовом пространстве. Но уже в проективном пространстве она становится линейной $T$: $\vec{v'} = \begin{pmatrix} 0 & 0 & t_x \\ 0 & 0 & t_y \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} v_x & v_y & 1 \end{pmatrix} = \begin{pmatrix} v_x + t_x \\ v_y + t_y \\ 1 \end{pmatrix}$.
Из предыдущей статьи мы помним, что матричные операции, можно объединять. Объединим поворот и смещение:
$\vec{v'} = T \cdot R \cdot \vec v = \begin{pmatrix} 0 & 0 & t_x \\ 0 & 0 & t_y \\ 0 & 0 & 1 \end{pmatrix} \cdot \begin{pmatrix} R_{11} & R_{12} & 0 \\ R_{21} & R_{22} & 0 \\ 0 & 0 & 1 \end{pmatrix} \cdot \begin{pmatrix} v_x \\ v_y \\ 1 \end{pmatrix}$


$\vec{v'} = \begin{pmatrix} R_{11} & R_{12} & t_x \\ R_{21} & R_{22} & t_y \\ 0 & 0 & 1 \end{pmatrix} \cdot \begin{pmatrix} v_x \\ v_y \\ 1 \end{pmatrix}$


Метод наименьших квадратов


Прежде чем двигаться дальше, вооружимся новым инструментом методом наименьших квадратов (МНК).
Возьмем для примера такую систему линейных уравнений:
$\begin{cases} A_{11} \cdot x_1 + A_{12} \cdot x_2 = b_1 \\ A_{21} \cdot x_1 + A_{22} \cdot x_2 = b_2 \\ A_{31} \cdot x_1 + A_{32} \cdot x_1 = b_3\end{cases}$
Эту систему можно представить в матричном виде:
$\begin{pmatrix} A_{11} & A_{12} \\ A_{21} & A_{22} \\ A_{31} & A_{32} \end{pmatrix} \cdot \begin{pmatrix} x_1 \\ x_2 \end{pmatrix} = \begin{pmatrix} b_1 \\ b_2 \\ b_3 \end{pmatrix} = A \cdot \vec{x} = \vec{b}$.
Неизвестных у нас два, количество уравнений три, значит это переопределенная система уравнений. А значит решения одних уравнений может противоречить другим, и система может не иметь точного решения. Ошибка получаемых решений это обычное дело для вычислительной математики, нужно только ее минимизировать. В качестве ошибки возьмем сумму квадратов разницы: пусть $\vec{b'} = A \cdot \vec{x}$, $\vec{r} = (\vec{b'} - \vec{b})$ вектор остатков, $e(\vec{x}) = \sum{{r_i}^2} = \sum{({b'}_i - b_i)^2}$ функция ошибки. $e(\vec{x}) \rightarrow min$.
В матричном виде задачу минимизации можно записать так: $(A \vec x - \vec b)^T (A \vec x - \vec b) \rightarrow min$.
Решение нашей системы:
$A \cdot \vec x = \vec b \space \Rightarrow \space A^T \cdot A \cdot \vec x = A^T \vec b \space \Rightarrow \space \vec x = (A^T \cdot A)^{-1} \cdot A^T \cdot \vec b$
$\vec x = (A^T A)^{-1} A^T \vec b$
Метод можно применять и для системы нелинейных уравнений. Мы определили функцию ошибки как сумму квадратов остатков. Дальше нам нужно найти минимум этой функции. Минимум функции нужно искать в ее экстремумах. Экстремумы находятся там, где производные функции равны нулю:
$\begin{cases}\frac{\partial e(\vec x)}{\partial x_0} = 0 \\ .. \\ \frac{\partial e(\vec x)}{\partial x_n} = 0 \end{cases}$
Получим систему уравнений, которая в отличии от предыдущей, будет точно не переопределена. Если система уравнений линейная, то решение будет только одно и получить его не составляет труда. Если решений несколько, то перебираем их и выбираем минимальное.


Немного практики


Перейдем наконец к практическим экспериментам. Для того, чтобы упростить себе работу, возьмем пока библиотеку OpenCV и используем ее для поиска маркеров. Найдем маркер и наложим на него изображение.
Пусть $w$ и $h$ ширина и высота накладываемого изображения. Тогда четыре угла будут иметь следующие локальные координаты:
$[\vec{v_1}, \vec{v_2}, \vec{v_3}, \vec{v_4}] = [\begin{pmatrix} 0 & 0 \end{pmatrix}^T, \begin{pmatrix} w & 0 \end{pmatrix}^T, \begin{pmatrix} w & h \end{pmatrix}^T, \begin{pmatrix} 0 & h \end{pmatrix}^T]$
От OpenCV мы получаем координаты 4х углов маркера в кадре: $[\vec{m_1}, \vec{m_2}, \vec{m_3}, \vec{m_4}]$
Для наложении изображения на маркер нам необходимо описать преобразование из локальных координат накладываемого изображения в координаты кадра с камеры. Для этого возьмем два вектора $\vec{aX}$ и $\vec{aY}$, описывающих базовые оси $X$, $Y$ и вектор смещения от начала координат $\vec{t}$:
$\begin{pmatrix} {{m'}_i}_x \\ {{m'}_i}_y \\ 1 \end{pmatrix} = \begin{pmatrix} aX_x & aY_x & t_x \\ aX_y & aY_y & t_y \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} {v_i}_x \\ {v_i}_y \\ 1 \end{pmatrix}$
Можем упростить формулу, вычеркнув последнюю строку:
$\begin{pmatrix} {{m'}_i}_x \\ {{m'}_i}_y \end{pmatrix} = \begin{pmatrix} aX_x & aY_x & t_x \\ aX_y & aY_y & t_y \end{pmatrix} \begin{pmatrix} {v_i}_x \\ {v_i}_y \end{pmatrix}$
$\vec{{m'}_i} = \begin{pmatrix} \vec{aX} & \vec{aY} & \vec t \end{pmatrix} \vec{v_i}$
Одна пара точек задает два линейных уравнений, при этом имеем 6 неизвестных. Значит нужно 3 пары точек, чтобы система была определена. Мы имеем 4, а значит она переопределена. Воспользуемся МНК для нахождения нашего преобразования:
Пусть искомый вектор $\vec x$ будет собран из наших неизвестных следующим образом $\vec x = \begin{pmatrix} aX_x & aY_x & t_x & aX_y & aY_y & t_y \end{pmatrix}^T$.
Исходя из такого вектора $\vec x$, матрица $A$, задающая нашу линейную систему уравнений будет иметь следующий вид:
$A = \begin{pmatrix} {v_1}_x & {v_1}_y & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & {v_1}_x & {v_1}_y & 1 \\ ... \\ {v_4}_x & {v_4}_y & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & {v_4}_x & {v_4}_y & 1 \end{pmatrix}$
А вектор $\vec b$ будет равен:
$\vec b = \begin{pmatrix} {m_1}_x \\ {m_1}_y \\ ... \\ {m_4}_x \\ {m_4}_y \end{pmatrix}$
Тогда, используя МНК, можем получить решение нашей системы: $\vec x = (A^T A)^{-1} A^T \vec b$. Отсюда получаем $\vec{aX}$, $\vec{aY}$ и $\vec{t}$.
Пробуем!
Affine
Красным рисуются координаты маркера, полученного от OpenCV. В целом работает, но если взять низкий угол по отношению к маркеру, то видно, что искажения работают неправильно. А так получилось, потому что для моделирования пространственных искажений маркера мы использовали аффинную матрицу. Важное свойство аффинной матрицы линии, полученные после преобразования, остаются параллельными. Однако обычно, смотря на параллельные линии на плоскости, мы видим такую картину:

Параллельные линии при проецировании сходятся в одной точке, т.е. становятся не параллельными. Значит аффинной матрицы нам недостаточно.


Direct linear transformation


Получить лучший результат нам поможет алгоритм, который называется Direct linear transformation.
А описать перспективные искажения поможет перспективная матрица размером 3x3: $H = \begin{pmatrix} h_{11} & h_{12} & h_{13} \\ h_{21} & h_{22} & h_{23} \\ h_{31} & h_{32} & h_{33} \end{pmatrix}$.
Перевод из евклидовых координат в однородные обозначим как $project$ и распишем полученное преобразование для наших $n$ точек ($i = 0...n$, а $n = 4$ в нашем случае):
$\vec{p_i} = project(H \cdot \begin{pmatrix} {v_i}_x \\ {v_i}_y \\ 1 \end{pmatrix}) = \begin{pmatrix} \frac{ H_{11} \cdot {v_i}_x + H_{12} \cdot {v_i}_y + H_{13}}{H_{31} \cdot {v_i}_x + H_{32} \cdot {v_i}_y + H_{33}} & \frac{ H_{21} \cdot {v_i}_x + H_{22} \cdot {v_i}_y + H_{23}}{H_{31} \cdot {v_i}_x + H_{32} \cdot {v_i}_y + H_{33}} \end{pmatrix}^T$
Представим наши уравнения в виде системы уравнений:
$\begin{cases} ... \\ {p_i}_x = \frac{ H_{11} \cdot {v_i}_x + H_{12} \cdot {v_i}_y + H_{13}}{H_{31} \cdot {v_i}_x + H_{32} \cdot {v_i}_y + H_{33}} \\ {p_i}_y = \frac{ H_{21} \cdot {v_i}_x + H_{22} \cdot {v_i}_y + H_{23}}{H_{31} \cdot {v_i}_x + H_{32} \cdot {v_i}_y + H_{33}} \\ .. \end{cases}$


$\begin{cases} ... \\ {p_i}_x \cdot (H_{31} \cdot {v_i}_x + H_{32} \cdot {v_i}_y + H_{33}) = H_{11} \cdot {v_i}_x + H_{12} \cdot {v_i}_y + H_{13} \\ {p_i}_y \cdot (H_{31} \cdot {v_i}_x + H_{32} \cdot {v_i}_y + H_{33}) = H_{21} \cdot {v_i}_x + H_{22} \cdot {v_i}_y + H_{23} \\ ... \end{cases}$


$\begin{cases} ... \\ H_{11} \cdot v_x + H_{12} \cdot v_y + H_{13} - H_{31} \cdot v_x \cdot {p_i}_x - H_{32} \cdot v_y \cdot {p_i}_x - H_{33} \cdot {p_i}_x = 0 \\ H_{21} \cdot v_x + H_{22} \cdot v_y + H_{23} - H_{31} \cdot v_x \cdot {p_i}_y - H_{32} \cdot v_y \cdot {p_i}_y - H_{33} \cdot {p_i}_y = 0 \\ ... \end{cases}$


Представим матрицу $H$ как вектор $\vec{h} = \begin{pmatrix} H_{11} & H_{12} & H_{13} & H_{21} & H_{22} & H_{23} & H_{31} & H_{32} & H_{33} \end{pmatrix}^T$.
Теперь систему уравнений можно перевести в матричный вид: $A = \begin{pmatrix} ... \\ v_x & v_y & 1 & 0 & 0 & 0 & - v_x \cdot {p_i}_x & - v_y \cdot {p_i}_x & - {p_i}_x \\ 0 & 0 & 0 & v_x & v_y & 1 & - v_x \cdot {p_i}_y & - v_y \cdot {p_i}_y & - {p_i}_y \\ ... \end{pmatrix}$
$A \cdot \vec{h} = \vec{0}$
Получили однородную систему уравнений в которой нам неизвестен вектор $\vec{h}$ (т.е. матрица $H$). Однородную систему уравнений мы не сможем решить при помощи метода наименьших квадратов. Однако одно решение найти несложно это нулевой вектор $\vec{h} = \vec{0}$. Но такое решение нас не интересует. Если же система имеет какое-то ненулевое решение, то мы получаем сразу бесконечное множество решений. Для примера пусть мы уже имеем ненулевое решение вектор $\vec{h}$: $A \cdot \vec{h} = \vec{0}$. Теперь умножим вектор $\vec{h}$ на любой ненулевой скаляр $s$, система уравнений все равно останется справедливой, а $\vec{h} \cdot s$ также будет еще одним решением системы: $A \cdot \vec{h} \cdot s = \vec{0}$. Так как мы работаем с однородными координатами, то умножение на скаляр ничего не меняет. Тем не менее, чтобы не работать сразу со множеством решений, будем искать только одно пусть длина вектора будет равна единицы $|\vec{h}| = 1$.


Сингулярное разложение и решение однородных систем уравнений


Решить однородную систему уравнений можно при помощи сингулярного разложения матрицы (singular value decomposition). Сингулярное разложение это разложение вида: $svd(M) = U \cdot W \cdot V^T$, где $U$ и $V$ ортонормальные матрицы, а $W$ диагональная, при этом диагональные элементы больше либо равны нулю и располагаются в порядке убывания (сверху вниз). Если воспринимать матрицу как операцию трансформации векторов, то это разложение будет декомпозицией этой трансформации на три последовательных: поворот, масштабирование по осям, второй поворот. Матрицы $U$ и $V$ это ортонормальные матрицы, а значит можно назвать их матрицами поворота. Только следует учитывать, что $|U| = \pm 1$ и $|V| = \pm 1$, а значит, например, если $M$ имеет размер 3x3, то тройка базисных векторов и этих матриц поворота могут быть левосторонними, а не правосторонними как обычно.
Не будем останавливаться на том, как вычислять это разложение. В более-менее полноценных математических фреймворках оно будет реализовано. Например Eigen.
Воспользуемся этим разложением для полученной выше матрицы $A$: $svd(A) = U \cdot W \cdot V^T$. Лучшее решение для нашей системы уравнений $A \cdot \vec{h} = \vec{0}$ это последняя строка матрицы $V$: $\vec{h} = V_{n}$. А так как матрица $V$ ортонормальная, то длина вектора, составленного из любого его столбца, будет равна как раз единице.


Доказательство

В процессе вычислений у нас всегда есть погрешность, а это значит после $A \cdot \vec{h}$ мы можем получить не нулевой вектор, хотя его компоненты должны быть близки к нулю. Нужно минимизировать получаемую погрешность, а для ее оценки воспользуемся обычной для таких вещей суммой квадратов: $\vec{e} = \sum{{e_i}^2} = \vec{e}^T \vec{e} \rightarrow min$.
$\vec{e} = H \cdot \vec{h} \Rightarrow \vec{e}^T \vec{e} = (H \cdot \vec{h})^T (H \cdot \vec{h}) = (U \cdot W \cdot V^T \cdot \vec{h})^T (U \cdot W \cdot V^T \cdot \vec{h}) \rightarrow min$
$E = \vec{e}^T \vec{e} = \vec{h}^T \cdot V \cdot W \cdot U^T \cdot U \cdot W \cdot V^T \cdot \vec{h}$, так как $U$ ортонормальная матрица, то $U^T \cdot U = I$. Так как $W$ диагональная матрица с ненулевыми элементами, расположенными по убыванию, то и $(W^T \cdot W)$ также будет диагональной матрицей с ненулевыми элементами, расположенными по убыванию это будет эквивалентно возведению в квадрат диагональных элементов матрицы. Обозначим $W^2 = W^T \cdot W$.
$E = \vec{e}^T \vec{e} = \vec{h}^T \cdot V \cdot W^2 \cdot V^T \cdot \vec{h} \rightarrow min$
Обозначим $\vec{c} = V^T \cdot \vec{h}$, заметим, что $V$ не сохраняет длину вектора $\vec{h}$, значит $|\vec{c}| = 1$:
$E = \vec{c}^T \cdot W^2 \cdot \vec{c} \rightarrow min$.
Представим диагональ матрицы $W$ как вектор $\vec{w}$. Тогда $E = \vec{c}^T \cdot W^2 \cdot \vec{c} = \sum{{c_i}^2 \cdot {w_i}^2} \rightarrow min$
Теперь подумаем чему должен быть равен $\vec{c}$, чтобы $E$ стало минимальным. Так как значения в $\vec{w}$. Судя по $\sum{{c_i}^2 \cdot {w_i}^2}$, наибольший вклад должен вносить последняя компонента вектора $\vec{w}$. Так как $|\vec{c}| = 1$, то выходит $\vec{c} = \begin{pmatrix} 0 & ... & 0 & 1 \end{pmatrix}^T$.
$\begin{pmatrix} 0 & ... & 0 & 1 \end{pmatrix}^T = V^T \cdot \vec{h} \Rightarrow \vec{h} = V \cdot \begin{pmatrix} 0 & ... & 0 & 1 \end{pmatrix}^T$.
Из описанного выше делаем вывод, что $\vec{h} = V_n$ это последний столбец матрицы $V$.


Возвращаемся к нашей системе уравнений $A \cdot \vec{h} = \vec{0}$. Решаем ее описанным выше способом, получаем вектор $\vec{h}$. Затем из этого уже получаем искомую матрицу $H$. Пробуем:


Homography


Как мы видим стало заметно лучше и искажения уже похожи на правду.
Соответствующий код для аффинных и перспективных искажений можно найти в тестах проекта sonar/tests/test_marker_transform


Pinhole camera model


Хорошо, мы получили какую-то матрицу для преобразования изображений, но в идеале хотелось бы получить координаты камеры в пространстве. Чтобы их найти, построим математическую модель, которая будет описывать как точки мирового пространства проецируются на изображение камеры.
В качестве модели формирования изображения возьмем центральную проекцию. Суть центральной проекции состоит в том, что все точки, которые формируют выходное изображение, формируют лучи, сходящиеся в одной точке центре проекции. Примерно так лучи себя и ведут в модели глаза или в цифровой камере.
Проецировать точки будем на плоскость, которая и будет формировать наше изображение. В качестве такой плоскости возьмем плоскость, заданную таким условием $z = 1$. В таком случае наша камера будет сонаправлена с осью $Z$:


Зададим параметрически формулу луча, формирующего точку на проекции: $\vec{r} = \vec{v} \cdot t$, где $\vec{r}$ координаты точки луча, $\vec{v}$ координаты проецируемой точки, а $t$ является свободным параметром. Если луч не лежит на нашей плоскости и не параллелен ей, то он будет пересекать ее только в одной точке. При этом мы знаем, что $r_z = 1$.
$r_z = v_z \cdot t \Rightarrow t = \frac{1}{v_z}$
$\vec{r} = \vec{v} \cdot t = \vec{v} \cdot \frac{1}{v_z} = \begin{pmatrix} \frac{v_x}{v_z} & \frac{v_y}{v_z} & 1\end{pmatrix}$
Получаем координаты спроецированной на плоскость точки: $\vec{p} = \begin{pmatrix} \frac{v_x}{v_z} & \frac{v_y}{v_z}\end{pmatrix}$. Ничего не напоминает? Это формула перевода из однородных координат в двумерные евклидовы. Получается, что трехмерные координаты можно воспринимать как однородные координаты, соответствущие двумерным евклидовым координатам на плоскости. А перевод из однородных координат в двумерные евклидовы координаты выполняет центральную проекцию. Можно сказать, что трехмерные евклидово пространство эквивалентно двумерному проекционному.
После проецирования точки на плоскость можно переводить ее в координаты изображения. Обычно координаты изображения определены следующим образом, где $width$ и $height$ высота и ширина изображения:

Пусть у нас для камеры задан угол обзора по горизонтали $angle_x$.
Масштабируем координату $x$ точки $p$ так, чтобы расстояние от крайней левой видимой точки камеры до крайней правой было равно ширине изображения. При проецировании имеем такую картину:
Image coordinates
Здесь $A, C$ левая и правые крайние точки, $B$ точка между ними, у которой координата $x = 0$, а $z = 1$. Угол $\angle{AOC} = angle_x$. При этом имеем треугольник $AOB$, у которого угол $\angle{ABO} = 90^{\circ}$, а угол $\angle{AOB} = \frac{angle_x}{2}$, так как $OB$ делит угол $\angle{AOC}$ пополам. Длина $OB$ равна 1. Отсюда через тангенс угла $\angle{AOB}$ найдем длину $AB$ $\tan{\angle{AOB}} = \tan{\frac{angle_x}{2}} = \frac{AB}{OB} = AB$. А уже отсюда $AC = 2 \cdot AB = 2 \cdot \tan{\frac{angle_x}{2}}$.
После проецирования сводим диапазон координаты $x$ $[- \tan{\frac{angle_x}{2}}, \tan{\frac{angle_x}{2}}]$ к диапазону $[- \frac{width}{2}, \frac{width}{2}]$ умножением на коэффициент:
$f_x = \frac{width}{2 \cdot \tan{\frac{angle_x}{2}}}, \space x' = x \cdot f_x$
Чтобы сохранить пропорции изображения, то координату $y$ мы должны масштабировать на ту же величину. Но на выходном изображении ось $Y$ направлена в обратную сторону (вниз). Чтобы отразить ось, умножим коэффициент масштаба по оси $Y$ на $-1$. Получаем такое преобразование:
$f_y = - f_x, \space , \space \space y' = y \cdot f_y$
$\vec{p'} = \begin{pmatrix} f_x \cdot p_x & f_y \cdot p_y \end{pmatrix}^T$
А координаты видимого изображения получили такой диапазон:
Image range
Получилось так, что центр изображения нулевая точка. Чтобы получить нужные на координаты на изображении, сместим крайнюю левую точку в центр:
$\vec{p'} = \begin{pmatrix} f_x \cdot p_x & f_y \cdot p_y \end{pmatrix}^T + \begin{pmatrix} \frac{width}{2} & \frac{height}{2} \end{pmatrix}^T$
Теперь центральная точка изображения будет иметь координаты $\begin{pmatrix} \frac{width}{2} & \frac{height}{2} \end{pmatrix}^T$, как ей и положено. Эта центральная называется оптическим центром, обозначим ее как $\vec{c} = \begin{pmatrix} \frac{width}{2} & \frac{height}{2} \end{pmatrix}^T$. И финальная формула преобразования в координаты изображения:
$\vec{p'} = \begin{pmatrix} f_x \cdot p_x & f_y \cdot p_y \end{pmatrix}^T + \vec{c}$, где $f_x = \frac{width}{2 \cdot \tan{\frac{angle_x}{2}}}, \space f_y = - f_x$, а $\vec{c} = \begin{pmatrix} \frac{width}{2} & \frac{height}{2} \end{pmatrix}^T$.
Также, как мы делали до этого, можно упростить преобразование, работая сразу с однородными координатами точки и трансформируя все выражения в матричную форму:
Пусть $K = \begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{pmatrix}$, тогда $\vec{p'} = project(K \cdot \begin{pmatrix} p_x \\ p_y \\ 1\end{pmatrix})$.
Возвращаемся немного назад. Вспоминаем, что трехмерном пространстве это проективное пространство для плоскости, на которую мы проецируем. Т.е. трехмерные координаты это однородные координаты. Тогда формула перевода из трехмерного пространства в координаты изображения при центральной проекции принимает вид:
$\vec{p'} = project(K \cdot \begin{pmatrix} v_x \\ v_y \\ v_z \end{pmatrix}) = project(K \cdot \vec{v})$
Матрицу $K$ называют внутренними параметрами камеры (intrinsic camera parameters).
Эта марица может в себе иметь еще параметр $s_{skew}$:
$K = \begin{pmatrix} f_x & s_{skew} & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{pmatrix}$.
Эффект от его применения чем схож эффектом от rolling shutter. На практике обычно этот параметр не учитывается и остается равно нулю.
Следует учитывать, что формулы вычисления параметров $f_x, f_y, c_x, c_y$ справедливы, если мы хотим перевести именно в пиксельные координаты изображения. Но других случаях нам может быть нужно перевести в другую координатную систему, например, в текстурные координаты, где значения координат $x$ и $y$ меняются в диапазоне $[0, 1]$. Тогда эти параметрами будут уже другими.
Погрешности на производстве при сборки камер немного сказываются и на получаемых с них изображениях, оптический центр смещается немного от центра изображения, а каждая камера имеет слегка отличающиеся внутренние параметры. Поэтому каждая камера калибруется индивидуально. Реализацию калибровки можно найти в OpenCV.
В API Android OS и iOS можно найти методы получения внутренних параметров камеры, заданные производителем.
Сейчас же предлагаю просто считать оптическим центром центр изображения, а параметры $f_x, f_y$ подбирать "на глаз". Для начала хватит и такого варианта.


Перемещение камеры в пространстве


Все относительно и хотя наблюдатель перемещается в пространстве, а мир стоит на месте, мы также можем сказать, что движется мир, а наблюдатель остается на месте. Меняется только система координат, в которой происходит движение. Значит выберем ту, которая для нас удобнее. А удобнее нам оставить локальную систему координат на месте, и все остальное пусть движется вокруг нее. Движение у нас определяется поворотом и смещением матрицей поворота $R$ и вектором смещения $\vec{t}$. Также считаем, что у нас есть внутренние параметры камеры, заданные матрицей $K$. Таким образом мы можем разбить процесс вычисления координат на изображении из мировых координат на два этапа:


  1. Перевод мировых координат $\vec{v}$ в локальные координаты камеры $\vec{v'}$ при помощи $R$ и $\vec{t}$: $\vec{v'} = R \cdot \vec{v} + \vec{t}$
  2. Перевод из локальных координат камеры в пиксельные координаты изображения при помощи матрицы $K$: $\vec{p} = project(K \cdot \vec{v'})$

Как мы уже знаем, матрицу $R$ и вектор $\vec{t}$ можно объединить в одну матрицу, только для того, чтобы ее применить придется перевести $\vec{v}$ в однородные координаты:
$\begin{pmatrix} {v'}_x \\ {v'}_y \\ {v'}_z \\ 1 \end{pmatrix} = \begin{pmatrix} 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{pmatrix} \begin{pmatrix} v_x \\ v_y \\ v_z \\ 1 \end{pmatrix}$
Мы знаем, что ${v'}_w = 1$ и нам необязательно ее вычислять, поэтому можем чуть сократить формулу:
$\begin{pmatrix} {v'}_x \\ {v'}_y \\ {v'}_z \end{pmatrix} = \begin{pmatrix} R_{11} & R_{12} & R_{13} & t_x \\ R_{21} & R_{22} & R_{23} & t_y \\ R_{31} & R_{32} & R_{33} & t_z\end{pmatrix} \begin{pmatrix} v_x \\ v_y \\ v_z \\ 1 \end{pmatrix}$
Более короткая версия этой же формулы:
$\vec{v'} = [R | \vec{t}] \begin{pmatrix} \vec{v} \\ 1 \end{pmatrix}$
Теперь мы можем объединить два этапа в одну формулу:
$\vec{p} = project(K \cdot [R | \vec{t}] \begin{pmatrix} \vec{v} \\ 1 \end{pmatrix})$
К этому моменту можно нас поздравить мы построили математическую модель проецирования изображения камеры. Опираясь на нее далее мы сможем понять, что происходит в видеопотоке. А точнее нашей основной задачей станет поиск параметров $R$ и $\vec{t}$. Эти параметры называют внешними параметрами камеры (extrinsic camera parameters). Так как мы предположили, что камера остается на месте, а двигается окружающий мир, то $R$ это не матрица поворота самой камеры, а вектор $\vec{t}$ не координаты позиции камеры. Матрица $R$ это поворот из мировых координат в локальные координаты камеры, а когда говорят о повороте объекта, то имеют ввиду поворот из локальных координат в мировые. Поэтому поворот камеры это $R_{camera} = R^{-1}$. Вектор $\vec{t}$ вектор смещения камеры. Позицию камеры в мировых координатах можем найти следуя такой логике: при переводе в локальные координаты позиция камеры должна стать нулевым вектором $\vec{0} = R \cdot \vec{pos_{camera}} + \vec{t} \space \Rightarrow \space (\vec{0} - \vec{t}) = R \cdot \vec{pos_{camera}} \space \Rightarrow \space \vec{pos_{camera}} = - R^{-1} \cdot \vec{t}$.


Получаем координаты камеры от маркера


Возвращаемся к предыдущему примеру. В нем мы нашли перспективную матрицу $H$, описывающую преобразования наших точек:
$\vec{p_i} = project(H \cdot \begin{pmatrix} {v_i}_x \\ {v_i}_y \\ 1 \end{pmatrix})$
Попробуем разобраться как эта формула соотноситься с нашей моделью:
$\vec{p} = project(K \cdot [R | \vec{t}] \begin{pmatrix} \vec{v} \\ 1 \end{pmatrix}) = project(K \cdot \begin{pmatrix} R_{11} & R_{12} & R_{13} & t_x \\ R_{21} & R_{22} & R_{23} & t_y \\ R_{31} & R_{32} & R_{33} & t_z \end{pmatrix} \begin{pmatrix} v_x \\ v_y \\ v_z \\ 1 \end{pmatrix})$
Точки маркера в нашем случае задаются двумерными координатами, так как они все лежат на плоскости, т.е. $v_z = 0$. А так как $v_z = 0$, то столбец $\begin{pmatrix} R_{13} \\ R_{23} \\ R_{33} \end{pmatrix}$ не вносит вклада в результат, а значит его можно вычеркнуть, сократив формулу:
$\vec{p} = project(K \cdot \begin{pmatrix} R_{11} & R_{12} & t_x \\ R_{21} & R_{22} & t_y \\ R_{31} & R_{32} & t_z \end{pmatrix} \begin{pmatrix} v_x \\ v_y \\ 1 \end{pmatrix})$
Пусть $H' = K \cdot \begin{pmatrix} R_{11} & R_{12} & t_x \\ R_{21} & R_{22} & t_y \\ R_{31} & R_{32} & t_z \end{pmatrix}$, тогда получаем:
$\vec{p_i} = project(H' \cdot \begin{pmatrix} {v_i}_x \\ {v_i}_y \\ 1 \end{pmatrix})$
Получили идентичную формулу. Но сделать вывод, что $H' = H$ нельзя. При вычислении матрицы $H$ мы получили целое множество решений и выбрали из этого множества удобное для нас. При работе с однородными координатами умножение на скаляр не играет роли, но в нашей модели мы уже оперируем трехмерными евклидовыми координатами. Поэтому $s \cdot H' = H$. Отсюда:
$H = s \cdot K [R | \vec{t}] = K \cdot \begin{pmatrix} s \cdot R_{11} & s \cdot R_{12} & s \cdot t_x \\ s \cdot R_{21} & s \cdot R_{22} & s \cdot t_y \\ s \cdot R_{31} & s \cdot R_{32} & s \cdot t_z \end{pmatrix}$.
Пусть $T = K^{-1} \cdot H = \begin{pmatrix} s \cdot R_{11} & s \cdot R_{12} & s \cdot t_x \\ s \cdot R_{21} & s \cdot R_{22} & s \cdot t_y \\ s \cdot R_{31} & s \cdot R_{32} & s \cdot t_z \end{pmatrix} = K^{-1} \cdot H$
Первые два столбца полученной матрицы составляют два базисных вектора матрицы поворота. Длины этих векторов должны быть равны единице. Отсюда мы можем найти параметр $s = |\begin{pmatrix} T_{11} & T_{21} & T_{31} \end{pmatrix}^T| = |\begin{pmatrix} T_{12} & T_{22} & T_{32} \end{pmatrix}^T|$. Из-за погрешностей вычислений первый и второй вариант вычислений будут давать немного разные варианты, поэтому возьмем среднее между ними: $s = \frac{|\begin{pmatrix} T_{11} & T_{21} & T_{31} \end{pmatrix}^T| + |\begin{pmatrix} T_{12} & T_{22} & T_{32} \end{pmatrix}^T|}{2}$.
Теперь вычислим два базисных вектора матрицы поворота:
$\vec{r_1} = \frac{\begin{pmatrix} T_{11} & T_{21} & T_{31} \end{pmatrix}^T}{|\begin{pmatrix} T_{11} & T_{21} & T_{31} \end{pmatrix}^T|}$
$\vec{r_2} = \frac{\begin{pmatrix} T_{12} & T_{22} & T_{32} \end{pmatrix}^T}{|\begin{pmatrix} T_{12} & T_{22} & T_{32} \end{pmatrix}^T|}$
$R = \begin{pmatrix} \vec{r_1} & \vec{r_2} & \vec{r_1} \times \vec{r_2} \end{pmatrix}$
$\vec{t} = \frac{\begin{pmatrix} T_{13} & T_{23} & T_{33} \end{pmatrix}^T}{s}$
$R_{camera} = R^{-1}, \space \vec{pos_{camera}} = - R^{-1} \cdot \vec{t}$
Итак, мы получили координаты камеры в пространстве. Теперь можем передать эти координаты трехмерному движку, чтобы он вывел трехмерные модели поверх реального изображения. Что и было реализовано в плагине для Unity. Видео, записанное с android-сматфона:



Точки маркера, на которых мы основываем нашу мировую систему координат в наших вычислениях и которая одновременно с этим является нашим полом, находятся в плоскости $XY$. У Unity же пол является плоскостью $XZ$. Чтобы повернуть пол в нужные нам координаты, я воспользовался встроенными в Unity средствами создал пустой объект, назначил его "родителем" камеры, и повернул так, как мне нужно. Таким образом, мне самому не пришлось делать дополнительных вычислений. Также важно сопоставить характеристики виртуальной камеры и реальной. Пример этого можно увидеть в коде плагина.


Нужно еще отметить, что описать преобразование точек при помощи перспективной матрицы размером 3x3 можно только в случае, когда идет отображение точек с одной плоскости на другую. Такая матрица называется матрицей гомографии (homography matrix). В нашем случае шло отображение с плоскости маркера на плоскость проекции. Но при работе трехмерными координатами алгоритм direct linear transformation задействовать уже не получится и нужно будет искать другие способы.


Заключение


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

Подробнее..
Категории: Алгоритмы , Математика , Ar и vr , Разработка под ar и vr , Ar , Slam , Augmented reality , Homography , Direct linear transformation , Markers

Исследование методов SLAM для навигации мобильного робота внутри помещений. Опыт исследования R2 Robotics. (продолжение)

06.06.2021 12:22:20 | Автор: admin

Введение

В прошлой статье мы рассмотрели несколько современных алгоритмов SLAM для ROS. Во данной статье будет рассмотрено применение SLAM на практике. В качестве робота используется опытный образец мобильного робота мерчандайзера компании R2 Robotics. Робот имеет базу с двумя ведущими колесами, расположенными на одной оси в центре, что позволяет совершать развороты на месте и способствует высокой манёвренности. Диаметр робота составляет ~60 см, а его высота 1.5 метра.

4. Тестирование

В качестве сенсоров на роботе используются: 2D лидар RPLidar A1, RGBD камера Intel RealSense D435i и трекинг Intel RealSense T265 камера для отслеживания одометрии. Лидар установлен в нижней части робота и сканирует только фронтальный сектор в 180 градусов, в то время как камера установлена на уровне 1.1 м и наклонена вниз на угол 40 град. Учитывая, что высота робота 150 см, камера позволяет распознать препятствия на высоте, недоступной лидару.

Рисунок 8 Модель робота в RVizРисунок 8 Модель робота в RViz

Помещение для тестов показано на рисунке 9. Площадь в 70 м2 достаточна для простых тестов и перемещения робота по заданному оператором маршруту. Также, в помещении расположены несколько препятствий, сложные для распознавания при помощи лидара.

Рисунок 9 Помещение для тестов картографированияРисунок 9 Помещение для тестов картографирования

4.1 Rtabmap

В результате картографирования были получены две карты построенная по лидару и по камере глубины (рисунок 10).

А)А)B)B)

Рисунок 10 Rtabmap карта построенная по а) лидару b) RGBD камере

Как можно наблюдать из рисунка 10 карта, построенная по лидару, содержит меньше шумов и более читабельна. Основные границы, такие как стены помещения и мебель имеют ярко выраженные очертания. На такой карте проще ориентироваться и, к примеру, составлять маршрут передвижения для робота, особенно если это помещение большой площади с множеством объектов.

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

Рисунок 11 Сравнение отображаемых на картах объектов: красный стойки и высокие стулья, синий столы и кресла, желтый диваны и шкаф, зеленый стульяРисунок 11 Сравнение отображаемых на картах объектов: красный стойки и высокие стулья, синий столы и кресла, желтый диваны и шкаф, зеленый стулья

На рисунке 11 выделены области, в которых расположены ключевые объекты, представляющие препятствия для робота. Синяя область обозначает два стола и кресла. Логично, что лидар распознал только ножки столов и нижние части кресел, в то время как камера глубины полностью показывает занимаемое объектами пространство, проецируя данные на 2D карту. Также, лидар полностью проигнорировал препятствия, обозначенные красными областями (высокий стул и стойка). Аналогично с зеленой областью в ней расположены 7 стульев.

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

Рисунок 12 Отображение данных с RGBD камеры в RVizРисунок 12 Отображение данных с RGBD камеры в RViz

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

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

Рисунок 13 Объезд препятствия на карте, построенной по камере глубиныРисунок 13 Объезд препятствия на карте, построенной по камере глубиныРисунок 14 Объезд препятствия на карте, построенной по лидару (черный препятствие на карте, серый свободное пространство на карте, розовый препятствие на costmap obstacle layer, голубой inflation layer global costmap, красный и синий inflation layer local costmap)Рисунок 14 Объезд препятствия на карте, построенной по лидару (черный препятствие на карте, серый свободное пространство на карте, розовый препятствие на costmap obstacle layer, голубой inflation layer global costmap, красный и синий inflation layer local costmap)

В результате можно наблюдать, что при картографировании с камеры глубины робот изначально строит маршрут, огибая препятствие, отмеченное на карте (чёрный объект на рисунке 13). В это же время, при картографировании с лидара, робот не наносит препятствие на карту. Однако, если использовать данные с камеры для формирования obstacle_layer в стеке навигации, робот также распознает препятствие и сможет его объехать (препятствие отображается розовым цветом и в меньшем объеме на рисунке 14). Более того, изначальная карта будет выглядеть намного менее зашумленной. Единственным существенным недостатком будет то, что препятствия на obstacle_layer не будут сохранены и робот будет каждый раз сканировать их заново. Кроме того, зачастую препятствие будет отображаться не в полном объеме. Таким образом, при построении продолжительного маршрута, робот будет изначально строить кратчайший путь, который будет проходить сквозь подобные объекты и, в процессе движения, будет перестраивать свой маршрут при обнаружении препятствий снова и снова. К тому же имеется вероятность, что координаты цели, заданные оператором, могут совпасть с местоположением препятствия, что приведет к ошибкам в процессе работы.

4.2 Google Cartographer

В процессе тестирования Google Cartographer было выбрано две конфигурации. В первой в качестве источника данных используется лидар RPLidar A1, в то время как во второй используется камера глубины Intel RealSense D435i.

Попытка построить карту при помощи камеры не была успешной. Робот сканировал небольшую часть карты при помощи локального SLAM, после чего переходил к сканированию другого участка. Однако, при склеивании данных частей, глобальный SLAM не смог справиться со своей задачей. Части карты накладывались друг на друга, кроме того, ориентация некоторых локальных карт также была не верной. Основной причиной такого результата можно назвать камеру глубины. В отличает от лидара, камера глубины способна хорошо сканировать пространство в пределах приблизительно 2-х метров. Собственно, картографирование в Rtabmap также проводилось с дальностью видимости камеры на 2 метра. Rtabmap визуальный метод SLAM, так как глобальная навигация в первую очередь ориентируется на визуальные образы и не привязывается к контуру карты настолько сильно, а Cartographer геометрический метод, использующий в первую очередь контур карты для ориентации. Однако, поскольку камера может дать информацию только на 2 метра, Cartographer сложнее составить крупные локальные карты. В то же время, если увеличить дальность видимости камеры, шумы будут настолько сильными, что данная информация будет полностью бесполезной для алгоритма. Возможно необходима более тщательная настройка Cartographer, к примеру, настроить коэффициенты данных одометрии, линейного и углового соответствия, что позволит оптимизировать глобальный SLAM.

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

Рисунок 15 карта построенная по лидару при помощи а) Cartographer b) RtabmapРисунок 15 карта построенная по лидару при помощи а) Cartographer b) Rtabmap

Как видно из рисунка 15, карта, построенная с использованием лидара и пакета Cartographer имеет такие же отличия, как и карты, построенные при помощи SLAM пакета Rtabmap.

Рисунок 16 Визуализация Rviz карты Cartographer, построенной по лидаруРисунок 16 Визуализация Rviz карты Cartographer, построенной по лидару

На рисунке 16 отчетливо видны слегка размытые серым цветом границы помещения. Это особенность работы Cartographer. В процессе сканирования Cartographer наносит препятствия на карту несколько раз, убеждаясь, что это действительно существующий объект, а не помехи или шум, который необходимо избегать. Таким образом, все объекты на карте постепенно меняют свой окрас от бледно-серого до черного оттенка. Чем темнее оттенок объекта на карте, тем больше Cartographer уверен в существовании данного объекта. Данная особенность позволяет довольно быстро убирать помехи с карты в процессе её сканирования. Более того, Cartographer практически не обращает внимания на быстродвижущиеся динамические объекты например, людей. Он попросту не успевает нанести их на карту в виде препятствий, если человек постоянно перемещается. Участок карты, на котором обозначается человек (изначально бледно-серым оттенком), в следующий момент времени уже пуст и этот участок снова помечается как свободный от препятствий.

Однако, это не означает, что робот во время движения столкнется с человеком. Данные с датчиков по-прежнему поступают в navigation stack и наносятся на obstacle layer. Благодаря этому локальный и глобальный планировщики не дадут построить маршрут движения, который приведет к столкновению. В этом случае необходимо обратить внимание на скорость движения человека и скорость обработки данный в navigation stack, чтобы робот успевал обрабатывать поступающую информацию. В противном случае, если скорость движения человека будет слишком высока, столкновение может произойти.

Следующей особенностью Cartographer является разбиение процесса построение карты на глобальный SLAM и локальный SLAM. Благодаря этому, Cartographer способен сканировать карту даже при небольших отклонениях в одометрии, поскольку, при склеивании локальных частей карты, он ориентируется не только на одометрию, но и сами карты. Таким образом, Cartographer подгоняет кусочки карт друг под друга, чтобы сопоставить их как можно более точно. В особенности это заметно на примере стен. При резком развороте робота данные с лидара наносятся на карту с небольшой задержкой, но этого достаточно чтобы ровная стена отображалась в виде нескольких линий, которые пересекаются друг с другом под углом около 5-10 град. Однако, спустя определенное время (количество итераций до склеивания задается в параметрах Cartographer) происходит присоединение нового участка карты и, как правило, большая часть несоответствий уходит.

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

На рисунке 17 представлена карта 2-х помещений, созданных при помощи Cartographer. В начале было исследовано помещение 1, затем робот исследовал помещение 2, после чего снова вернулся в первое помещение, где были сделаны небольшие правки карты. Как можно заметить, левая часть помещения 2 исследована плохо, однако общая карта обоих помещений соответствует реальному зданию. Во время картографирования, промежуточная стена, разделяющая оба помещения была исследована с двух сторон. В случае использования Rtabmap есть большая вероятность получить на карте две стены, которые могут не соответствовать друг другу, однако Cartographer справился с поставленной задачей.

Рисунок 17 Cartographer карта 2-х помещений, построенная по лидаруРисунок 17 Cartographer карта 2-х помещений, построенная по лидару

Заключение

В результате сравнения режимов конфигурации и работы SLAM методов Rtabmap и Cartographer было выявлено, что выбор источника данных для построения карты помещения имеет значительное влияние. При картографировании по данным с 2D лидара, карта получается менее зашумленной и более читаемой для оператора. К тому же, при использовании данных с камеры в качестве дополнительного источника информации для стека навигации, не распознанные лидаром объекты попадут на obstacle_layer, и робот сможет их объехать при обнаружении.

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

Стоит также обратить внимание на сенсоры, доступные для использования с каждым типом SLAM. Кроме того, различия есть и в формате выходных данных.

Gmapping предоставляет лишь базовые возможности подключения 2D лидар и одометрию, в то время как Cartographer и Rtabmap позволяют подключать широкий спектр различных датчиков для считывания окружающей информации. Кроме того, в отличие от Gmapping, данные методы позволяют создавать 3D карту местности.

Разные подходы в Rtabmap и Cartographer также привносят особенности в работу SLAM. Rtabmap лучше показывает себя на открытом пространстве, что предполагает использование визуальных образов. В качестве примера может быть огромный павильон, в котором лидар попросту не достанет до границ помещения, как и камера глубины. К тому же, шум у камеры довольно сильно возрастает с увеличением расстояния до объекта например стены.

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

Список источников


  1. Pedrosa, E., L. Reis, C. M. D. Silva and H. S. Ferreira. Autonomous Navigation with Simultaneous Localization and Mapping in/outdoor. 2020.

  2. Gmapping [Электронный ресурс] URL: http://wiki.ros.org/gmapping, свободный ресурс Загл. с экрана. Язык англ. Дата обращения: 14.08.2020 г.

  3. Google Cartographer ROS [Электронный ресурс] URL: https://google-cartographer-ros.readthedocs.io/en/latest/#, свободный ресурс Загл. с экрана. Язык англ. Дата обращения: 04.11.2020 г.

  4. RTAB-Map, Real-Time Appearance-Based Mapping [Электронный ресурс] URL: http://introlab.github.io/rtabmap/, свободный ресурс Загл. с экрана. Язык англ. Дата обращения: 22.06.2020 г.

  5. Adaptive Monte Carlo localization [Электронный ресурс] URL: http://wiki.ros.org/amcl, свободный ресурс Загл. с экрана. Язык англ. Дата обращения: 03.08.2020 г.

  6. Building Maps Using Google Cartographer and the OS1 Lidar Sensor [Электронный ресурс] URL: https://ouster.com/blog/building-maps-using-google-cartographer-and-the-os1-lidar-sensor/, свободный ресурс Загл. с экрана. Язык англ. Дата обращения: 25.02.2021 г.

  7. Labb, M, Michaud, F. RTABMap as an opensource lidar and visual simultaneous localization and mapping library for largescale and longterm online operation. J Field Robotics. 2019; 35: 416 446.

  8. Silva, B.M.F.D.; Xavier, R.S.; Gonalves, L.M.G. Mapping and Navigation for Indoor Robots under ROS: An Experimental Analysis. Preprints 2019.

  9. Mathieu Labb and Franois Michaud. Online Global Loop Closure Detection for LargeScale Multi-Session Graph-Based SLAM. 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 26612666, 2014.

Подробнее..
Категории: Разработка робототехники , Робототехника , Slam , Робот мерчендайзер , Google cartographer , Gmapping , Rtabmap

Starline Hackathon 2020

25.11.2020 14:11:26 | Автор: admin

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

Если вы не знаете что такое ROS советую к изучению ресурсVoltbto.com, вот напримерстатья по работе с сервисами в ROS.

В конце марта 2020 года компания Starline во второй раз в своей истории провелахакатон.

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

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

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

  1. StarlineHackathon

  2. COEXHackathon

  3. РобофестAutonet 18+

  4. UpGreatЗимний город+ очень крутаястатья на vc.ru

  5. Робокросстык-1,тык-2

  6. РобофинистРТК Экстремал Pro (информация естьздесь)

  7. КрокКонкурс летающих роботов[больше не проводит]

Все эти соревнования отличаются друг от друга достаточно сильно, но в них мне как специалисту было [бы] интересно участвовать.

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

Я@urpylkaс моим другом Артуром@goldarteрешили участвовать вдвоём (в предыдущий раз я занял третье место, а он первое).

Дано

Каждой команде был выделен робот Turtlebot E2 следующего содержания:

  1. Платформа Kobuki

  2. RGBD-камера Orbbec Astra

  3. Rplidar A2

  4. Компьютер Intel NUC [BOXNUC7I7BNH]

  5. Дополнительная камера Logitech HD Pro C920

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

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

Организаторами изначально был построен целый город (7x7 клеток): из квадратных черных матов было сделано дорожное полотно с разметкой, а из коробок разного размера с прикрепленными иллюстрациями были оформленны дома. У каждого дома на полу малярной лентой была отмечена его позиция, чтобы если кто-то сместил дом, знал куда его вернуть на место. Это важно для возможности ориентации робота с помощью построенной карты.

В городе имелись препятствия (про расположение которых мы не знали заранее):

  1. Светофоры

  2. Знаки Стоп

  3. Блоки Дорожные работы

  4. Другие участники движения в виде статичных моделей машин

Также была сплошная разметка, где было запрещено движение по встречной полосе и пересечение самой полосы.

Задание и решение

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

Мы разделили решение на несколько частей и занялись ими по отдельности:

  1. Стейт машина для управления роботом

  2. Определение дорожной разметки и способ не пересекать сплошную

  3. Машинное зрение для распознавания знаков Стоп и светофоров

  4. Ориентация и перемещение робота по городу с использованием алгоритмов, доступных в ROS

В начале решения задач по техническому зрению, мы изначально понимали какие есть алгоритмы и как они работают. Хорошей кодовой базой и документацией к ней являютсямануалы для TurtleBot3 на robotis.com, плюс в их репозитории с соревнованийautorace:turtlebot3autorace,https://github.com/ROBOTIS-GIT/turtlebot3autorace2020. В этих репозиториях есть хорошие примеры:

State Machine

Идея решения изначально была следующая: Делаем стэйт-машину с двумя состояниями:GOTO_0,GOTO_1. Они являются состояниями следования в точки0и1на карте соотвественно.

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

По завершению (достижению точки) состояния циклично меняются. Так достигается то, что робот ездит кругами.

В состоянияхGOTO_0,GOTO_1стэйт-машины проверяется, не был ли замечен красный свет светофора или знак Стоп. Если таковые были замечены, делаем остановку до окончания красного света или на одну секунду в случае знака.

Также впоследствии были добавлены состоянияINITиIDLE. ВINITмы включаем платформу, инициализируем объекты вспомогательных классов и переходим в состояниеIDLE. Также перейти в состояниеIDLEможно изGOTO_0иGOTO_1. В качестве фреймворка для state-machine было реализованособстсвенное решение, построенное на базестэйт-машины Karn Saheb. В реализацию классаStateMachineдобавлено полеSтипа пустого классаObjectStorage. Это позволяет организовать удобную работу с одними данными из разных состояний черезsetattr(Class, Attr, Value)и через его сокращенный вариантClass.Attr = Value.

Детекция разметки

В начале думали использовать, что вродеlane-detection:

  1. github/awesome-lane-detection

  2. github поиск lane-detection

  3. hackster.io curved-lane-detection

Также завели конфигурированиеbirdviewс помощьюdynamic_reconfigure.

Затем посмотрели в сторону использования нейронных сетей для детектирования попутной полосы цветомcolor-segmentation. Нам понравилось решениеdheera/ros-semantic-segmentation. По размеченной цветом картинке можно высчитать координаты области и передавать эти данные в локальный планировщик.

На тему семантической сегментации изображений естьстатьяна хабре.

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

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

Затем нам нужно было как-то правильно выбирать полосу и не ехать по встречке. Мы решили динамически наносить на глобальную карту препятствия, таким образом, что если робот въезжает в радиусR1центра некоторого перекрестка, то во всех запрещенных встречных направлениях перекрестка строятся препятствия, до тех пор пока робот не отдалится от центра данного перекрестка на радиусR2.

Распознание объектов

В рамках ограниченного времени хакатона, было решено заранее поискать готовые инструменты для выделения нужных объектов на изображении. Одним из инструментов оказался ROS пакетfind_object_2d: он позволяет на лету сконфигурировать детектор фич и дескриптор для классификации объектов по заранее имеющимся картинкам, а также предоставляет список определённых объектов и их координат с матрицами гомографии в топик/objectsиobjectsStamped. Помимо выделения объектов нужно было, например, правильно распознать цвет светофора, поэтому помимо готовых решений пришлось применять решения из библиотекиopencv. Итоговым решением задачи по определению знака стоп и цвета светофора стала нодаdetect_objects_node.py. Кстати вотнеплохой кодиз проекта по управлению машинойCarND-Capstoneв котором мы черпнули много полезного.

Распознавание знака стоп

Распознавание знака стоп было реализовано с помощью пакетаfind_object_2d. В качестве детектора фич и дескриптора был выбран ORB с увеличенным количеством итераций афинных преобразований для более робастного определения знака под разными углами. Все параметры распознавания доступныздесь. Для довольно уверенного распознавания знака с разных ракурсов хватило вот этой картинки:

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

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

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

Распознавание светофора и его цвета

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

Рабочий алгоритм по выделению цвета светофора выглядел следующим образом: вначале на изображении выделяются области определённой насыщенности в цветовом пространстве HSV, затем среди этих областей производится поиск областей, наиболее похожих на круг (методом Hough Circles) с определёнными границами по размеру. Если такие области есть - мы определили, что находимся у светофора с определённым цветом.

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

Особенным моментом стало то, что мы не могли использовать видимое изображение с камеры Astra в виду очень узкого угла обзора. А также не особо подходила камераLogitech HD Pro C920также из-за достаточно узкого угла обзора установленная камера внутри робота не позволяет увидеть знак с другого края дороги нужна более широкоугольная камера. Мы же более-менее решили эту проблему с выносом камеры за пределы робота.

Navigation Stack

Приведу схему ROS Navigation Stack (информация взята изпрезентации PhD PaulEdouard Sarlin) для общего понимания процессов.

  • Map prior: что заранее известно о окружающей среде

  • Kinect: камера глубины, возвращающая облако точек

  • Odometry: позиция робота в лабиринте

  • Map updater: обрабатывает данные для создания и обновления внутренней карты окружающей среды

  • Global planner: вычисляет маршрут от начальной точки в цель, используюя карту

  • Local planner: выполняет и корректирует с учетом обстановки процесс перемещения робота по построенному маршруту

Для управления роботом используетсясупер-пакетnavigation, он содержит в себеmove_base(утилитадля управления роботом через угловые скорости) и два планировщика: локальный и глобальный. Планировщик это программа, которая, опираясь на положение робота на карте окружающего пространства, может построить маршрут в заданную координату. Локальный планировщик работает с небольшим участком карты в ограниченном пространстве вокруг робота с учётом данных с датчиков робота об окружающих объектах. Глобальный планировщик строит общий маршрут, ориентируясь по всей карте, без учета возможного появления новых препятствий.

Полезным к изучению будетмануалпо настройке Navigation Stack на роботе. И туда же очень хорошаястатьяпо настройке ROS Navigation Stack.

Также вышелновый пакетrobot_navigation, как замена старому.

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

Вместо стандартного локального планировщика мы решили использоватьteb_local_planner (github), который написали ребята изTU Dortmund University. Несколько ссылочек c примерами:

Для описанных выше вещей нужно было как-то добавлять препятствия, построенные на отдельных картах в планировщик или на глобальную карту, которую также кушает планировщик.

В планировщиках обычно используются простые по назначению слои:

  • obstacle_layer динамические препятствия (примернастройки Obstacle inflation in costmap2d)

  • inflation_layer утолщение препятствий

  • static_layer статичная карта

Каждый слой должен быть определнного типа:

  • costmap_2d::StaticLayer препятствия, построенные на базе заданной статической карты

  • costmap_2d::ObstacleLayer препятствия добавляемые с помощью топика типаLaserScanилиPointCloud

  • costmap_2d::InflationLayer добавляет вcostmapлинию некоторого радиуса и определнной плотности вокруг препятствий

  • costmap_2d::VoxelLayer добавление 3D объектов

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

Подробнее проcostmap_2dвы можете прочитать в официальной документацииздесь. Там вы можете найти примеры конфигураций (1,2,3), описание простых типов (staticmap,obstacles), описаниеflatиlayered.

Также я нашел решениеSr4l/virtualobstacles, которое представляло из себя Costmap Plugin для добавления слоев типаcostmap_2d::MovingObjects. У проекта была не очень хорошая документация, поэтому я написалLars Kistnerи он дополнил репу и прислал еще дополнительные инструкции и интересноевидео, где он это использует. Однако я не успел завести это решение и решил использовать что-то более топорное. В дальнейшем наверное правильнее будет использовать его решение.

Для построения препятствий на карте я написалреализациюрисования линий с помощью простейшегоалгоритма Брезенхэма.

Для динамического слияния (наложения) карт я написалMaps Merger.

Итого у нас получилось пять карт:

  1. /maps/map_amcl чистая карта, полученная с помощью алгоритмаgmapping(подробнее о построении картыздесь)

  2. /maps/map_mb/maps/map_amcl+ нарисованные сплошные линии (передавалась в глобальный планировщик)

  3. /maps/crossroads карта с динамически наносимыми на неё перекрестками

  4. /maps/start_wall карта с динамически наносимыми на неё стенками у точки старта

  5. /maps/map_merged- карта динамически строящаяся из/maps/map_mb,/maps/crossroads,/maps/start_wall(передавалась в локальный планировщик)

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

После настроек стека навигации можно выполнить следующую команды для теста робота:

rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped '{header: {stamp: now, frame_id: "map"}, pose: {position: {x: 0.674, y: 0.119, z: 0.0}, orientation: {w: 1.0}}}'# PS Координаты и фреймы должны быть ваши (выбраны исходя из используемых карт)

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

Другие интересные источники по теме

Определение препятствий

Некоторые команды просто убирали несколько уровней платформы TurtleBot и использовали только лидар. Мы же решили разделить эти задачи и завести их на разные сенсоры.

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

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

Также для детекции более низких препятствий, нам пришлось сместить RGBD-камеру ниже.

Итог

В этот раз занять призовое место не удалось. Проблема заключалась в том, что мы слишком сосредоточились на прикладных вещах и рассчитывали, что отлаженный в симулятореteb_local_plannerбудет работать также хорошо. По сути это основной механизм в софте, отвечающий за перемещение. Каково было наше удивление, когда за два часа до защиты мы поняли, что робот даже нормально развернуться не может. В итоге мы потратили последние 2 часа в попытке отладить планировщик, и робот поехал. Однако на первом же повороте робот застрял, увидев препятствие, между которым до сплошной полосы оставалось небольшое пространство. Он не смог перестроить глобальный маршрут из-за ограничений планнера и застрял, замкнувшись в бесконечном цикле попыток въехать и выехать за пределы мётрвой зоны, куда он сам себя загнал.

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

У нас же была очень круто проведененная неделя в Питере в компании специалистов и друзей. Мы очень благодарны компанииStarline, в частности: Алексею Хованскому, Николаю Дема, Александру Никифорову, Кириллу Гореву, Веденину Даниилу и Маркеловой Виктории.

Бонусом укажу пару-тройку ссылочек на ресурсы ребят и компанию ораганизавторов:

  • developer.starline.ru открытое API некоторых продуктов Starline.

  • Портал РобоФинист, кстати его основным разработчик является всего один человек Кирилл Горев. А еще на этомпорталевы можете найти больше фоточек с этого замечательного мероприятия.

  • YouTube канал Николая Дема. Коля крутой робототехник и на его ютубчике периодически появляются интересные видосы.

На этом всё, спасибо большое за прочтение!

Подробнее..
Категории: Робототехника , Ros , Self-driving , Slam , Turtlebot , Hackathon , Starline , Lane-detection

Категории

Последние комментарии

  • Имя: Макс
    24.08.2022 | 11:28
    Я разраб в IT компании, работаю на арбитражную команду. Мы работаем с приламы и сайтами, при работе замечаются постоянные баны и лаги. Пацаны посоветовали сервис по анализу исходного кода,https://app Подробнее..
  • Имя: 9055410337
    20.08.2022 | 17:41
    поможем пишите в телеграм Подробнее..
  • Имя: sabbat
    17.08.2022 | 20:42
    Охренеть.. это просто шикарная статья, феноменально круто. Большое спасибо за разбор! Надеюсь как-нибудь с тобой связаться для обсуждений чего-либо) Подробнее..
  • Имя: Мария
    09.08.2022 | 14:44
    Добрый день. Если обладаете такой информацией, то подскажите, пожалуйста, где можно найти много-много материала по Yggdrasil и его уязвимостях для написания диплома? Благодарю. Подробнее..
© 2006-2024, personeltest.ru