Read ROS 2 Point Cloud Messages in Simulink and Perform Stitching Using Registration

This example uses:

This example shows how to read ROS 2 PointCloud2 messages into Simulink® and reconstruct a 3-D scene by combining multiple point clouds using the normal distributions transform (NDT) algorithm. This example requires Computer Vision Toolbox®.

Set Up ROS 2 Network

Load two sample sensor_msgs/PointCloud2 messages, ptcloudMsg1 and ptcloudMsg2. Create a ROS 2 node with two publishers to publish the messages on the topics /ptcloud1 and /ptcloud2. For the publishers, set the quality of service (QoS) property Durability to transientlocal. This ensures that the publishers maintain the messages for any subscribers that join after the messages have already been sent. Publish the messages using the send function.

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)

Read Messages from ROS 2 Network in Simulink and Perform Point Cloud Stitching

Open the model.

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

Ensure that the two Subscribe blocks are connected to the topics, /ptcloud1 and /ptcloud2. The value of the Durability QoS parameter of the Subscribe blocks is Transient local. This ensures compatibility with the QoS settings of the publishers that you configured in the previous section. The messages from the subscribers are fed to the Read Point Cloud blocks to extract the point cloud data, which is then fed to the registerAndStitchPointclouds MATLAB Function block to perform registration and stitching.

Under Simulation tab, select ROS Toolbox > Variable Size Messages. Notice that the Use default limits for this message type is clear. Then, in the Maximum length column, verify that the data property of the sensor_msgs/PointCloud2 message has a value greater than the number of points in the pointcloud (4915200). Run the model.

sim(modelName);

The model performs point cloud registration using the NDT algorithm and estimates the 3-D transformation of the point cloud published on the /ptcloud2 topic, with respect to the point cloud published on the /ptcloud1 topic. Next, it transforms the /ptcloud2 point cloud to the reference coordinate system of the /ptcloud1 point cloud. It then stitches the transformed point cloud with the /pcloud1 point cloud to create the 3-D world scene. It also visualizes the two point clouds along with the world scene.

MATLAB Function for Point Cloud Stitching Using Registration

This model uses the algorithm in the ExampleHelperRegisterAndStitchPointclouds MATLAB Function to perform point cloud stitching using registration, consisting of these steps:

Downsample the point clouds with a 10 cm grid filter to speed up registration and improve accuracy by using the pcdownsample (Computer Vision Toolbox) function.
Align the two downsampled point clouds using pcregisterndt (Computer Vision Toolbox), which uses the NDT algorithm to estimate the 3-D rigid transformation of second pointcloud with respect to the first point cloud.
Transform the second point cloud to the reference frame of the first point cloud using the pctransform (Computer Vision Toolbox) function.
Merge the transformed point cloud and the first point cloud with a 1.5 cm box grid filter by using the pcmerge (Computer Vision Toolbox) function.
Visualize the two point clouds and the reconstructed 3-D world scene.

For more information about registering and stitching point clouds, see 3-D Point Cloud Registration and Stitching (Computer Vision Toolbox). Registration is also a key initial step in point cloud SLAM applications. For an overview of the point cloud SLAM workflow using MATLAB, see Implement Point Cloud SLAM in 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