Main Content

Estimate Transformation Between Two Point Clouds Using Features

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 the extractFPFHFeatures function to extract fast point feature histogram (FPFH) features from the point clouds, you use the pcmatchfeatures function to search for matches in the extracted features. Finally, you use the estgeotform3d function and the matching features to estimate the rigid transformation.


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

Read the point cloud data into the workspace.

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

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

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

Create a rigid transformation matrix with a 30-degree rotation and translation of 5 units in x- and y-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.

axis on
xlim([-50 75])
ylim([-40 80])
legend("Original","Transformed",TextColor=[1 1 0])

Figure contains an axes object. The axes object contains 2 objects of type scatter. These objects represent Original, Transformed.

Feature Extraction and Registration

Extract features from both the point clouds using the extractFPFHFeatures function.

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

Find matching features and display the number of matching pairs.

[matchingPairs,scores] = pcmatchfeatures(fixedFeature,movingFeature, ...
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, ...
    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.

    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 retransform ptCloudTformed back to the initial point cloud.

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

Visualize the two point clouds.

axis on
xlim([-50 50])
ylim([-40 60])
title("Aligned Point Clouds")

Figure contains an axes object. The axes object with title Aligned Point Clouds contains 2 objects of type scatter.