Хабр·Russia·Saturday, September 12, 2026 at 10:35 AM

Эксперименты с Visual SLAM и стерео камерой PS5 HD для ROS 2

Translate

vladpriv9 минут назад

Это вторая статья из серии статей об использовании стерео камеры с ROS 2. В первой статье я рассказал о том как настроить камеру PS5 HD для использования в ROS 2. В этой статье я расскажу об использовании стерео камеры для визуального SLAM в связке с ROS 2. Конкретно мы попробуем RTAB-Map и ORB-SLAM 3. Кому интересно прошу под кат.

RTAB-Map

Для запуска RTAB-Map в режиме стерео (StereoSLAM) с камерой PS 5 HD Camera в ROS 2 вам потребуется цепочка из трех основных компонентов:

Драйвер камеры (публикует /left/image_raw, /right/image_raw, /left/camera_info, /right/camera_info).
Узел ректификации image_proc (преобразует сырые кадры в image_rect на основе калибровки).
Узел RTAB-Map (rtabmap_slam или rtabmap_nodes/stereo_odometry).

Установка и настройка

Для начала установим пакет ROS 2 для RTAB-Map

sudo apt install ros-$ROS_DISTRO-rtabmap-ros

Запустим узел камеры.

RTAB-Map требует на вход ректифицированные (исправленные от дисторсии) изображения. Подробнее о ректификации можно прочитать здесь.

Для этого запустим лаунч ps5_stereo_processing.launch.py (image_procзапускается вместе с драйвером камеры).

ros2 launch ps5_camera ps5_stereo_processing.launch.py

Если вы увидите такой ворнинг все в порядке, это не критично

Выведем список топиков. В списке топиков вы должны увидеть топики /left/image_rect и /right/image_rect. Это ректифицированные кадры с камеры.

Запустим RTAB-Map

ros2 launch rtabmap_ros rtabmap.launch.py \
        stereo:=true \
        left_image_topic:=/left/image_rect \
        right_image_topic:=/right/image_rect \
        left_camera_info_topic:=/left/camera_info \
        right_camera_info_topic:=/right/camera_info \
        frame_id:=camera_link_optical \
        approx_sync:=true \
        rtabmapviz:=true

Объяснение параметров

stereo:=true — переключает RTAB-Map из режима RGB-D/лидара в режим обработчика стереопары (Stereo Visual Odometry + SLAM)
left_image_topic и right_image_topic — топики ректифицированных кадров
left_camera_info_topic и right_camera_info_topic — топики с параметрами калибровки кадра
frame_id:=camera_link_optical — система координат (фрейм) вашей камеры (он публикуется нодой ps5_stereo_processing)
approx_sync:=true — включение мягкой (приблизительной) синхронизации по времени между левым и правым кадрами (критично для USB стереокамер)
rtabmapviz:=true — открывает визуальное окно RTAB-Map с 3D-облаком точек, траекторией (Odometry) и обнаружением замыканий циклов (Loop Closures)

Решение проблем в работе RTAB-Map

При запуске RTAB-Map может появиться такая ошибка

Причина в том, что в калибровочном файле правой камеры right.yaml элемент матрицы проекции был записан со знаком плюс (79.659252) вместо минуса (-79.659252).

По стандарту ROS базовая линия стереопары рассчитывается как baseline = -, поэтому положительное значение привело к ошибочному отрицательному расстоянию между объективами (-0.0585 м).

Чтобы исправить ошибку нужно внести некоторые изменения в секции projection_matrix в файле right.yaml. Исходно была такая запись:

projection_matrix:
      data: [1361.202126, 0.000000, 652.803085, 79.659252, 0.000000, 1361.202126, 431.091667, 0.000000, 0.
  000000, 0.000000, 1.000000, 0.000000]

Правильно должно быть так:

projection_matrix:
      data: [1361.202126, 0.000000, 652.803085, -79.659252, 0.000000, 1361.202126, 431.091667, 0.000000, 0.
  000000, 0.000000, 1.000000, 0.000000]

Мы изменили знак для 79.659252.

Запустим и увидим окно программы RTAB-Map

Обратите внимание на то, что в окне 3D Map у нас красный фон (индикатор статуса LOST - потеря визуальной одометрии), а в окне Loop closure detection отсутствует изображение. Красный фон означает, что алгоритм не может найти минимально необходимое число устойчивых 3D-соответствий (inliers) между соседними кадрами во времени. Также стоит заметить что на нижнем изображении (в панели Odometry) видны желтые точки — RTAB-Map успешно находит ключевые признаки, но не может сопоставить их между левым и правым кадрами в 3D.

Одной из причин такой ситуации могут быть неверные данные калибровки в топике /left/camera_info и /right/camera_info.Также возможен рассинхрон кадров по времени (Timestamp Mismatch).

Добавим в команду запуска RTAB-Map параметр approx_sync_max_interval который задает максимально допустимую разницу во времени (в секундах) между топиками при их примерной синхронизации (approx_sync:=true):

ros2 launch rtabmap_ros rtabmap.launch.py
  stereo:=true   
  left_image_topic:=/left/image_rect   
  right_image_topic:=/right/image_rect
  left_camera_info_topic:=/left/camera_info
  right_camera_info_topic:=/right/camera_info
  frame_id:=camera_link_optical
  approx_sync:=true
  approx_sync_max_interval:=0.05
  rtabmap_args:="--Vis/CorType 0 --Vis/MinInliers 5 --Vis/MaxFeatures 1200 --Stereo/MaxDisparity 200 --Stereo/OpticalFlow false"

Теперь мы видим зеленый фон (сигнал успешного трекинга одометрии).

Также при запуске RTAB-Map со стереокамерой (например Sony PS5 HD Camera или любой другой кастомной стереопарой) может возникнуть неприятный эффект: кроме красного фона в окне rtabmapviz на панели Odometry а картинка выглядит «двоящейся» — левый и правый кадры накладываются друг на друга со сдвигом по вертикали и углу (как видно на картинке ниже). При старте SLAM успевает захватить десяток желтых 3D-точек, после чего трекинг намертво умирает.

Разберёмся, почему это происходит и как вернуть одометрию к жизни.

Что происходит «под капотом»?
Вертикальное и угловое двоение — нарушение базового правила эпиполярной геометрии. В правильно ректифицированной (rectified) стереопаре строки пикселей выровнены строго параллельно: любой объект на обоих снимках должен находиться на одной высоте (Δy = 0). Сдвиг обязан быть только горизонтальным — это и есть диспаратность (параллакс), из которой вычисляется глубина Z. Если наблюдается сдвиг по вертикали (ΔY ≠ 0) и углу наклона, RTAB-Map не может сопоставить точки вдоль горизонтальной эпиполярной линии, триангуляция глубины разрушается, и через несколько кадров одометрия уходит в красный экран.

4 главные причины срыва трекинга:

В RTAB-Map подаются сырые топики вместо ректифицированных (например, мы случайно подписали узел на /left/image_raw вместо /left/image_rect - в этом случае в SLAM попадают кадры с оптической дисторсией и механическим перекосом объективов). Поиск соответствий вдоль горизонтальных линий ломается сразу.
Погрешности стереокалибровки. Если калибровка выполнена с ошибками, матрицы ректификации оставляют остаточный сдвиг по вертикали. Ошибки даже в 1–2 пикселя достаточно, чтобы стерео- триангуляция отсекла большинство точек как выбросы (outliers).
Конфликт в дереве TF. Частая ошибка в launch-файлах — публикация статичного фрейма static_transform_publisher … map camera_link. RTAB-Map сам строит дерево map -> odom -> base_link -> camera_link. Наличие альтернативного родителя у map приводит к конфликтам в TF-буфере.
Завышенные пороги сопоставления точек. По умолчанию RTAB-Map ожидает идеальных текстур и жестко требует не менее 15–20 inliers на кадр (Vis/MinInliers).

Пошаговый чеклист решения

1. Проверяем ректификацию через rqt.

Запустите rqt GUI:

rqt

В верхнем меню выберите Plugins / Visualization / Image View. Повторим это действие дважды чтобы у нас было два Image View (одно для /left/image_rect, второе для /right/image_rect).

Одинаковые детали объектов на левом и правом кадрах должны лежать строго на одной горизонтальной линии. Если есть наклон или сдвиг по вертикали — стереопару нужно перекалибровать. Здесь на фото видно что ректификация корректная.

Шаг 2. Направляем в RTAB-Map правильные топики

Подписывайтесь исключительно на выпрямленные изображения image_rect (/left/image_rect и /right/image_rect).

Шаг 3. Убираем лишние TF-трансформации В launch-файле драйвера камеры уберите статические связи с фреймами map или odom. Оставьте только связь между базой робота и оптическим центром камеры

Так например у меня в launch файле была ошибка в трансформациях. У меня был такой код:

Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    name='static_tf_pub',
    arguments=['0', '0', '0', '0', '0', '0', 'map', 'camera_link_optical']
),

В строке с "arguments" была одна из ключевых причин сбоя SLAM и срыва одометрии в красный экран. Здесь присутствуют сразу две критические проблемы: конфликт дерева TF и нарушение оптических стандартов ROS (REP-103).

Корректный код должен быть таким:

Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    name='camera_to_optical_tf',
    arguments=['0', '0', '0', '-1.5707963', '0', '-1.5707963', 'camera_link', 'camera_link_optical']
),

При запуске RTAB-Map укажите параметр базового фрейма: frame_id:=camera_link.

После перекомпиляции пакета и source воркспейса проверим дерево трансформаций

ros2 run tf2_ros tf2_monitor

Мы должны увидеть что-то подобное

Давайте немного изменим команду запуска RTAB-Map SLAM:

ros2 launch rtabmap_ros rtabmap.launch.py \
  stereo:=true \
  left_image_topic:=/left/image_rect \  
  right_image_topic:=/right/image_rect \
  left_camera_info_topic:=/left/camera_info \
  right_camera_info_topic:=/right/camera_info \
  frame_id:=camera_link_optical \
  approx_sync:=true \
  publish_tf:=true \
  rtabmap_args:="--delete_db_on_start --Odom/ResetCountdown 1 --Odom/PublishNullWhenLost false --Vis/MinInliers 8" \
  rtabmap_viz:=true

Здесь мы добавили два новых параметра publish_tf:=true и rtabmap_args. Первый заставляет RTAB-Map публиковать трансформации в общую сеть (например, связь map -> odom), позволяя программам визуализации и навигационным системам понимать текущее положение робота на карте. Второй - rtabmap_args - это параметры алгоритма SLAM. Что дают эти параметры:

frame_id:=camera_link_optical — напрямую связывает одометрию с фреймом ваших картинок
--Odom/PublishNullWhenLost false — одометрия продолжает удерживать последнюю позицию фрейма odom даже при кратковременном срыве трекинга (TF-дерево не ломается).
--Odom/ResetCountdown 1 — при срыве трекинга одометрия не зависает намертво в красном экране, а автоматически пытается переинициализироваться со следующего кадра.
--Vis/MinInliers 8 — снижает порог чувствительности к количеству найденных ключевых точек (по умолчанию параметр Vis/MinInliers равен 15–20).

Пример команды запуска rtabmap_rosrtabmap_ros

ros2 launch rtabmap_ros rtabmap.launch.py \
  stereo:=true \
  left_image_topic:=/left/image_rect \
  right_image_topic:=/right/image_rect \
  left_camera_info_topic:=/left/camera_info \
  right_camera_info_topic:=/right/camera_info \
  frame_id:=camera_link_optical \
  approx_sync:=true \
  approx_sync_max_interval:=0.1 \
  publish_tf:=true \
  rtabmap_args:="--delete_db_on_start --Vis/CorType 0 --Vis/MaxFeatures 1500 --Vis/MinInliers 8 --Stereo/MaxDisparity 200 --Grid/RangeMax 4.0 --Odom/ResetCountdown 1" \
  rtabmap_viz:=true

В случае успешного трекинга кадров (например на улице) между кадрами окно будет выглядеть так

А вот здесь уже успешно строится карта

Для успешной работы RTAB-Map важно чтобы в окружении было достаточно много текстурных объектов.

Карта записывается в файл ~/.ros/rtabmap.db в домашней папке.

Чтобы просмотреть его выполним в терминале

rtabmap-databaseViewer ~/.ros/rtabmap.db

Повышение качество результата построения карты

Можно попробовать улучшить качество RTAB-Map.

Это требует комплексной настройки как фронтенда (одометрии), так и бэкенда (построения карты).

Во-первых, это тюнинг одометрии. Чтобы одометрия не «сыпалась» при поворотах и поиске признаков, оптимизируйте параметры поиска соответствий:

Можно заменить дескрипторы точек с чистого Optical Flow на ORB или SURF. Как показал ваш лог, стандартный трекинг по потоку часто дает сбои из-за разницы экспозиции между объективами. Используйте гибридный подход или классические дескрипторы (--Vis/CorType 0).
Увеличение количества ключевых признаков: Стандартного значения часто не хватает в комнатных условиях. Увеличьте лимит: --Vis/MaxFeatures 1500 или 2000.
Смягчение порога инлаеров: Если сцена бедна на текстуры, снизьте минимальное число валидных совпадений: --Vis/MinInliers 8 (или даже 6–7 для медленного перемещения).
Диапазон диспаратности: Для близко расположенных объектов расширьте поиск глубины: --Stereo/MaxDisparity 200 (или 256).

Во-вторых, фильтрация и уменьшение шума (Cloud & Map Optimization)

Облака точек со стереокамер часто содержат много шума («артефактов» висящих в воздухе точек). Настройте фильтры в RTAB-Map:

Фильтрация по максимальному расстоянию: Ограничьте дальность построения карты, так как стереобаза вашей камеры (~5.8 см) теряет точность на больших дистанциях: --Reg/MaxCorrespondenceDistance 0.05 (или ограничьте через параметры Grid: --Grid/RangeMax 4.0).
Отказ от шумовых точек (Grid Global / Noise Filtering): Включите удаление изолированных точек в облаке: --Grid/NoiseFilteringRadius 0.05 --Grid/NoiseFilteringMinNeighbors 5.

Во-третьих, Настройка Loop Closure (Замкнуть петлю)

Качество глобальной карты сильно зависит от успешности распознавания ранее пройденных мест: Увеличение порога гипотез петли: Чтобы избежать ложных замыканий (когда алгоритм ошибочно принимает один угол комнаты за другой), подкрутите порог уверенности: --Kp/MaxFeatures 800 --RGBD/LoopClosureRejection 0.6.

И наконец. Аппаратные и программные хитрости для ROS 2:

Абсолютная синхронизация времени: Обязательно используйте approx_sync:=true с увеличенным интервалом (approx_sync_max_interval:=0.1), так как программные драйверы веб-камер и PS5-камеры в Linux (v4l2_camera или аналоги) часто грешат джиттером таймстампов.
Качественное освещение: Убедитесь в отсутствии сильных теней и мерцания ламп (особенно 50/60 Гц), так как алгоритмы сопоставления яркости пикселей крайне чувствительны к перепадам света на одной из матриц.
Плавность движений: Перемещайте камеру медленно, избегайте резких рывков из-за rolling shutter эффекта.

ORB-SLAM 3

Теперь попробуем ORB-SLAM 3. Для начала нужно скомпилировать ORB-SLAM 3 как динамическую библиотеку.

Установим все необходимые пакеты:

sudo apt update && sudo apt upgrade -y
sudo apt install -y build-essential cmake pkg-config libglew-dev libboost-all-dev libssl-dev libgtk2.0-dev libeigen3-dev libopencv-dev

Скомпилируем Pangolin

cd ~
git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
git checkout v0.6                      
     
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
sudo make install

Клонируем оригинальный репозиторий проекта:

cd ~                                   
git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git ORB_SLAM3
cd ORB_SLAM3

На Ubuntu 20.04 могут возникнуть проблемы совместимости. При сборке могут возникнуть ошибки стандарта C++, путей к Eigen3 и заголовков OpenCV. Для решения проблемы со стандартом переключим стандарт на C++14 в CMakeLists.txt. В корневом CMakeLists.txt и в Thirdparty/Sophus/CMakeLists.txt замените стандарт C++11 на C++14 (Pangolin и современные версии Eigen требуют C++14).

В файле CMakeLists.txt замените строку:

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")

на

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")

Если компилятор ругается на отсутствие констант вроде CV_LOAD_IMAGE_UNCHANGED или CV_RGB2GRAY, добавьте в начало проблемных файлов (или в include/Settings.h):

#include <opencv2/imgproc/types_c.h>                                                                                                                                
#include <opencv2/imgcodecs/legacy/constants_c.h>

В файле Thirdparty/Sophus/sophus/so3.cpp найдите и закомментируйте/исправьте строку с unit_complex. Замените:

unit_complex.real() = 1.;                                                                                                                                        
unit_complex.imag() = 0.;

на

unit_complex.real(1.);                                                                                                                                              
unit_complex.imag(0.);

Сборка проекта. В корне репозитория есть скрипт build.sh, который собирает сторонние библиотеки (DBoW2, g2o, Sophus), распаковывает словарь и компилирует сам ORB-SLAM3.

Запускаем сборку:

chmod +x build.sh                                                                                                                                                   
./build.sh

Если в конце вы увидите [100%] Built target ORB_SLAM3 и список скомпилированных примеров в папке Examples/ — поздравляем, сборка прошла успешно!

Запускаем launch файл c ORB-SLAM 3:

ros2 launch ps5_camera ps5_orbslam3_stereo.launch.py rectify:=False config_path:=/home/vlad/configs/ps5_orbslam3_rectified.yaml

Откроется несколько окон

с rviz

ros2 launch ps5_camera ps5_orbslam3_stereo.launch.py rectify:=True rviz:=True config_path:=/home/vlad/configs/ps5_orbslam3_rectified.yaml

Если среда имеет хорошую текстуру (например улица летом) то мы увидим зеленые точки на кадре в окне Current Frame

немного переместим камеру и с помощью мышки изменим угол обзора в окне Map Viewer

В окне Map Viewer появится множество позиций камеры.

Добавим дисплей SLAM Path в rviz

и скроем одометрию

Тогда в основном окне мы увидим путь между позициями камеры

На этом пока все. Мы попробовали стерео камеру с двумя популярными алгоритмами Visual SLAM - RTAB-Map и ORB-SLAM 3. До новых встреч!

Если эта публикация вас вдохновила и вы хотите поддержать автора — не стесняйтесь нажать на кнопку

View the original on Хабр →

KioskNews shows a cleaned-up reading view extracted from the publisher’s page — the original always lives on their site, not ours.