Read ROS 2 Point Cloud Messages in Simulink and Perform Stitching Using Registration
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
ptcloudMsg2. Create a ROS 2 node with two publishers to publish the messages on the topics
/ptcloud2. For the publishers, set the quality of service (QoS) property
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
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,
/ptcloud2. The value of the Durability QoS parameter of the Subscribe blocks is
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.
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
10cm 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