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 со следующим содержанием:

<launch>
        <node name="left" ns="stereo" pkg="usb_cam" type="usb_cam_node" output="screen" >
                <param name="video_device" value="/dev/video0"/>
                <param name="image_width" value="640"/>
                <param name="image_height" value="480"/>
			 <param name="camera_frame_id" value="camera_link"/>
        </node>
	<node name="right" ns="stereo" pkg="usb_cam" type="usb_cam_node" output="screen" >
                <param name="video_device" value="/dev/video1"/>
                <param name="image_width" value="640"/>
                <param name="image_height" value="480"/>
			 <param name="camera_frame_id" value="camera_link"/>
        </node>
</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 8x6 указывает количество внутренних углов шахматного узора (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:

<?xml version="1.0"?>
<launch>
   <arg name="pi/2" value="1.5707963267948966" />
   <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
   <node pkg="tf" type="static_transform_publisher" name="camera_base_link"
        args="$(arg optical_rotate) base_link camera_link 100" /> 

<!-- Choose visualization -->
  <arg name="rtabmapviz"              default="true" /> 
  <arg name="rviz"                    default="false" />
  
  <!-- Corresponding config files -->
  <arg name="rtabmapviz_cfg"          default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
  <arg name="rviz_cfg"                default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="base_link"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="time_threshold"          default="0"/>             <!-- (ms) If not 0 ms, memory management is used to keep processing time on this fixed limit. -->
  <arg name="optimize_from_last_node" default="false"/>         <!-- Optimize the map from the last node. Should be true on multi-session mapping and when time threshold is set -->
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="rtabmap_args"            default="--delete_db_on_start"/>              
  
  <arg name="stereo_namespace"        default="/stereo"/>
  <arg name="left_image_topic"        default="$(arg stereo_namespace)/left/image_rect_color" />
  <arg name="right_image_topic"       default="$(arg stereo_namespace)/right/image_rect" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="$(arg stereo_namespace)/left/camera_info" />
  <arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
  <arg name="approximate_sync"        default="true"/>         <!-- if timestamps of the stereo images are not synchronized -->
  <arg name="compressed"              default="false"/>
   
  <arg name="subscribe_scan"          default="false"/>         <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
  <arg name="scan_topic"              default="/scan"/>
   
  <arg name="visual_odometry"         default="true"/>          <!-- Generate visual odometry -->
  <arg name="odom_topic"              default="/odom"/>         <!-- Odometry topic used if visual_odometry is false -->
  
  <arg name="namespace"               default="rtabmap"/>
  <arg name="wait_for_transform"      default="0.1"/>
  
  <!-- Odometry parameters: -->
  <arg name="strategy"            default="0" />       <!-- Strategy: 0=BOW (bag-of-words) 1=Optical Flow -->
  <arg name="feature"             default="2" />       <!-- Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK -->
  <arg name="estimation"          default="0" />       <!-- Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP) -->
  <arg name="nn"                  default="3" />       <!-- Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE (SIFT, SURF), 2=FLANN_LSH, 3=BRUTEFORCE (ORB/FREAK/BRIEF/BRISK) -->
  <arg name="max_depth"           default="10" />      <!-- Maximum features depth (m) -->
  <arg name="min_inliers"         default="20" />      <!-- Minimum visual correspondences to accept a transformation (m) -->
  <arg name="inlier_distance"     default="0.1" />     <!-- RANSAC maximum inliers distance (m) -->
  <arg name="local_map"           default="1000" />    <!-- Local map size: number of unique features to keep track -->
  <arg name="odom_info_data"      default="true" />    <!-- Fill odometry info messages with inliers/outliers data. -->
  <arg name="variance_inliers"    default="true"/>    <!-- Variance from inverse of inliers count -->  
        
  <!-- Nodes -->
  <group ns="$(arg namespace)">
  
    <!-- Odometry -->
    <node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" args="--udebug">
      <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
	  
      <param name="frame_id"                 type="string" value="$(arg frame_id)"/>
      <param name="wait_for_transform_duration"       type="double"   value="$(arg wait_for_transform)"/>
      <param name="approx_sync"              type="bool"   value="$(arg approximate_sync)"/>
	  
      <param name="Odom/Strategy"            type="string" value="$(arg strategy)"/> 
      <param name="Odom/FeatureType"         type="string" value="$(arg feature)"/>  
      <param name="OdomBow/NNType"           type="string" value="$(arg nn)"/>
      <param name="Odom/EstimationType"      type="string" value="$(arg estimation)"/> 
      <param name="Odom/MaxDepth"            type="string" value="$(arg max_depth)"/>  
      <param name="Odom/MinInliers"          type="string" value="$(arg min_inliers)"/> 
      <param name="Odom/InlierDistance"      type="string" value="$(arg inlier_distance)"/>       
      <param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/> 
      <param name="Odom/FillInfoData"        type="string" value="true"/>   
      <param name="Odom/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>
    </node>
  
    <!-- Visual SLAM (robot side) -->
    <!-- args: "delete_db_on_start" and "udebug" -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="subscribe_depth"     type="bool"   value="false"/>
      <param name="subscribe_stereo"    type="bool"   value="true"/>
      <param name="subscribe_laserScan"      type="bool"   value="$(arg subscribe_scan)"/>
      <param name="frame_id"            type="string" value="$(arg frame_id)"/>
      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
      <param name="database_path"       type="string" value="$(arg database_path)"/>
      <param name="stereo_approx_sync"  type="bool"   value="$(arg approximate_sync)"/>
	
      <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
      
      <param name="Rtabmap/TimeThr"           type="string" value="$(arg time_threshold)"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/>
      <param name="LccBow/MinInliers"         type="string" value="10"/>
      <param name="LccBow/InlierDistance"     type="string" value="$(arg inlier_distance)"/>
      <param name="LccBow/EstimationType"     type="string" value="$(arg estimation)"/> 
      <param name="LccBow/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>
      
      <!-- when 2D scan is set -->
      <param if="$(arg subscribe_scan)" name="RGBD/OptimizeSlam2D"          type="string" value="true"/>
      <param if="$(arg subscribe_scan)" name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
      <param if="$(arg subscribe_scan)" name="LccIcp/Type"                  type="string" value="2"/> 
	  <param if="$(arg subscribe_scan)" name="LccIcp2/CorrespondenceRatio"  type="string" value="0.25"/>
    </node>
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen">
      <param name="subscribe_depth"      type="bool"   value="false"/>
      <param name="subscribe_stereo"     type="bool"   value="true"/>
      <param name="subscribe_laserScan"       type="bool"   value="$(arg subscribe_scan)"/>
      <param name="subscribe_odom_info"  type="bool"   value="$(arg visual_odometry)"/>
      <param name="frame_id"             type="string" value="$(arg frame_id)"/>
      <param name="wait_for_transform_duration"   type="double"   value="$(arg wait_for_transform)"/>
    
      <remap from="left/image_rect"        to="$(arg left_image_topic)"/>
      <remap from="right/image_rect"       to="$(arg right_image_topic)"/>
      <remap from="left/camera_info"       to="$(arg left_camera_info_topic)"/>
      <remap from="right/camera_info"      to="$(arg right_camera_info_topic)"/>
      <remap from="scan"                   to="$(arg scan_topic)"/>
      <remap unless="$(arg visual_odometry)" from="odom"  to="$(arg odom_topic)"/>
    </node>
  
  </group>
  
  <!-- Visualization RVIZ -->
  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
    <remap from="left/image"        to="$(arg left_image_topic)"/>
    <remap from="right/image"       to="$(arg right_image_topic)"/>
    <remap from="left/camera_info"  to="$(arg left_camera_info_topic)"/>
    <remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
    <remap from="cloud"             to="voxel_cloud" />

    <param name="decimation"  type="double" value="2"/>
    <param name="voxel_size"  type="double" value="0.02"/>
    <param name="approx_sync" type="bool"   value="$(arg approximate_sync)"/>
  </node>

</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 файле или передать через командную строку):

1. strategy — алгоритм одометрии: 0=bag-of-words 1=оптический поток
2. feature — тип детектора фич: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK. Лучше всего, по нашему мнению, себя показывает ORB, он и используется по умолчанию.
3. estimation — режим работы одометрии: 0:3D, 1:2D.
4. max_depth — максимальная используемая глубина фич из disparity map в метрах.
5. inlier_distance — максимальное расстояние между фичами для RANSAC в метрах.
6. 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 с параметрами по умолчанию. Насколько хорошо оно так будет работать, зависит от конфигурации Вашего вычислительного железа, а также от используемой камеры глубины. Алгоритм может работать только в реалтайме; вообще, Вас могут поджидать такие проблемы:

1. медленное “заполнение” карты сурфелями. Решается использованием более мощного видеоадаптера, а также настройкой параметров:
   1. установкой меньшего значения Surfel confidence threshold (параметр -c), например, в 2 вместо дефолтных 10;
   2. включением Fast odometry (-fo);
   3. есть еще пара вариантов, которые можно посмотреть на странице проекта, но их использовать не советуем – сильно падает качество.
2. нестабильное нахождение замыканий. Решается подстройкой параметров модуля поиска замыканий (-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 может работать и с камерами глубины. Такое решение является компромиссным и отлично впишется в систему навигации менее умного робота.

Выводы и общие рекомендации по применению

1. Использование стереокамеры или RGB-D сенсора позволяет устранить главную проблему монокулярных алгоритмов SLAM — принципиальную невозможность определения масштаба карты.
2. В целом, требования алгоритмов к вычислительным ресурсам не зависят от того, что Вы используете — стерео или камеру глубины.
3. Если Вам необходимо строить карту сцены с большой глубиной (например, вне помещений), то Вам нужна стереокамера (или LiDAR, что существенно дороже).
4. По возможности используйте стереокамеры, уже произведённые в одном корпусе. Это единственный нормальный способ получить надёжную disparity map.

Этим мы завершаем серию уроков по визуальному SLAM. Не стесняйтесь уточнить/спросить/дополнить/опровергнуть/дискутировать, это для нас всегда приятно :)

Источники и ссылки

Статья 1: настройка окружения для работы со SLAM
Статья 2: монокулярный SLAM
Страница ElasticFusion на сайте разработчиков
Сайт RTAB-Map

Автор: Singularis

Источник