Обзор алгоритмов SLAM для камер глубины в ROS

Добрый день уважаемые читатели! В последней статье я уже писал об алгоритме rtabmap SLAM в контексте методов визуальной одометрии. В этой статье я расскажу об этом алгоритме SLAM более подробно, а также представлю обзор другого известного алгоритма SLAM, предназначенного для камер глубины — RGBDSLAM. Кого заинтересовало, прошу под кат.

rtabmap


О проекте можно прочитать подробно на официальной странице.

Процедуру установки rtabmap я подробно описывал в предыдущей статье. Например, на системе Ubuntu 14.04 установка будет выглядеть таким образом:

sudo apt-get install ros-indigo-rtabmap ros-indigo-rtabmap-ros


Однако, на Raspberry Pi 3 с установленным ROS Kinetic этот способ не будет работать, поскольку билд rtabmap отключен в релизе Kinetic для ARM из-за проблемы с libpcl-dev (подробнее о проблеме можно прочитать здесь). Поэтому скомпилируем его из исходников следуя инструкции на странице rtabmap:

source /opt/ros/kinetic/setup.bash
cd ~
git clone https://github.com/introlab/rtabmap.git rtabmap
cd rtabmap/build
cmake ..  [<---double dots included]
make


Если на Raspberry Pi не доступен swap, то во время компиляции может не хватить виртуальной памяти:

virtual memory exhausted: Cannot allocate memory


Нужно добавить swap память по инструкции отсюда и заново запустить компиляцию.
Компиляция должна занять около часа. После этого выполним установку:

sudo make install


Установим rtabmap_ros пакет в нашем рабочем каталоге catkin:

cd ~/catkin_ws
git clone https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros
catkin_make -j1
source devel/setup.bash


При выполнении catkin_make возможно появление ошибки из-за отсутствия файла image_transportConfig.cmake. В таком случае нужно скомпилировать пакет image_transport в рабочем каталоге catkin_ws:

cd src
git clone https://github.com/ros-perception/image_common.git
cd ~/catkin_ws
catkin_make -j1
source devel/setup.bash


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

echo 'export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib' >> ~/.bashrc

Использование rtabmap для построения карты


Запустим rtabmap:

rtabmap

image

Создадим новую базу данных: File → New database:

image

Для камеры Kinect по умолчанию используется драйвер OpenNI-PCL:

image

Запустим процедуру построения карты, нажав на кнопку «Start»:

image

rtabmap также можно использовать с rviz:

roslaunch openni_launch openni.launch depth_registration:=true
roslaunch rtabmap_ros rtabmap.launch rtabmapviz:=false rviz:=false rtabmap_args:="--delete_db_on_start"


Откроется окно rviz:

image

Добавим дисплей Odometry и выберем топик »/rtabmap/odom». После некоторого перемещения камеры в пространстве мы получим в rviz подобную картинку:

image

После того, как мы полностью пройдем помещение с камерой, мы получим:

image

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

image

Мы можем добавить дисплей Map типа OccupancyGrid и выбрать топик »/rtabmap/proj_map» для визуализации плоской карты (как если бы мы использовали gmapping):

image

При закрытии окна rviz база данных и карта автоматически сохраняются на диске (путь выводится в строке в терминале).

Использование rtabmap на Raspberry Pi 3 с ASUS Xtion Pro Live


Запустим rtabmap:

rtabmap


Откроется уже знакомое нам окно. Создаем базу данных: File → New database:

image

Выбираем драйвер OpenNI2 для камеры Xtion Pro Live:

image

Запускаем процедуру построения карты кнопкой Start:

image

Мы получаем:

image

Спустя некоторое время в процессе перемещения с камерой:

image

На Raspberry Pi процесс rtabmap у меня оказался довольно ресурсоемкий (использовал 250 — 300% CPU). Иногда окно темнело, один раз rtabmap упал с ошибкой Segmentation fault.

При использовании настроек по умолчанию rtabmap работает на Raspberry Pi крайне медленно, фреймрейт очень низкий. Для эффективной работы нужно настроить входную частоту кадров. Для этого в верхнем меню открываем Window → Preferences → General settings (GUI) и наживаем кнопку «Load settings…». Устанавливаем значение 30 Hz для «input rate»:

image

О настройке параметров в rtabmap можно подробнее прочитать здесь.

Теперь процедура построения карты работает намного быстрее:

На видео видно, как один раз теряются данные одометрии (возникает красный фон вокруг найденного кандидата Loop closure detection) и я делаю сброс одометрии через Detection → Reset odometry. Потеря одометрии часто связана с недостаточным числом найденных признаков (например, слаботекстурные поверхности) и слишком быстрым перемещением камеры. У нас полностью очищается карта и все начинается заново.

image

Карта почти готова:

image

Наша задача здесь получить успешную детекцию циклов (loop closure detection). В случае успешной детекции циклов кандидат подсвечивается зеленым фоном.

Также можно показать результат детекции цикла в панели »3D loop closure». Для этого в верхнем меню выберем: Window→Show view→3D Loop closure. Используя кнопки с цифрами от 1 до 5 мы можем изменить формат представления облаков точек (произвольные цвета, цветовой градиент по осям (axis oriented colors) или RGB):

image

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

image

Нам предложат загрузить карту для базы данных.

image

У меня получилась такая карта (я снимал часть помещения):

image

В настройках rtabmap (в верхнем меню Window → Preferences) можно выбрать алгоритм вычисления одометрии (использумый дескриптор визуальных признаков). Для этого в настройках выберем: RTAB-Map Settings → Memory → Vocabulary около пункта Feature selection и выберем алгоритм из выпадающего списка Visual word type:

image

В моих экспериментах одометрия с визуальным словарем GFTT+BRIEF работает лучше всего, BRISK показала худший результат (не удалось получить детекцию цикла совсем). При использовании GFTT+BRIEF, детекция цикла была получена сразу после полного оборота с камерой вокруг комнаты. Алгоритм ORB используется по умолчанию и дает не очень хорошие результаты.

rtabmap поддерживает довольно широкий выбор камер (RGB-D камеры Intel RealSense, ASUS Xtion, Kinect v1 и v2, а также стерео камеры Bumblebee2 и ZED camera) и несколько возможных сценариев построения карты с использованием различных комбинаций камеры, лидара и сенсора IMU (туториалы можно посмотреть здесь).

RGBDSLAM


Установка RGBDSLAMv2


О RGBDSLAM можно прочитать подробно на официальной странице ROS и на странице github.

Устанавливаем RGBDSLAM из исходников:

cd ~/catkin_ws/src
git clone https://github.com/felixendres/rgbdslam_v2.git


Устанавливаем библиотеку libg2o:

sudo apt-get install ros--libg2o


где ros_version — версия ROS (hydro, indigo или kinetic).
Компилируем RGBDSLAM в рабочем каталоге catkin:

cd ~/catkin_ws
catkin_make
source devel/setup.bash
rosdep install rgbdslam


Мне не удалось установить RGBDSLAM на Raspberry Pi из-за проблемы с Qt, поэтому здесь я рассматриваю работу алгоритма только на ROS Indigo с камерой Microsoft Kinect.

Использование RGBDSLAMv2


Запустим rosmaster:

roscore


RGBDSLAMv2 можно запустить двумя способами. Во-первых, с помощью launch файла:

roslaunch rgbdslam openni+rgbdslam.launch


Либо отдельно запустить openni_launch и узел RGBDSLAMv2:

roslaunch openni_launch openni.launch
roslaunch rgbdslam rgbdslam.launch


Мы увидим подобный экран:

image

Построение карты начнется сразу после запуска программы.
RGBDSLAM довольно ресурсоемкий, у меня команда top показала 155% загрузки процессора для процесса rgbdslam.
Когда построение карты завершено, необходимо остановить обработку потока с камеры, сняв галочку Processing в меню Processing:

image

Построение карты завершено:

image

Мы можем сохранить карту. Также можно сбросить текущий прогресс в процедуре построения карты, выбрав Reset в меню Processing:

image

Подробную инструкцию по использованию RGBDSLAM можно найти на официальной странице.

Как показали эксперименты, данные алгоритмы SLAM вполне пригодны для использования в робототехнических проектах на основе ROS при наличии RGBD камеры Microsoft Kinect и дают в результате неплохую карту местности. К сожалению, не все методы работают на ARM платформах (по крайней мере, на Raspberry Pi). Преимуществом инструмента rtabmap является возможность гибкой настройки различных параметров по своему желанию.

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

© Geektimes