Exponenta Banner

Считайте сообщения облака точек ROS 2 в Simulink и выполните сшивание Используя регистрацию

Этот пример использует:

Скрипт Open Live Script

В этом примере показано, как считать ROS 2 PointCloud2 сообщения в Simulink® и восстанавливают 3-D сцену путем объединения нескольких облаков точек с помощью алгоритма нормальных распределений преобразовывают (NDT). Этот пример требует Computer Vision Toolbox®.

Настройте сеть ROS 2

Загрузите два демонстрационных sensor_msgs/PointCloud2 сообщения, ptcloudMsg1 и ptcloudMsg2. Создайте узел ROS 2 с двумя издателями, чтобы опубликовать сообщения по темам /ptcloud1 и /ptcloud2. Для издателей, набор свойство Durability качества сервиса (QoS) к transientlocal. Это гарантирует, что издатели обеспечивают сообщения для любых подписчиков, которые присоединяются после того, как сообщения были уже отправлены. Опубликуйте сообщения с помощью send функция.

load("ros2PtcloudMsgs.mat");

node = ros2node("/point_cloud_publisher");
ptcloudPub1 = ros2publisher(node,"/ptcloud1","sensor_msgs/PointCloud2",Durability="transientlocal",Depth=2);
ptcloudPub2 = ros2publisher(node,"/ptcloud2","sensor_msgs/PointCloud2",Durability="transientlocal",Depth=2);

send(ptcloudPub1,ptcloudMsg1)
send(ptcloudPub2,ptcloudMsg2)

Считайте сообщения из сети ROS 2 в Simulink и выполните сшивание облака точек

Откройте модель.

modelName = "read_pointcloud_and_perform_stitching_example_model.slx"; 
open_system(modelName)

Убедитесь, что эти два Подписываются, блоки соединяются с темами, /ptcloud1 и /ptcloud2. Значением Длительности параметр QoS Подписать блоков является Transient local. Это гарантирует совместимость с настройками QoS издателей, которых вы сконфигурировали в предыдущем разделе. Сообщения от подписчиков питаются блоки Облака точек Чтения, чтобы извлечь данные об облаке точек, которые затем питаются registerAndStitchPointclouds Блок MATLAB function, чтобы выполнить регистрацию и сшивание.

Под вкладкой Simulation выберите ROS Toolbox> Variable Size Messages. Заметьте, что пределы значения по умолчанию Использования для этого типа сообщения ясны. Затем в Максимальном столбце длины проверьте что data свойство sensor_msgs/PointCloud2 сообщение имеет значение, больше, чем число точек в pointcloud (4915200). Запустите модель.

sim(modelName);

Модель выполняет регистрацию облака точек с помощью алгоритма NDT и оценивает 3-D преобразование облака точек, опубликованного на /ptcloud2 тема, относительно облака точек опубликована на /ptcloud1 тема. Затем это преобразовывает /ptcloud2 облако точек к системе координат привязки /ptcloud1 облако точек. Это затем сшивает преобразованное облако точек с /pcloud1 облако точек, чтобы создать 3-D мировую сцену. Это также визуализирует эти два облака точек наряду с мировой сценой.

Функция MATLAB для облака точек, сшивающего Используя регистрацию

Эта модель использует алгоритм в ExampleHelperRegisterAndStitchPointclouds Функция MATLAB, чтобы выполнить облако точек, сшивающее использование регистрации, состоя из этих шагов:

  1. Downsample облака точек с 10 сеточный фильтр cm, чтобы ускорить регистрацию и улучшить точность при помощи pcdownsample (Computer Vision Toolbox) функция.

  2. Выровняйте два прореженных облака точек с помощью pcregisterndt (Computer Vision Toolbox) , который использует алгоритм NDT, чтобы оценить 3-D твердое преобразование второго pointcloud относительно первого облака точек.

  3. Преобразуйте второе облако точек к системе координат первого облака точек с помощью pctransform (Computer Vision Toolbox) функция.

  4. Объедините преобразованное облако точек и первое облако точек с сеточным фильтром поля на 1,5 см при помощи pcmerge (Computer Vision Toolbox) функция.

  5. Визуализируйте эти два облака точек и восстановленную 3-D мировую сцену.

Для получения дополнительной информации о регистрации и сшивании облаков точек, смотрите 3-D Регистрацию облака точек и Сшивание (Computer Vision Toolbox). Регистрация является также ключевым начальным шагом в облаке точек приложения SLAM. Для обзора облака точек рабочий процесс SLAM с помощью MATLAB смотрите Облако точек Реализации SLAM в MATLAB (Computer Vision Toolbox).

function ExampleHelperRegisterAndStitchPointclouds(xyzPoints1, rgbValues1, xyzPoints2, rgbValues2)
    %%
    %#codegen
    % Declare functions not supported for code generation as extrinsic
    coder.extrinsic("pcshow");
    %% 
    %Create pointcloud objects from extracted xyz points and color data
    pcloud1 = pointCloud(xyzPoints1);
    pcloud1.Color = uint8(rgbValues1*255);
    pcloud2 = pointCloud(xyzPoints2);
    pcloud2.Color = uint8(rgbValues2*255);
    
    %Downsample the point clouds
    pcloud1Downsampled = pcdownsample(pcloud1,gridAverage=0.1);
    pcloud2Downsampled = pcdownsample(pcloud2,gridAverage=0.1);
    
    % Register the two pointclouds using the NDT algorithm 
    gridStep = 0.5;
    tform = pcregisterndt(pcloud2Downsampled,pcloud1Downsampled,gridStep);
    
    % Transform and align the frame of point cloud 2 to the frame of point cloud 1
    pCloudAligned = pctransform(pcloud2,tform);
    
    % Stitch the transformed point cloud 2 with point cloud 1
    mergeSize = 0.015;
    ptCloudScene = pcmerge(pcloud1, pCloudAligned, mergeSize);
    
    % Visualize the two point clouds and the merged point clouds
    subplot(2,2,1)
    pcshow(pcloud1.Location,pcloud1.Color,VerticalAxis="Y",VerticalAxisDir="Down");
    title("Point Cloud 1")
    view(0,-90);
    
    subplot(2,2,2)
    pcshow(pcloud2.Location,pcloud2.Color,VerticalAxis="Y",VerticalAxisDir="Down");
    title("Point Cloud 2")
    view(0,-90);
    
    subplot(2,2,[3,4])
    pcshow(ptCloudScene.Location,ptCloudScene.Color,VerticalAxis="Y",VerticalAxisDir="Down");
    title("Merged Point Cloud")
    view(0,-90);
end

Документация ROS Toolbox

Поддержка