Санкт-Петербург

 
PARROT
 
ARDRONE
 
2.0 
Осинкин Е.А., Баев П.А., Зенкин А.М. 
Научный руководитель – к.т.н. Капитонов А.А. 
В работе рассмотрен один из подходов к решению задачи одновременного картирования и 
локализации в заранее неизвестном окружении. Этот подход основан на извлечении и отслеживании 
ключевых точек в каждом кадре с последующей оценкой поворота и смещения камеры между 
кадрами. Таким образом, данные для одометрии дрон получает посредством одной камеры. 
Ключевые слова: компьютерное зрение, локализация, картирование, проективная геометрия, 
эпиполярная геометрия, feature-based SLAM. 
Задача одновременного картирования и локализации SLAM (Simultaneous Localization 
and Mapping) является одной из главных проблем компьютерного зрения. Она включает в 
себя множество низкоуровневых подзадач, таких как калибровка камеры или стереопары, 
извлечение и отслеживание ключевых точек, построение карты глубины, триангуляция, 
оценка перемещения камеры. Однако, помимо хорошо изученных подзадач, SLAM также 
включает в себя сложные методы оптимизации, такие как bundle adjustment [1]. Набор 
подзадач варьируется в зависимости от подхода к решению задачи одновременного 
картирования и локализации. 
По плотности карты SLAM подразделяется на плотный, полуплотный и разреженный; 
по количеству камер – на монокулярный и стерео. Также для задач картирования и 
локализации часто используют RGBD-камеры и лидары. Из всех вышеперечисленных 
способов монокулярный SLAM является наиболее оптимальным для квадрокоптера Parrot 
ARDrone 2.0, обладающего небольшой грузоподъемностью. Установка RGBD-камеры или 

---

Альманах научных работ молодых ученых 
XLVII научной и учебно-методической конференции Университета ИТМО. Том 1 
222 
стереопары неизбежно ухудшит динамику дрона и только монокулярный подход не требует 
никаких конструктивных изменений. Главным недостатком картирования по одной камере 
является то, что абсолютный масштаб невозможно определить без использования 
дополнительных датчиков, например, дальномера, расположенного в нижней части дрона. 
Как правило, результатом монокулярного SLAM, основанного на поиске ключевых 
точек, является разреженная карта, однако существует решение [2] с использованием 
сверточных нейронных сетей, которое позволяет построить плотную карту. В работе 
рассмотрен классический способ получения разреженной карты. 
Описание алгоритма SLAM начато с модели камеры. Для точки в Х-трехмерном 
пространстве рис. 1, а, формула проекции на плоскость изображения выглядит следующим 
образом: 
𝑥 = 𝐊[𝐑|𝐭][𝑋 1] 
𝑇
, где K – матрица внутренних параметров камеры; R – 
ортогональная матрица поворота размерностью; t – вектор-столбец смещения, а 
[𝐗 1] 
𝑇
– 
вектор-столбец положения реальной точки, записанный в однородных координатах. 
а 
б 
Рис. 1. Модель камеры (а); эпиполярное ограничение (б) 
Поиск ключевых точек производился при помощи алгоритмов FAST и ORB. 
Отслеживание ключевых точек FAST осуществлялось с использованием метода KLT 
tracker [3], а для ключевых точек ORB был использован метод библиотеки OpenCv brute-
force matching. В случае с brute-force matching для качественного отслеживания ключевых 
точек необходимо осуществлять проверку и удалять ложные соответствия. Одним из 
критериев проверки является утверждение, что наилучшее соответствие из первого 
изображения во второе должно совпадать с наилучшим соответствием из второго в первое. 
Самой эффективной фильтрацией является фильтрация по эпиполярному ограничению 
рис. 1, б, которая подразумевает, что наблюдаемые точки, лежащие на одной линии для 
первого положения камеры, также будут лежать на одной линии после смещения камеры 
[1]. В основе метода KLT лежит оценка области, в которой ключевая точка окажется на 
следующем кадре. Отследив ключевые точки в двух последовательных кадрах, оценим 
поворот и смещение камеры, для этого необходимо знать сущностную матрицу Е 
размерностью
3 × 3, такую, что (𝑥
1
′) 
𝑇
𝐄 𝑥
2
′ = 0, где 𝑥
1
′и 𝑥
2
′ – нормализованные 
однородные координаты одной точки в двух последовательных кадрах. Наиболее 
эффективным, с точки зрения производительности и робастности, является пятиточечный 
[4] алгоритм поиска матрицы Е. Матрицы поворота R и смещения t получаются путем 
сингулярного разложения E [1]. 
Примем, что в начальный момент времени вращение и смещение отсутствуют, таким 
образом матрица 
𝐑
0
является единичной матрицей 
3 × 3, а 𝐭
0
= 
[0 0 0]
𝑇
. Перемещение 
камеры между кадрами записывается как 
𝐑
𝑖
= 𝐑 ⋅ 𝐑
𝑖−1
, 
𝐭
𝑖
= 𝐭
𝑖−1
+ 𝐑 ⋅ 𝑡, 𝑖 = 1,2. . . 𝑛 . 
Тестирование визуальной одометрии с использованием FAST и ORB проводилось на 
наборе данных «KITTI odometry data set». Набор данных представляет собой 
последовательность изображений, снятых с камеры, закрепленной на автомобиле. 

---

Альманах научных работ молодых ученых 
XLVII научной и учебно-методической конференции Университета ИТМО. Том 1 
223 
а 
б 
Рис. 2. Красным цветом обозначена истинная траектория, зеленым – траектория на основе 
визуальной одометрии с использованием сочетаний: ORB/brute-force matcher (а) и FAST/KLT 
tracker (б) 
Как можно видеть сочетание алгоритмов FAST и KLT дает более точную оценку 
траектории рис. 2, однако со временем ошибка неизбежно накапливается, что ведет к 
серьезному отклонению от истинной траектории. Еще одна проблема возникает, когда не 
наблюдается ни одной ключевой точки, например, в какой-то момент времени камера была 
закрыта препятствием или движение было слишком быстрым, и кадр оказался размытым. 
Такие нарушения препятствуют продолжению работы простейшего алгоритма визуальной 
одометрии, поэтому возникает необходимость реинициализации. Вышеуказанные проблемы 
решаются представлением позиций камеры в виде иерархической [5] системы графов, где 
вершины хранят информацию о кадре, в том числе ключевые точки, которые также служат 
набором визуальных слов, на основе которого можно произвести повторную 
инициализацию. Ребра графа представляют перемещение камеры. Фреймворк g2o позволяет 
решить задачу bundle adjustment, т.е. минимизировать дрейф. 
Алгоритм монокулярного SLAM широко применим за рамками проекта с 
квадрокоптером Parrot ARDrone 2.0, поскольку для его работы необходима лишь одна 
откалиброванная камера. Визуальная одометрия может быть использована для коррекции 
информации о местоположении робота в случаях, когда другие датчики имеют значительную 
ошибку, которая накапливается с течением времени. Карта, полученная в результате работы 
алгоритма, является разреженной и не подходит для точных измерений, однако позволяет 
составить общее представление о видимых препятствиях. В дальнейшем эта карта может 
быть конвертирована в структуру octomap, удобную для решения задач поиска пути.