Хабр·Russia·Monday, October 12, 2026 at 11:41 AM UTC

Построение dense облака точек с помощью стерео камеры и ROS 2

Translate

vladpriv13 минут назад

Это третья статья из серии статей об использовании стерео камеры с 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 пиксель между соседними пикселями.Формула: $P_1 = 8 \times C \times \text{correlation\_window\_size}^2$ (где — число каналов изображения, обычно 1). Влияние: Отвечает за гладкость на гранях и плавно наклонных поверхностях.
P2 (double) - Штраф за изменение диспаратности более чем на 1 пиксель между соседними пикселями.Формула: $P_2 = 32 \times C \times \text{correlation\_window\_size}^2$ . Влияние: Чем выше , тем сильнее алгоритм сопротивляется разрывам и резким скачкам глубины внутри одного объекта (делает плоские стены и столы сплошными). Примечание: Условие обязательно.

Очень хорошее объяснение интуиции в алгоритме диспаратности можно найти в этом видео.

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

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

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.