Main Content

Estimate Transformation Between Two Point Clouds Using Features

Open Live Script

This example shows how to estimate a rigid transformation between two point clouds. In the example, you use feature extraction and matching to significantly reduce the number of points required for estimation. After you use theextractFPFHFeaturesfunction to extract fast point feature histogram (FPFH) features from the point clouds, you use thepcmatchfeaturesfunction to search for matches in the extracted features. Finally, you use theestgeotform3d功能和匹配特性来估计rigid transformation.

Preprocessing

Create two point clouds by applying rigid transformation to an input point cloud.

Read the point cloud data into the workspace.

rng("default") ptCld = pcread("highwayScene.pcd"); ptCld.Count
ans = 65536

Downsample the point cloud to improve the computation speed, as it contains around 65,000 points.

ptCloud = pcdownsample(ptCld,gridAverage=0.2); ptCloud.Count
ans = 24596

Create a rigid transformation matrix with a 30-degree rotation and translation of 5 units inx- andy-axes.

rotAngle = 30; trans = [5 5 0]; tform = rigidtform3d([0 0 rotAngle],trans);

Transform the input point cloud.

ptCloudTformed = pctransform(ptCloud,tform);

Visualize the two point clouds.

pcshowpair(ptCloud,ptCloudTformed) axisonxlim([-50 75]) ylim([-40 80]) legend("Original","Transformed",TextColor=[1 1 0])

Feature Extraction and Registration

Extract features from both the point clouds using theextractFPFHFeaturesfunction.

fixedFeature = extractFPFHFeatures(ptCloud); movingFeature = extractFPFHFeatures(ptCloudTformed);

发现匹配功能和显示的数量atching pairs.

[matchingPairs,scores] = pcmatchfeatures(fixedFeature,movingFeature,...ptCloud,ptCloudTformed,Method="Exhaustive"); length(matchingPairs)
ans = 1814

Select matching points from the point clouds.

fixedPts = select(ptCloud,matchingPairs(:,1)); matchingPts = select(ptCloudTformed,matchingPairs(:,2));

Estimate the transformation matrix using the matching points.

estimatedTform = estgeotform3d(fixedPts.Location,...matchingPts.Location,"rigid"); disp(estimatedTform.A)
0.8660 -0.5000 -0.0003 4.9995 0.5000 0.8660 0.0000 5.0022 0.0002 -0.0002 1.0000 0.0020 0 0 0 1.0000

Display the defined transformation matrix.

disp(tform.A)
0.8660 -0.5000 0 5.0000 0.5000 0.8660 0 5.0000 0 0 1.0000 0 0 0 0 1.0000

Use the estimated transformation to retransformptCloudTformedback to the initial point cloud.

ptCloudTformed = pctransform(ptCloudTformed,invert(estimatedTform));

Visualize the two point clouds.

pcshowpair(ptCloud,ptCloudTformed) axisonxlim([-50 50]) ylim([-40 60]) title("Aligned Point Clouds")