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

Это вторая статья из серии статей об использовании стерео камеры с 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. До новых встреч!

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

RTAB‑Map

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

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

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

ORB‑SLAM 3

Локальный ИИ класса Opus 4.6 «задешево»: две Tesla P100, 28-поточный Xeon и тесты на реальных задачах

Anthropic раскрыла сеть из дейтинговых приложений с ИИ‑персонажами, работающими на Claude

Microsoft показала, как с помощью ИИ‑агента можно собрать нативное приложение для Windows

ИИ уже умеет собирать микросервисы по одному промпту, но, как разработчик, я всё ещё нужен

Публикации