Это третья статья из серии статей об использовании стерео камеры с ROS 2. В первой статье я рассказал о том как настроить камеру PS5 HD для использования в ROS 2. В этой статье я расскажу об использовании стерео камеры для построения dense облака в связке с ROS 2. Кому интересно прошу под кат.
Подготовка к построению dense облака
Для того чтобы получить корректный расчет глубины нам необходимо чтобы было выполнено важное условие: левый и правый кадры стереокамеры обязательно должны быть выровнены по горизонтали (эпиполярные линии должны совпадать) и ректифицированы. Мы уже говорили о ректификации в предыдущей статье. Чтобы убедиться что у нас запущены узлы ректификации для обоих каналов мы должны увидеть в списке топиков чистые ректифицированные топики /left/image_rect и /right/image_rect с сопутствующими /camera_info для каждой камеры.
В предыдущих статьях мы работали с пакетом ps5_camera_ros который можно скачать с github. В этой статье мы будет использовать файл ps5_stereo_processing.launch.py в котором уже есть узлы для ректификации кадров.
Для построения плотного облака точек используется ROS 2 пакет stereo_image_proc из стека image_pipeline. В файле ps5_stereo_processing.launch.py уже есть нужный узел.
Выполним команду в нашем воркспейсе
ros2 launch ps5_camera ps5_stereo_processing.launch.py
Выведем список топиков
ros2 topic list
Среди топиков должен быть топик /points2.
Посмотрим что данные в топик публикуются:
ros2 topic echo /points2 --no-arr
Параметр --no-arr скрывает детализированный вывод больших массивов данных, таких как изображения или облака точек, заменяя их метаданными, предотвращая зависания и перегрузку памяти.
Запустим rviz
ros2 run rviz2 rviz2
Откроется окно rviz. Добавим дисплей для топика /points2
Что мы видим? Облако точек рендерится и статус Ok зеленого цвета, но в геометрии облака есть явная аномалия: точки вытянуты длинными вертикальными «столбами» или лучами, уходящими вдаль от камеры (вдоль оси Z).
Это связано с несоответствием систем координат в оптическом стандарте (используется в OpenCV) и в стандарте ROS. Синяя ось (
), которая в стандартах ROS отвечает за глубину (расстояние от камеры до объекта), направлена строго вверх (вдоль мировой оси 
).
Нужно добавить следующий код для разворота оптической системы координат (camera_link_optical) относительно обычной физической камеры в наш launch файл:
Node( package='tf2_ros', executable='static_transform_publisher', name='camera_base_to_optical', arguments=[ '0', '0', '0', # x, y, z (смещение) '-1.5707', '0', '-1.5707', # Roll, Pitch, Yaw в радианах (-90° и -90°) 'camera_link', # Родительский фрейм 'camera_link_optical' # Дочерний оптический фрейм ] ),
Пересоберем пакет и запустим снова. Обратите внимание на Status: Warn
Проверьте, идет ли вещание в сеть:
ros2 run tf2_ros tf2_echo camera_link camera_link_optical
У меня была такая ошибка в выводе: "[INFO] [1788383812.087373889] [tf2_echo]: Waiting for transform camera_link -> camera_link_optical: Invalid frame ID “camera_link” passed to canTransform argument target_frame - frame does not exist".
Причина в том что фрейма camera_link не существует. Фрейм camera_link обычно должен публиковать ваш драйвер камеры (например, узел захвата PS5 камеры) или отдельный статический узел, который задает базовое положение камеры относительно робота (или начала координат).
Нужно добавить следующий код в launch файл:
Node( package='tf2_ros', executable='static_transform_publisher', name='world_to_camera_base', arguments=['0', '0', '0', '0', '0', '0', 'map', 'camera_link'] )
После пересборки Warning должен исчезнуть а команда tf2_ros tf2_echo camera_link должна выдавать такой вывод:
Вернемся в RVIZ и выберем слева для Fixed Frame значение map
Мы видим что теперь облако ориентировано влево (вдоль горизонтальной оси) что кажется разумным. Дело в том, что сетка в RViz всегда строится в плоскости XY выбранного Fixed Frame, а ось Z этого фрейма RViz считает вертикалью, направленной строго вверх (перпендикулярно сетке). В индустрии для камер принят оптический стандарт по которому ось Z направлена вдаль от объектива (глубина сцены). Поэтому когда мы выбирали Fixed Frame = camera_link_optical (оптический фрейм камеры) RViz брал оптическую ось Z (которая является глубиной вдаль от камеры) и направлял её вверх от сетки (облако вытягивалось «в потолок»). А когда Fixed Frame = map трансформация TF поворачивает её в горизонтальную ось X («вперёд»), укладывая облако точек естественно вдоль плоскости сетки. Данную трансформацию выполняет статический поворот tf2_ros в launch файле (camera_base_to_optical).
Настройка параметров для повышения качества алгоритма диспаратности
Самый быстрый способ подобрать оптимальные значения параметров алгоритма диспаратности под нашу PS5 камеру без правки кода и перезапуска лаунча это запустить в соседнем терминале утилиту rqt_reconfigure и настроить параметры динамически сразу глядя в RViz, как пропадают шумовые вертикальные «лучи».
Для начала установим пакет rqt-reconfigure
sudo apt install ros-{DISTRO}-rqt-reconfigure -y
Сделаем source /opt/ros/{DISTRO}/setup.bash и запустим
ros2 run rqt_reconfigure rqt_reconfigure --force-discover
Флаг --force-discover принудительно заставляет графический интерфейс rqt выполнить повторный поиск активных узлов и параметров, помогая избежать пустых списков из-за задержек инициализации в ROS 2.
Найдем в списке слева и наш узел disparity_node и нажмем на него. Мы увидим все параметры узла с ползунком (для численных параметров) и чекбоксами (для булевых):
Чтобы карта глубины не была «шумной» и заполняла как можно больше пространства (без провалов на однотонных стенах), параметры алгоритма сопоставления нужно настроить под вашу камеру (например, через динамические параметры ROS 2 / rqt_reconfigure):
prefilter_size / prefilter_cap: Нормализация яркости и контраста перед сопоставлением (помогает бороться с разницей автоэкспозиции левого и правого объективов).
correlation_window_size (int, по умолчанию: 5 или 9) - размер квадратного окна сопоставления в пикселях (должен быть нечетным: 3, 5, 7, 9 и т.д.). Влияние: Маленькое окно сохраняет четкие границы мелких деталей, но создает много шума. Большое окно хорошо сглаживает однотонные области, но размывает края и детали.
block_size: Размер окна сопоставления (обычно от 5 до 11). Меньше размер — выше детализация, но больше «дыр» и шума; больше размер — гладкая картинка, но теряются мелкие объекты.
min_disparity и num_disparities: Определяют диапазон поиска глубины. Для объектов вблизи увеличьте num_disparities (кратно 16, например, 128 или 256).
uniqueness_ratio: Процент уверенности. Повышение этого параметра (например, до 10–15) отсекает ложные срабатывания и шум на однотонных поверхностях.
P1 (double) - Штраф за изменение диспаратности на 1 пиксель между соседними пикселями.Формула:
(где 
— число каналов изображения, обычно 1). Влияние: Отвечает за гладкость на гранях и плавно наклонных поверхностях.P2 (double) - Штраф за изменение диспаратности более чем на 1 пиксель между соседними пикселями.Формула:
. Влияние: Чем выше 
, тем сильнее алгоритм сопротивляется разрывам и резким скачкам глубины внутри одного объекта (делает плоские стены и столы сплошными). Примечание: Условие 
обязательно.
Очень хорошее объяснение интуиции в алгоритме диспаратности можно найти в этом видео.
В настройке параметров нам также поможет визуализация карты диспаратности в RVIZ.
Для визуализации диспаратности в RViz я добавил в пакет ps5_camera легкий вспомогательный узел-конвертер, который слушает топик /disparity и публикует готовое цветное изображение в топик /disparity_color, чтобы его можно было без ошибок открывать обычным дисплеем Image в RViz2. В ROS 1 для этого был отдельный плагин RViz2 для DisparityImage.
Добавим новый дисплей с топиком /disparity_color
Добавим еще один дисплей для кадра с камеры (н-р, /left/image_rect)
Решение проблем с узлом диспаратности
Первой трудностью, с которой я столкнулся, стало замирание картинки в RViz2: карта глубины появлялась на секунду и зависала. Проблема оказалась двойной. Сначала подвела сетевая политика QoS: узел раскраски ожидал «легкие» данные (BEST_EFFORT), а источник отправлял их надежно (RELIABLE). Из-за высокого разрешения кадр весил около 4 мегабайт и разбивался на тысячи мелких UDP-пакетов. Под нагрузкой часть пакетов терялась, и система просто отбрасывала картинку целиком, даже не пытаясь ее восстановить. Второй причиной стала конкуренция за процессор. Функция раскраски в OpenCV пыталась задействовать все ядра процессора через OpenMP, что в итоге вошло в жесткий конфликт с потоками Python и ROS 2, полностью заморозив работу узла.
Для решения во-первых я перевёл QoS подписчика на RELIABLE с глубиной очереди depth=1.
Было:
sub_qos = QoSProfile( reliability=ReliabilityPolicy.BEST_EFFORT, ... depth=5 ) # Reliable publisher matches RViz2 default Image display pub_qos = QoSProfile( reliability=ReliabilityPolicy.RELIABLE, ... depth=5 )
Стало:
sub_qos = QoSProfile( reliability=ReliabilityPolicy.RELIABLE, ... depth=1 ) # Reliable publisher matches RViz2 default Image display; depth=1 ensures latest frame without latency lag pub_qos = QoSProfile( reliability=ReliabilityPolicy.RELIABLE, ... depth=1 )
Это включило механизм досылки потерянных пакетов (NACK/ACK) и гарантировало отображение только самого свежего кадра без накопления задержек.
Во-вторых, в коде узла ограничил многопоточность OpenCV через cv2.setNumThreads(1) во избежание дедлоков:
cv2.setNumThreads(1)
Во-третьих, я заменил тяжелые операции с булевыми масками NumPy на быстрый SIMD-метод cv2.convertScaleAbs.
Было:
valid_mask = (disp > min_disp) & (disp <= max_disp) & (~np.isnan(disp)) disp_range = max_disp - min_disp disp_scaled = np.zeros(disp.shape, dtype=np.uint8) if disp_range > 0: disp_scaled[valid_mask] = np.clip( ((disp[valid_mask] - min_disp) / disp_range) * 255.0, 0, 255 ).astype(np.uint8) color_img = cv2.applyColorMap(disp_scaled, self.colormap) color_img[~valid_mask] = [0, 0, 0]
Стало:
disp_range = max_disp - min_disp # Fast C++ SIMD scaling of float disparity to uint8 [0, 255] if disp_range > 0: scale = 255.0 / disp_range disp_scaled = cv2.convertScaleAbs(disp - min_disp, alpha=scale) else: disp_scaled = np.zeros(disp.shape, dtype=np.uint8) # Apply colormap color_img = cv2.applyColorMap(disp_scaled, self.colormap) # Mask out invalid disparity regions (negative, <= min_disp, > max_disp, or NaN) invalid_mask = (disp <= min_disp) | (disp > max_disp) | np.isnan(disp) color_img[invalid_mask] = 0
И наконец в launch-файле включил автоперезапуск respawn=True
Было
Node( package='ps5_camera', executable='disparity_color_node', name='disparity_color_node', output='screen', parameters=[{ 'colormap': 'TURBO', }] ),
Стало
Node( package='ps5_camera', ... respawn=True, respawn_delay=2.0, parameters=[{ 'colormap': 'TURBO', }] ),
В результате поток диспаратности ожил и стал стабильно транслироваться в RViz2 на частоте 13–15 FPS в реальном времени.
Другой проблемой был такой ворнинг при запуске rqt_reconfigure
Это означает что rqt_reconfigure отправил сервисный запрос параметров узлу /disparity_node, но не получил ответа в течении заданного таймаута в 1 секунду. Почему узел не отвечает за 1 секунду? Оказалось что узел запустился но его исполнитель (executor) наглухо занят расчетом кадров.
Как исправить? Изменить параметр full_dp на False. Затем 'queue_size' = 2 (было 30) для узлов disparity_node и point_cloud_node.
Посмотрим потребление памяти и CPU до изменений:
После изменений посмотрим потребление памяти
Покажу несколько примеров как изменение параметров алгоритма диспаратности влияет на карту глубины.
Если поставим маленькое значение speckle_size (н-р, 30) то мы увидим много мелких разноцветных пятен
Если поставим speckle_size = 1000 то из карты диспаратности исчезнут мелкие блобы
Можно переключить алгоритм на StereoSGBM, который значительно лучше справляется с однородными текстурами и дает намного меньше шума. Затем нужно подобрать значение параметра disparity_range: 64 или 128 (значение обязательно должно быть кратно 16). Значение 128 идеально подходит для близких и средних дистанций при базе 5.8 см.
Такой результат для speckle_size = 0
Изменим значение uniqueness_ratio на 5.0
В итоге после долгих экспериментов мне удалось получить хороший результат только на радиаторе
С такими параметрами алгоритма
К сожалению "идеальной конфигурации" алгоритма не существует. Нужно индивидуально подбирать параметры.
На этом пока все. Мы попробовали стерео камеру для построения dense облака в связке с ROS 2. До новых встреч!
