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



Для более подробного разбора деталей сегодня мы ограничимся двумя реализациями монокулярного SLAM: ORB SLAM и LSD SLAM. Эти алгоритмы являются наиболее продвинутыми в своем классе из open-source проектов. Также очень распространен PTAM, однако он не так крут, как, например, ORB SLAM.

Извлечение параметров калибровки


Для всех алгоритмов монокулярного SLAM необходима точная калибровка камеры. Мы это проделали на прошлом уроке, теперь извлечем параметры камеры. Для используемой нами модели камеры нужно извлечь матрицу камеры (fx, fy, cx, cy) и 5 параметров функции дисторсии (k1, k2, p1, p2, k3). Перейдите в директорию ~/.ros/camera_info и откройте там YAML-файл с параметрами камеры. Содержимое файла будет выглядеть примерно так (вместо ardrone_front будет другое имя):

Файл калибровки
image_width: 640
image_height: 360
camera_name: ardrone_front
camera_matrix:
rows: 3
cols: 3
data: [569.883158064802, 0, 331.403348466206, 0, 568.007065238522, 135.879365106014, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.526629354780687, 0.274357114262035, 0.0211426202132638, -0.0063942451330052, 0]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [463.275726318359, 0, 328.456687172518, 0, 0, 535.977355957031, 134.693732992726, 0, 0, 0, 1, 0]

Нас интересуют поля camera_matrix и distortion_coefficients, они содержат нужные значения в следующем формате:

camera_matrix:
rows: 3
cols: 3
data: [fx, 0, fy, 0, cx, cy, 0, 0, 1]

distortion_coefficients:
rows: 1
cols: 5
data: [k1, k2, p1, p2, k3]

Сохраните эти значения, они нам пригодятся далее.

ORB SLAM


Принцип работы


Алгоритм ORB SLAM в целом не слишком отличается приципом работы от других визуальных SLAM. Из изображений извлекаются фичи (features). Далее при помощи алгоритма Bundle Adjustment фичи с разных изображений расставляются в 3D-пространстве, одновременно задавая расположение камеры в моменты съемки. Однако здесь есть и особенности. Во всех случаях используется единственный детектор фич – ORB (Oriented FAST and Rotated BRIEF). Это очень быстрый детектор (что позволяет достигать реалтайма без использования GPU), а получаемые ORB-дескрипторы фич с высокой степенью инвариантны к углу зрения, повороту камеры и освещенности. Это позволяет алгоритму с высокой точностью и надежностью отслеживать замыкания циклов, а также обеспечивает высокую надежность при релокализации. Алгоритм в итоге принадлежит к классу так называемых feature-based. ORB SLAM строит неплотную (sparse) карту местности, однако существует возможность построить плотную карту на основе изображений ключевых кадров. Поближе познакомиться с алгоритмом можно в статье разработчиков.

Запуск


Мы не описали процесс установки ORB SLAM в предыдущем уроке, поэтому остановимся на этом здесь. В дополнение к уже установленному окружению нам понадобится установить Pangolin (не клонируйте репозиторий в ROS workspace):

git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
mkdir build
cd build
cmake -DCPP11_NO_BOOST=1 ..
make -j

Далее установим собственно ORB SLAM (снова не стоит клонировать исходники в workspace):

git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2
cd ORB_SLAM2
chmod +x build.sh
./build.sh

Для использования пакета в ROS необходимо добавить путь к бинарникам в ROS_PACKAGE_PATH (замените PATH на путь, куда вы установили ORB SLAM):

echo export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS >> ~/.bashrc
source ~/.bashrc

Теперь нам потребуется внести данные калибровки камеры и настроек ORB SLAM собственно в файл настроек. Перейдем в директорию Examples/Monocular и скопируем файл TUM1.yaml:

cd Examples/Monocular
cp TUM1.yaml our.yaml

Откроем скопированный файл our.yaml и заменим параметры калибровки камеры на полученные выше, а также выставим FPS:

Файл настройки
%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------

# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 563.719912
Camera.fy: 569.033809
Camera.cx: 331.711374
Camera.cy: 175.619211

Camera.k1: -0.523746
Camera.k2: 0.306187
Camera.p1: 0.011280
Camera.p2: 0.003937
Camera.k3: 0

# Camera frames per second
Camera.fps: 30.0

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000

# ORB Extractor: Scale factor between levels in the scale pyramid
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid
ORBextractor.nLevels: 8

# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7

#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500


Сохраним файл. Теперь мы можем запустить ORB SLAM (выполните три команды в разных вкладках терминала):

roscore

rosrun usb_cam usb_cam_node _video_device:=dev/video0 < Ваше устройство может отличаться

rosrun ORB_SLAM2 Mono ../../Vocabulary/ORBvoc.txt our.yaml /camera/image_raw:=/usb_cam/image_raw

Если все прошло успешно, то вы должны увидеть два окна:



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



Все это замечательно, однако ORB SLAM был разработан как ROS-независимый пакет. Бинарник, который мы запускали, на самом деле всего лишь пример использования алгоритма в ROS. По неясной логике разработчики не включили в этот пример публикацию траектории движения, и лишь сохраняют её в виде текстового файла KeyFrameTrajectory.txt после завершения работы. Хотя такая публикация займет несколько строк кода.

Настройка параметров


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

Когда использовать ORB SLAM?


Если Вам нужен быстрый алгоритм, который должен работать, например, onboard, и окружение не содержит крупных плоских однотонных объектов – тогда Вам отлично подойдет ORB SLAM.

LSD SLAM


Принцип работы


Мы уже вкратце затрагивали принцип работы LSD SLAM в статье об опытах навигации AR.Drone. Более подробный разбор алгоритма явно не вписывается в формат урока, почитать о нем можно в статье разработчиков.

Запуск


После того, как Вы установили LSD SLAM (руководствуясь предыдущим уроком), для запуска необходимо подготовить:

  1. Файл калибровки камеры camera.cfg
    Создайте файл camera.cfg в директории ~/ros_workspace/rosbuild/package/lsd_slam/lsd_slam_core/calib
    и скопируйте параметры калибровки в первую строку camera.cfg по такому образцу (обратите внимание на то, что пятый параметр дисторсии не используется):

    fx fy cx cy k1 k2 p1 p2
    640 360
    crop
    640 480

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

  2. Файл запуска lsd_slam.launch

    <?xml version="1.0"?>
    <launch>
    <node pkg="usb_cam" type="usb_cam_node" name="camera" output="screen">
                    <param name="video_device" value="/dev/video0"/>
            </node>
    <node name="lsd_slam_node" type="live_slam" pkg="lsd_slam_core" args="/image:=usb_cam/image_raw _calib:=$(find lsd_slam_core)/calib/camera.cfg" output="screen">
      	<param name="minUseGrad" value="10" />
    	<param name="cameraPixelNoise" value="1"/>
      	<param name="KFUsageWeight" value="14" />
      	<param name="KFDistWeight" value="14" />
      	<param name="useAffineLightningEstimation" value="True" />
      	<param name="relocalizationTH" value="0.1" />
    	<param name="useFabMap" value="True"/>
      </node>
    
      <node name="image_view_node" type="image_view" pkg="image_view" args="image:=usb_cam/image_raw" respawn="true"/>
    </launch>
    


Запустите LSD SLAM (из папки с файлом запуска):

roslaunch lsd_slam.launch

Если все получилось, Вы должны увидеть два окна:



Также запустите просмотрщик облаков точек из поставки LSD SLAM (в другом окне терминала):

rosrun lsd_slam_viewer viewer

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



Настройка параметров



Алгоритм предоставляет несколько параметров для настройки, самые важные – вот эти:

  • minUseGrad – минимальный градиент интенсивности для создания новой 3D-точки. Чем меньше значение, тем лучше алгоритм работает с однотонными объектами и тем плотнее карта. Однако чем ниже это значение, тем более явно сказываются ошибки ректификации камеры на качестве SLAM. Кроме того, меньшие значения значительно снижают производительность алгоритма.
  • cameraPixelNoise – шум значений интенсивности пикселей. Нужно установить в значение больше, чем настоящий шум матрицы, чтобы учесть ошибки дисктретизации и интерполяции.
  • useAffineLightningEstimation – можно попробовать включить, чтобы устранить проблемы с автоэкспозицией.
  • useFabMap – включает openFabMap для поиска циклов.

Рекомендации


  1. Запускайте алгоритм на производительном CPU. В отличие от ORB SLAM, LSD SLAM выставляет значительные требования к железу. Кроме того, алгоритм обязательно должен работать в реалтайме, в противном случае о приемлемом качестве SLAM не может быть и речи.
  2. Как можно более точно откалибруйте камеру. Direct методы, к которым относится LSD SLAM, очень чувствительны к качеству калибровки.
  3. По возможности используйте global-shutter камеру. Rolling shutter можно использовать (собственно, мы использовали только такой тип затвора), но результаты будут хуже.

Когда использовать LSD SLAM?


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

Feature-based vs. Direct


Сравнивая монокулярные алгоритмы, основанные на фичах, с так называемыми direct алгоритмами, использующими изображения целиком, создатель LSD SLAM Jacob Engel показал на одной из своих презентаций такую таблицу (перевод наш):
Сравнение алгоритмов мнокулярного SLAM
Feature-based Direct
Используют только фичи (например, углы) Используют полное изображение
Быстрее Медленнее (но хорошо распараллеливаются)
Легко удалять шум (outliers) Не так просто удалить шум
Устойчивы к rolling shutter Неустойчивы к rolling shutter
Используют малую часть информации из изображений Используют более полную информацию
Не требуют сложной инициализации Требуют хорошей инициализации
более 20 лет интенсивной разработки около 4-х лет исследований

Сложно что-то добавить.

Общие рекомендации по применению


Все монокулярные алгоритмы обладают набором схожих требований и ограничений, а именно:

  1. Необходимость точной калибровки камеры. Не так критично для feature-based алгоритмов.
  2. Невозможность определить масштаб без помощи внешних сенсоров или пользователя.
  3. Требования к камере: высокий FPS + широкий угол обзора. Эти параметры связаны как друг с другом, так и с максимальной скоростью перемещения камеры.

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

  1. Вы строго ограничены одной камерой;
  2. у Вас есть возможность оценить масштаб локализации и карты за счет внешних источников либо для решения Вашей задачи масштаб не имеет значения;
  3. характеристики камеры удовлетворяют указанным выше требованиям и позволяют провести точную калибровку и ректификацию изображения.


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

Источники


Предыдущий урок – установка и настройка окружения
Сайт разработчиков LSD SLAM
Сайт разработчиков ORB SLAM

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


  1. MaxAlekseev
    21.02.2016 16:03

    Если подразумевается расположение бортового компьютера на квадрокоптере, то выбор скорее всего сведется Raspberry Pi 2 или его близким аналогам. Хватает ли производительности современных "малин" для решения этих задач на борту аппарата?


    1. niklyaev
      22.02.2016 02:18

      Для нормальной работы всех таких алгоритмов необходимо добиться реалтайма. Для LSD SLAM на Raspberry это вряд ли получится, ORB SLAM и PTAM можно попробовать, особенно ORB — извлечение ORB-фич достаточно быстрое, а на Raspberry Pi 2 уже неплохой CPU. Плюс можно попробовать использовать небольшое разрешение картинки. Мы пока не пробовали запускать все это onboard, однако в первом приближении ответ скорее "Да, хватит" :)