Добрый день, уважаемые читатели! В предыдущих статьях по робототехнической платформе ROS я коснулся темы локализации и построения карты местности, в частности мы изучили методы SLAM: gmapping в статье и hector_slam в статье. В этой статье я продолжу знакомство с алгоритмами локализации в ROS и представлю обзор нескольких алгоритмов визуальной одометрии, реализованных на платформе ROS. Визуальная одометрия имеет важное значение в робототехнике поскольку позволяет оценить перемещение робота, его текущую позицию и ускорение на основе данных видеопотока с камеры. Можно использовать как обычную RGB камеру (в этом случае говорят о монокулярной одометрии), так и стереокамеру (стерео одометрия) и даже RGBD камеру.
При использовании камер RGBD таких как Microsoft Kinect возможно получить более аккуратную визуальную одометрию, чем со стереокамерами, так как в этом случае мы используем 3D данные. В данной статье мы рассмотрим такие алгоритмы. Кого заинтересовала эта тема, прошу под кат.
rtabmap
rtabmap по своей сути является алгоритмом SLAM в ROS. В этом пакете кроме инструментов для SLAM есть приложение odometryViewer для тестирования различных методов визуальной одометрии. В rtabmap визуальная одометрия работает следующим образом: для вычисления одометрии алгоритм использует визуальные признаки, полученные из RGB изображения и данные глубины из карты глубины. Используя соответствия визуальных признаков (matching) между двумя изображениями, алгоритм RANSAC вычисляет трансформацию между последовательными кадрами.
Установить rtabmap на ROS Indigo и Kinetic очень просто через apt-get:
sudo apt-get install ros-<version>-rtabmap ros-<version>-rtabmap-ros
Также можно установить rtabmap и rtabmap_ros из исходников:
source /opt/ros/<version>/setup.bash
cd ~
git clone https://github.com/introlab/rtabmap.git rtabmap
cd rtabmap/build
cmake ..
make
cd ~/catkin_ws
git clone https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros
catkin_make -j1
Запустим odometryViewer:
rtabmap-odometryViewer
Откроется окно подобное этому:
Немного переместим камеру:
Попробуем запустить с параметрами. Например, с использованием метода bag-of-words (по умолчанию используется дескриптор SURF):
rtabmap-odometryViewer -bow
С использованием метода bag-of-words с дескриптором SIFT (0=SURF, 1=SIFT)
rtabmap-odometryViewer -bow 1
Используя метод FAST+BRIEF:
rtabmap-odometryViewer -bin
Также можно попробовать одометрию на основе метода ICP (Iterative Closest Point) используя параметр -icp.
Можно настроить частоту обработки с помощью параметра hz (целое число кадров секунду):
rtabmap-odometryViewer -hz 2
Также можно настроить внутренние параметры алгоритма такие как максимальное расстояние между инлайерами, максимальное число визуальных признаков для нахождения соответствия (matching), количество итераций в методе RANSAC/ICP.
В целом эксперименты с визуальной одометрией rtabmap показали, что алгоритм работает быстро, без задержек и точно определяет позицию камеры относительно сцены. Единственный минус алгоритма из пакета rtabmap в том, при его использовании не удалось отобразить данные одометрии в rviz. Здесь необходимо глубже изучить интеграцию с ROS.
Пакет fovis_ros
Пакет fovis_ros работает только под версией ROS Hydro. В репозитории есть ветка Indigo, но при компиляции в catkin воркспейсе fovis_ros падает с ошибкой.
Для установки fovis_ros нам будет нужна библиотека libfovis и сам пакет fovis_ros. Загрузим их из репозиториев github:
cd ~/catkin_ws/src
git clone https://github.com/srv/libfovis.git
cd libfovis
git checkout hydro
cd ..
git clone https://github.com/srv/fovis.git
cd fovis
git checkout hydro
cd ~/catkin_ws
catkin_make
source devel/setup.bash
Здесь нам нужно убедиться, что текущая ветка выбрана Hydro, иначе могут возникнуть проблемы при компиляции (текущая ветка в репозиториях — Indigo).
Теперь создадим лаунч для запуска fovis_ros:
cd ~/catkin_ws/src
git clone https://github.com/vovaekb/fovis_demo.git
сd ~/catkin_ws
catkin_make
source devel/setup.bash
Запустим fovis_demo.launch:
roslaunch fovis_demo fovis_demo.launch
Откроется окно rviz:
Немного переместим камеру и получим обновленненную позицию:
fovis_ros публикует данные в два топика: /kinect_odometer/odometry (одометрия) и /kinect_odometer/pose (позиция).
Теперь разберемся с содержимым лаунч файлов в моем примере. Для сведения лаунч файлы взяты из книги “Learning ROS for robotics programming — Second edition” из главы 5 под названием Computer vision.
Начнем с основного файла fovis_demo.launch.
В строке
<arg name="mode" default="no_registered"/>
мы устанавливаем параметру mode значение no_registered. Это означает, что мы используем no_registered информацию о глубине, т.е. карта глубины не регистрируется и не трансформируется в картинку с камеры RGB. Это сделано для ускорения обработки, поскольку в случае регистрации глубины алгоритм бы работал медленно.
Проверим частоту обновления одометрии:
rostopic hz /kinect_odometer/odometry
Мы получим подобный вывод:
average rate: 8.759
min: 0.084s max: 0.156s std dev: 0.02417s window: 9
average rate: 7.938
min: 0.084s max: 0.180s std dev: 0.02724s window: 16
average rate: 7.493
min: 0.084s max: 0.217s std dev: 0.03286s window: 23
average rate: 8.111
min: 0.068s max: 0.217s std dev: 0.03645s window: 33
Запустим fovis_demo с программной регистрацией с помощью параметра mode:=sw_registered:
roslaunch fovis_demo fovis_demo.launch mode:=sw_registered
Получим следующую информацию о частоте обновления одометрии:
average rate: 0.963
min: 1.022s max: 1.056s std dev: 0.01676s window: 3
average rate: 0.968
min: 1.020s max: 1.056s std dev: 0.01635s window: 4
average rate: 1.212
min: 0.509s max: 1.056s std dev: 0.25435s window: 6
Далее мы определяем файл конфигурации дисплеев для rviz:
<arg name="rviz_config" default="$(find fovis_demo)/config/rviz_$(arg mode).rviz"/>
Я не буду здесь рассматривать его содержимое. Только скажу, что он определяет внешний вид окна rviz: активные дисплеи для топиков, глобальные настройки типа Fixed Frame и т.д.
Далее следует определение параметра rviz и запуск драйвера для сенсора Kinect в зависимости от параметра mode:
<include file="$(find fovis_demo)/launch/openni_kinect_$(arg mode).launch"/>
Запускаем лаунч для fovis из нашего пакета также в зависимости от параметра mode:
<include file="$(find fovis_demo)/launch/fovis_$(arg mode).launch"/>
Поскольку мы оцениваем перемещение робота на основе перемещения камеры нам необходимо знать смещение или трансформацию из системы координат камеры в систему координат робота. Для этого мы публикуем статическую трансформацию между системами координат base_link → camera_link с помощью static_transform_publisher из пакета tf:
<node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf"
args="0 0 -0.05 0 0 0 base_link camera_link 100"/>
И наконец запускаем rviz:
<group if="$(arg rviz)">
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rviz_config)"/>
</group>
Я не буду рассматривать остальные лаунч файлы в данной статье. Это можно сделать самостоятельно при желании. Только скажу, что при запуске fovis_ros с параметром mode=sw_registered мы делаем throttling кадров с RGB камеры, т.е. перепубликацию сообщений из одного топика в другой с меньшей частотой обновлений (2.5 Гц) (подробнее можно почитать об этом здесь).
Для тех, кому интересно изучить алгоритм fovis вглубь, есть статья о деталях алгоритма.
Эксперименты с визуальной одометрией fovis_ros показали, что алгоритм работает не так быстро, как rtabmap, с маленькими задержками при перемещении камеры, но все-таки довольно точно определяет позицию камеры относительно сцены.
Надеюсь данный обзор алгоритмов визуальной одометрии будет полезным в вашей работе и поможет вам решить некоторые проблемы. Желаю вам успехов в ваших проектах и до новых встреч!
PS: Также прошу вас поучаствовать в опросе и выбрать версию ROS, которую вы используете в своей работе.
Автор: vovaekb90
