Добрый день, уважаемые читатели! В предыдущих статьях по робототехнической платформе 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, которую вы используете в своей работе.