Vision-based SLAM: стерео- и depth-SLAM
После небольшого перерыва мы продолжаем серию статей-уроков по SLAM. В предыдущих выпусках мы подготовили программное окружение, а также поработали с монокулярным SLAM. Под катом — урок по использованию SLAM на основе стереокамеры и камеры глубины. Мы расскажем о настройке пакетов и оборудования и дадим советы по использованию двух ROS-пакетов: ставшего традиционным RTAB-Map и свежего вкусного ElasticFusion.
RTAB-Map + стереокамера
Принцип работы
RTAB-Map (Real-Time Appearance-Based Mapping) — это алгоритм визуального графового SLAM, основанный на детекторе замыканий. Детектор использует алгоритм bag-of-words для определения степени подобия новых изображений с камеры изображениям уже посещённых локаций. Каждое такое замыкание добавляет в граф положений камеры новое ребро, после чего граф оптимизируется. Для достижения реалтайма алгоритм применяет хитрый способ управления памятью, ограничивающий количество используемых локаций. В статье разработчиков подробно описано, как все это работает.
Запуск
Настройку RTAB-Map с использованием сенсора Kinect мы рассмотрели на первом уроке, поэтому здесь займёмся настройкой этого пакета для использования со стереокамерой. В качестве таковой будем использовать две обычных USB веб-камеры.
Настройка и калибровка стереопары в ROS
Для корректной работы две камеры должны быть жёстко зафиксированы друг относительно друга (для этого можно использовать веб-камеры на прищепке:). Желательно, чтобы камеры были одной модели и не имели возможности ручной настройки фокусировки. Мы будем использовать вот такую горизонтальную «стереокамеру»:
Настройка камер в ROS
Мы воспользуемся пакетом usb_cam для получения видеопотока с камер. Создадим файл stereo_camera.launch со следующим содержанием:
где /dev/video0 следует заменить на идентификатор соответствующий левой/правой камере.
Если при запуске появляется ошибка вида:
[ERROR] [1455879780.737902972]: VIDIOC_STREAMON error 28, No space left on device
то это означает, что пропускной способности USB недостаточно по какой-то причине. Нужно попробовать подключить камеры к разным USB-концентраторам, это чаще всего решает проблему.
Удостовериться в том, что обе камеры работают, можно при помощи утилиты ROS rqt_image_view. Утилита должна показывать видео для топиков /stereo/left/image_raw и /stereo/right/image_raw.
Калибровка
Утилита camera_calibration в ROS поддерживает калибровку горизонтальной стереокамеры. Перед запуском нужно подготовить калибровочный образец — распечатать шахматный маркер. Запуск утилиты производится следующим образом:
rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.054 right:=/stereo/right/image_raw left:=/stereo/left/image_raw left_camera:=/stereo/left right_camera:=/stereo/right --approximate=0.01
где
--size 8×6 указывает количество внутренних углов шахматного узора (8×6 соответствует узору из 9×7 черных квадратов);
--square 0.054 — размер (стороны) квадрата шахматного узора в метрах;
--approximate=0.01 — в нашем случае используются неспециализированные камеры, поэтому необходимо указать параметр, который устанавливает допустимое время рассинхронизации между камерами в секундах.
Процесс калибровки стереопары при помощи этой утилиты не сильно отличается от калибровки одной камеры, приведенного в первой статье. По завершению калибровки необходимо нажать кнопку «commit» для сохранения данных калибровки.
Обработка стерео-изображения
После калибровки камер можно перейти к обработке стерео-изображения. Пакет stereo_image_proc выполняет ректификацию изображений с камер и строит карту диспаратности (disparity map). Запуск производится следующим образом:
rosrun stereo_image_proc stereo_image_proc __ns:=stereo _approximate_sync:=true
для проверки работоспособности можно запустить утилиту image_view со следующими параметрами:
rosrun image_view stereo_view stereo:=/stereo image:=image_rect_color _approximate_sync:=true
Параметры алгоритма обработки стерео-изображений можно настроить при помощи утилиты dynamic_reconfigure. Для запуска нужно выполнить команду:
rqt
и выбрать пункт Dynamic reconfigure в меню Plugins → Configuration.
stereo_image_proc поддерживает два алгоритма обработки: StereoBM (быстрее, первая картинка) и StereoSGBM (semi-global block matching, качественнее, медленнее, вторая картинка).
RTAB-Map
После того как камера была успешно откалибрована, и мы получили адекватную disparity map, можно запустить RTAB-Map. Подготовим launch-файл rtabmap.launch:
Данный файл создан на основе стандартного файла конфигурации RTAB-Map для камеры Bumblebee. В нем устанавливается ориентация системы координат камер относительно глобальной системы координат, запускаются модули одометрии (stereo_odometry в нашем случае), SLAM (rtabmap) и визуализации (rtabmapviz или rviz).
Если при запуске отображается ошибка:
[FATAL] The stereo baseline (-0.177000) should be positive
то необходимо поменять левую и правую камеры местами в файле stereo_camera.launch и откалибровать их заново.
Запустим RTAB-Map (эту команду нужно запускать из папки, содержащей rtabmap.launch):
roslaunch rtabmap.launch
Если все прошло хорошо, то мы увидим окно RTAB-Map:
Настройка параметров и возможные проблемы
RTAB-Map предоставляет следующие параметры для настройки работы алгоритма (их можно изменить в launch файле или передать через командную строку):
- strategy — алгоритм одометрии: 0=bag-of-words 1=оптический поток
- feature — тип детектора фич: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK. Лучше всего, по нашему мнению, себя показывает ORB, он и используется по умолчанию.
- estimation — режим работы одометрии: 0:3D, 1:2D.
- max_depth — максимальная используемая глубина фич из disparity map в метрах.
- inlier_distance — максимальное расстояние между фичами для RANSAC в метрах.
- local_map — максимальное сохраняемое количество фич в карте.
Когда использовать SLAM со стереокамерой?
После работы с монокулярными SLAM иметь возможность использовать две камеры — просто счастье, поскольку здесь нет никаких проблем с определением масштаба карты и локализации. Если у Вас есть стереокамера или хотя бы две одинаковых монокамеры — используйте их и стереоSLAM. Однако если Вы хотите добиться хорошего качества карты, то обязательно обзаведитесь настоящей стереокамерой, смонтированной в одном корпусе. Две веб-камеры на скотче — это забавное решение из разряда «я у мамы инженер» для написания урока по SLAM, но для настоящих задач необходимо настоящее оборудование.
ElasticFusion + RGB-D камера
Принцип работы
ElasticFusion позволяет строить плотные 3D-модели окружения на базе сурфелей (от англ. surface element). Алгоритм не использует граф посещённых локаций и полностью опирается только на построенную карту при локализации и поиске замыканий. Для поиска замыканий алгоритм случайно выбирает небольшие части карты, с которыми впоследствии сравниваются новые кадры. После нахождения замыкания участок карты деформируется в соответствии с ошибкой. Все остальные интересности можно увидеть в статье разработчиков. Сразу отметим, что алгоритм очень требователен к железу: для нормальной работы необходима видеокарта nVidia с производительностью более 3.5 TFlOPS, а также CPU вроде Intel Core i5 — i7.
Установка и запуск
Склонируйте репозиторий ElasticFusion в удобную Вам папку:
git clone https://github.com/mp3guy/ElasticFusion.git
Зависимостей у проекта немало, и среди них — CUDA. Если Вы уже установили драйвер видеокарты, и это не CUDA, то, к сожалению, придется немного поплясать с бубном. Танцы включают занесение драйверов видеокарты в черный список, установку при остановленном lightdm и всякие другие гадости, о которых можно прочитать подробно, например, здесь. Выполняйте все эти процедуры очень осторожно и с пониманием того, что Вы делаете, иначе можно легко распрощаться с системой.
Проще всего собрать ElasticFusion сразу со всеми зависимостями при помощи любезно подготовленного разработчиками скрипта. Перейдём в репозиторий и выполним скрипт:
cd ElasticFusion
./build.sh
Волшебный скрипт сделает за нас почти всё. «Почти» — потому что из коробки ElasticFusion работает только с драйверами OpenNI2. Мы используем Kinect первой версии, и, к счастью, существует простой способ добавить его поддержку в OpenNI2. Для этого сначала соберём libfreenect (клонируйте её в ту же папку, что и ElasticFusion, рядышком):
git clone https://github.com/OpenKinect/libfreenect.git
cd libfreenect
mkdir build
cd build
cmake .. -DBUILD_OPENNI2_DRIVER=ON
make
Затем добавим ссылку на драйвер freenect в OpenNI2:
ln -s lib/OpenNI2-FreenectDriver/libFreenectDriver.so ../../ElasticFusion/deps/OpenNI2/Bin/x64-Release/OpenNI2/Drivers/
Ура, теперь мы можем запустить это чудо техники, если у нас подключён Kinect:
cd ElasticFusion/GUI
./ElasticFusion
Если все хорошо, то появится вот такое окно:
Настройка параметров и возможные проблемы
Алгоритм предоставляет пачку параметров для тонкой настройки, которые можно передать в командной строке (а некоторые можно покрутить прямо в GUI). Пока мы запустили ElasticFusion с параметрами по умолчанию. Насколько хорошо оно так будет работать, зависит от конфигурации Вашего вычислительного железа, а также от используемой камеры глубины. Алгоритм может работать только в реалтайме; вообще, Вас могут поджидать такие проблемы:
- медленное «заполнение» карты сурфелями. Решается использованием более мощного видеоадаптера, а также настройкой параметров:
- установкой меньшего значения Surfel confidence threshold (параметр -c), например, в 2 вместо дефолтных 10;
- включением Fast odometry (-fo);
- есть еще пара вариантов, которые можно посмотреть на странице проекта, но их использовать не советуем — сильно падает качество.
- нестабильное нахождение замыканий. Решается подстройкой параметров модуля поиска замыканий (-ic, -cv, -pt).
Также мы рекомендуем включить релокализацию (-rl), чтобы при потере трекинга алгоритм мог восстановиться сам, а также включить frame-to-frame RGB tracking (-ftf), что дает более плавное движение камеры. Кроме того, необходимо настроить процент использования двух используемых алгоритмом трекеров, что делается установкой параметра -i.
Для нашего железа (Core i5 + GeForce GTX Titan) и Kinect’а первой версии мы подобрали следующие параметры, позволяющие добиться весьма неплохой работы алгоритма:
./ElasticFusion -c 2 -d 4 -i 5 -fo -ftf -rl -ic 1000 -cv 1e-02 -pt 150
Параметр -d здесь устанавливает границу в метрах, за которой значения глубины, полученные с сенсора, не будут использоваться. В итоге мы получили вот такую реконструкцию одной из комнат нашего офиса:
Когда использовать SLAM с камерой глубины?
Очень часто алгоритмы SLAM, использующие карты глубины, работают с плотными (dense) картами. Это неизбежно влечёт за собой существенные требования к вычислительным ресурсам, поэтому использование такого подхода, например, на малогабаритных роботах затруднено. Кроме того, область видимости таких сенсоров не слишком велика (конечно, если Вы не используете дорогущий 3D LiDAR), поэтому область применения ещё немного сужается. Если Вы не очень ограничены в вычислительных ресурсах, и Вам нужно решить задачу навигации в помещении — тогда проекты вроде ElasticFusion отлично Вам подойдут.
Также существуют и менее ресурсоемкие алгоритмы, например, тот же RTAB-Map может работать и с камерами глубины. Такое решение является компромиссным и отлично впишется в систему навигации менее умного робота.
Выводы и общие рекомендации по применению
- Использование стереокамеры или RGB-D сенсора позволяет устранить главную проблему монокулярных алгоритмов SLAM — принципиальную невозможность определения масштаба карты.
- В целом, требования алгоритмов к вычислительным ресурсам не зависят от того, что Вы используете — стерео или камеру глубины.
- Если Вам необходимо строить карту сцены с большой глубиной (например, вне помещений), то Вам нужна стереокамера (или LiDAR, что существенно дороже).
- По возможности используйте стереокамеры, уже произведённые в одном корпусе. Это единственный нормальный способ получить надёжную disparity map.
Этим мы завершаем серию уроков по визуальному SLAM. Не стесняйтесь уточнить/спросить/дополнить/опровергнуть/дискутировать, это для нас всегда приятно :)
Источники и ссылки
Статья 1: настройка окружения для работы со SLAM
Статья 2: монокулярный SLAM
Страница ElasticFusion на сайте разработчиков
Сайт RTAB-Map
