Обзор алгоритмов 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
Создадим новую базу данных: File → New database:
Для камеры Kinect по умолчанию используется драйвер OpenNI-PCL:
Запустим процедуру построения карты, нажав на кнопку «Start»:
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:
Добавим дисплей Odometry и выберем топик »/rtabmap/odom». После некоторого перемещения камеры в пространстве мы получим в rviz подобную картинку:
После того, как мы полностью пройдем помещение с камерой, мы получим:
Если мы будем двигаться по помещению медленно, гладко перемещая камеру без резких движений, то мы может получить довольно точную карту:
Мы можем добавить дисплей Map типа OccupancyGrid и выбрать топик »/rtabmap/proj_map» для визуализации плоской карты (как если бы мы использовали gmapping):
При закрытии окна rviz база данных и карта автоматически сохраняются на диске (путь выводится в строке в терминале).
Использование rtabmap на Raspberry Pi 3 с ASUS Xtion Pro Live
Запустим rtabmap:
rtabmap
Откроется уже знакомое нам окно. Создаем базу данных: File → New database:
Выбираем драйвер OpenNI2 для камеры Xtion Pro Live:
Запускаем процедуру построения карты кнопкой Start:
Мы получаем:
Спустя некоторое время в процессе перемещения с камерой:
На Raspberry Pi процесс rtabmap у меня оказался довольно ресурсоемкий (использовал 250 — 300% CPU). Иногда окно темнело, один раз rtabmap упал с ошибкой Segmentation fault.
При использовании настроек по умолчанию rtabmap работает на Raspberry Pi крайне медленно, фреймрейт очень низкий. Для эффективной работы нужно настроить входную частоту кадров. Для этого в верхнем меню открываем Window → Preferences → General settings (GUI) и наживаем кнопку «Load settings…». Устанавливаем значение 30 Hz для «input rate»:
О настройке параметров в rtabmap можно подробнее прочитать здесь.
Теперь процедура построения карты работает намного быстрее:
На видео видно, как один раз теряются данные одометрии (возникает красный фон вокруг найденного кандидата Loop closure detection) и я делаю сброс одометрии через Detection → Reset odometry. Потеря одометрии часто связана с недостаточным числом найденных признаков (например, слаботекстурные поверхности) и слишком быстрым перемещением камеры. У нас полностью очищается карта и все начинается заново.
Карта почти готова:
Наша задача здесь получить успешную детекцию циклов (loop closure detection). В случае успешной детекции циклов кандидат подсвечивается зеленым фоном.
Также можно показать результат детекции цикла в панели »3D loop closure». Для этого в верхнем меню выберем: Window→Show view→3D Loop closure. Используя кнопки с цифрами от 1 до 5 мы можем изменить формат представления облаков точек (произвольные цвета, цветовой градиент по осям (axis oriented colors) или RGB):
Мы можем нажать кнопку Pause для приостановки и кнопку Stop для завершения процесса. Мы также можем выйти из программы и сохранить наш прогресс в базе данных. При закрытии программа покажет всплывающее окно с предложением сохранить изменения. В дальнейшем мы можем возобновить процесс, запустив программу и выбрав существующую базу данных.
Нам предложат загрузить карту для базы данных.
У меня получилась такая карта (я снимал часть помещения):
В настройках rtabmap (в верхнем меню Window → Preferences) можно выбрать алгоритм вычисления одометрии (использумый дескриптор визуальных признаков). Для этого в настройках выберем: RTAB-Map Settings → Memory → Vocabulary около пункта Feature selection и выберем алгоритм из выпадающего списка Visual word type:
В моих экспериментах одометрия с визуальным словарем 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
Мы увидим подобный экран:
Построение карты начнется сразу после запуска программы.
RGBDSLAM довольно ресурсоемкий, у меня команда top показала 155% загрузки процессора для процесса rgbdslam.
Когда построение карты завершено, необходимо остановить обработку потока с камеры, сняв галочку Processing в меню Processing:
Построение карты завершено:
Мы можем сохранить карту. Также можно сбросить текущий прогресс в процедуре построения карты, выбрав Reset в меню Processing:
Подробную инструкцию по использованию RGBDSLAM можно найти на официальной странице.
Как показали эксперименты, данные алгоритмы SLAM вполне пригодны для использования в робототехнических проектах на основе ROS при наличии RGBD камеры Microsoft Kinect и дают в результате неплохую карту местности. К сожалению, не все методы работают на ARM платформах (по крайней мере, на Raspberry Pi). Преимуществом инструмента rtabmap является возможность гибкой настройки различных параметров по своему желанию.
Таким образом, мы рассмотрели два наиболее известных алгоритма SLAM для RGBD камер, которые имеют собственную реализацию в ROS. Желающие могут ближе познакомиться с различными сценариями использования данных алгоритмов на официальных страницах (к сожалению, только на английском языке) и применить их в своих проектах. Желаю всем удачи в экспериментах и буду рад ответить на любые ваши вопросы в комментариях. До новых встреч!