Визуальный SLAM: делаем HD-карты при помощи смартфона

Привет! Меня зовут Александр Гращенков, я iOS-разработчик в компании RoadAR. С 2016 года живу и работаю в Иннополисе, занимаюсь компьютерным зрением и интеграцией нейросетей в мобильные платформы.

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

Сейчас мы создаем детализированные трехмерные сканы дорог при помощи смартфонов. Я расскажу, что такое HD-карты, сравню два подхода к их созданию и покажу вам трехмерный Иннополис.

HD-карты и зачем они нужны

Hight Definition (HD) maps — карты с точным месторасположением объектов в векторном формате. Они содержат детали, которых нет на обычных картах: знаки, светофоры, разметку, столбы, даже кусты, если это необходимо для навигации.

Прежде всего такие HD-карты необходимы для работы беспилотных автомобилей. 

При помощи бортовых сенсоров они получают данные о том, что находится поблизости, но слабо «представляют» себе, что ждет за следующим поворотом или через несколько кварталов. HD-карты дают беспилотникам дополнительную информацию о маршруте. В некотором смысле заменяют им память. 

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

Кроме того, регулярное картирование дорог при помощи этой технологии можно использовать для инвентаризации. Допустим, нужно посчитать, сколько фонарей установлено на участке трассы. Собрав HD-карту, мы получим точное количество и координаты каждого из них. Теперь можно будет проверить, что дорожные фонари находятся на достаточном расстоянии друг от друга и освещают всю дорогу. А если один из них не будет работать, мы заметим это при следующем проезде, не найдя источник света в ожидаемой части кадра.

HD-карты на мобильном устройстве — наша первая попытка

Начнем с простого — HD-карты, на которой отмечены дорожные знаки. Мы взялись за ее создание в 2014–2015 годах, когда нейросети только набирали популярность.

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

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

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

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

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

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

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

SLAM

Прошло семь лет. Беспилотники бороздят просторы Иннополиса и собирают HD-карты. Это стало возможно благодаря алгоритмам Simultaneous Localization And Mapping (SLAM) — они строят карту местности и локализуются на ней.

Алгоритмы беспилотников зачастую предназначены для работы с дорогим оборудованием: LIDAR’ами, радарами, ультразвуковыми датчиками. Но существует и Visual SLAM, который работает на основе видео с обычных камер.

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

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

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

Зная точную траекторию движения автомобиля, можно не только расставлять знаки, но и делать дополненную реальность. Например, проецировать подсказки навигатора прямо в поле зрения водителя. 

Эта технология также позволяет сканировать окружение для создания компьютерных игр в виртуальной реальности и метавселенных.

Погружение в SLAM

Разберемся в видах SLAM, в том, как они устроены и чем отличаются от одометрии.

Одометрия — это восстановление траектории движения. Алгоритмы для одометрии позволяют рассчитать, как устройство повернуто и в каком направлении смещается в пространстве.

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

• Если есть колеса, то для расчета траектории в 2D можно использовать их вращение.

• Гироскопы и акселерометры дают представление об ускорении и ориентации и позволяют восстановить траекторию в 3D.

• Через камеру можно следить за приближением и удалением окружающих объектов и по ним восстановить траекторию движения камеры. Такой подход называется визуальной одометрией (Visual Odometry).

SLAM = Odometry + Mapping. SLAM тоже восстанавливает траекторию движения и дополнительно строит карту местности, которую можно переиспользовать. Соответственно, Visual SLAM делает это на основе видеопотока с камеры. Дальше по тексту я опускаю слово Visual, просто имейте в виду, что речь именно о визуальном SLAM.

Feature based SLAM

Существует два направления SLAM. Они различаются тем, как мы ищем точки интереса и следим за ними. Разберем более популярный способ, основанный на поиске фич — точек с определенным ID.

Идея в том, чтобы найти четкие границы, за которыми удобно следить между кадрами. Оставляем только точки с самым сильным градиентом (изменением цвета) в небольших окошках. Для каждой точки считаем ORB-дескриптор — некий ID. Затем по битам сравниваем ID точек на сходство.

ORB-дескриптор инвариантен к поворотам, поэтому ID точки остается сходным даже на повернутых кадрах.

В рамках этого подхода можно справиться и с приближением и отдалением объектов. Для этого используют Image Pyramids — масштабируют картинку в несколько размеров и повторно ищут те же самые точки и дескрипторы.

Это не единственная техника, которая позволяет поиску фич стабильно работать и не ломаться, но сейчас главное в общих чертах понять, как работает Frontend.

Если говорить про отслеживание того, как точки смещаются между кадрами, то здесь все относительно просто. Для трекинга мы находим фичи на новом кадре и сопоставляем по ID. 

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

Вспомните, какую картинку дает fisheye объектив — ни одной прямой линии. Подобные особенности, пускай и в меньшем масштабе, есть у любой оптики.

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

Благодаря фичам, алгоритм нечувствителен к резкому изменению освещенности. В отличие от Direct-подхода, который привередлив к яркости изображения.

Direct SLAM

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

Справа показана имплементация, как мы следим за пикселями при Direct-подходе. Это не совсем SLAM, но сейчас это не важно. Главное то, как мы следим за точками интереса и затем строим карту местности.

Так как мы следим за пикселями напрямую, результат сильнее зависит от правильной калибровки камеры. В этом подходе не обойтись одним только исправлением дисторсии. Необходимо учитывать гамма-коррекцию, виньетирование (оно незаметно глазу, но есть во многих камерах), выравнивать параметры съемки — ISO и выдержку.

Мы учитываем эти параметры, чтобы точно знать, какое количество света выдает точка при попадании луча от нее на матрицу камеры.

Еще одно отличие этого алгоритма от Feature based SLAM заключается в более сложном трекинге точек между кадрами.

Даже если сравнивать перебором группу из 8 пикселей на отдельном участке изображения, на это уйдет уйма времени. Сделать это на всей картинке нереально. Чтобы ограничить область поиска, в Direct SLAM используется эпиполярная геометрия.

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

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

И последнее отличие — в Feature Based SLAM в бэкенд прокидываются только конечное соответствие точек и положение камеры. В Direct-подходе помимо точек мы прокидываем изображения с камеры. Внутри бэкенда происходит дополнительная фотограмметрическая оптимизация. 

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

Некоторые разработчики искренне считают, что ORB SLAM работает лучше, но они просто уделяют недостаточно внимания калибровке. На деле оба алгоритма работают хорошо, но Direct-подход возвращает гораздо более плотное облако точек.

Rolling Shutter

Со SLAM связана еще одна сложность. Считывание изображения с матриц большинства потребительских камер и камер смартфонов происходит построчно, обычно сверху вниз. Когда объект или сама камера движутся быстро, нижние строчки матрицы показывают сдвиг относительно положения, в котором объект был на верхних строчках. Это называется Rolling Shutter — «эффект бегущего затвора». Из-за него объекты в кадре искажаются.

Источник картинки

Такая особенность матриц тоже влияет на работу алгоритмов. Так как feature-based-метод менее чувствителен к дисторсиям, он также менее чувствителен к таким искажениям по сравнению с Direct-подходом.

Feature based vs Direct

Подведем итог на примере двух известных OpenSource-движков.

Обратите внимание, что DSO использует больше точек для трекинга, поэтому конечное облако получается более густым. Чтобы быстрее работать с таким большим числом точек, разработчик DSO отказался от библиотеки g2o для оптимизации на стороне бэкенда. Там используется самописный код с оптимизацией, математикой и сильной связностью. Поддерживать его очень сложно. 

В результате большинство плюсов на стороне feature-based-подхода: код проще, высокая модульность, меньше зависимость от качества калибровки камеры. И это, пожалуй, действительно отличный вариант для большинства задач.

Однако у feature-based-подхода есть неочевидные недостатки. Чтобы понять их, сначала разберемся, как сделать из визуальной одометрии полноценный SLAM.

Как строится карта в Feature based SLAM?

ORB-SLAM — наиболее популярный движок, основанный на фичах. Он умеет находить положение камеры на уже готовой карте. 

Для этого используется довольно простая техника — словарь с фиксированным количеством слов. Все слова в нем имеют фиксированную длину и каждое слово содержит значение дескриптора фичи. 

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

Для каждой точки, найденной на кадре, мы находим ближайшее слово из словаря. Затем строим для кадра битовую маску из видимых слов. Если видим на кадре какую-то фичу, то заполняем значение true в поле для этого слова. В противном случае записываем туда false.

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

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

Карта для SLAM, инвариантная к погодным условиям

Мы остановили выбор на Direct-подходе. В отличие от feature-based-подходов он строит намного более плотное облако точек. С такой плотностью мы можем использовать иные подходы для сопоставления карт.

Карта с Direct-фичами чуть менее плотная, чем созданная при помощи LIDAR-сенсоров, но вполне подходящая, чтобы использовать для поиска соответствий подходы, которые применяют с лидарами. В целом они напоминают ORB-SLAM, но точки интереса выбираются в облаке, по форме объектов. 

Вначале алгоритм ищет выделяющиеся точки (объекты), при необходимости строит по таким точкам дескриптор и проводит сопоставление этих точек с другим облаком.

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

DSO — не та лошадка, с которой мы доедем до конца

Мы разбирались с DSO больше года: исправляли мелкие баги, ускоряли код, улучшали инициализацию, пытались подружить его с GPS. 

Если в части фронтенда в этот движок еще возможно вносить правки, то в бэкенде сложность кода возрастает колоссально. Математика и оптимизации переплетены там так сильно, что распутать этот клубок очень сложно.

Без отлаженного бэкенда мы не могли двигаться дальше. Он принимает на вход множество точек в 3D, их 2D-положение на кадрах видео и положение камер в 3D. Далее находит итеративно оптимальные положения точек и камер в 3D. Оптимизация происходит по расстоянию между яркостями точек после проекции 3D-точки на кадр и реальным положением этой точки на кадре.

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

Наши планы

В результате последние три года мы занимаемся разработкой собственного SLAM-движка, и он уже работает стабильнее и качественнее DSO. Параллельно прорабатываем сбор объектов на дороге.

Посмотрите карту Иннополиса, которую мы сделали во время первых испытаний, проехавшись вокруг офиса с закрепленным на лобовом стекле смартфоном. Есть и пример со знаками.

Эта карта построена на основе пяти проездов, снятых на iPhone 8. Хорошо видны столбы, знаки, разметка, надписи. Сохраняется и цветовая информация.

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

С момента публикации наше приложение-видеорегистратор с детекцией знаков установили около 800 тысяч раз. С помощью пользователей мы собрали более 20 миллионов изображений знаков, причем не только в России и СНГ, но и в Европе. Уверены, пользователи будут рады помочь и с HD-картами и сканированием улиц.

---

Эта статья написана, чтобы познакомить вас с основами визуальной одометрии и SLAM-подходов и рассказать, какие задачи можно решать с их помощью. Возможно, в следующий раз мы расскажем о том, как экспериментировали с фильтром Калмана, ускоряли SLAM с помощью GPU на iPhone и решали проблемы с Rolling Shutter.