Improve CPD Performance Using FPFH Descriptors

This example shows how to improve the speed of coherent point drift (CPD) registration by using fast point feature histograms (FPFH) descriptors. You can use this method for estimating non-rigid, rigid, and affine transformations.

Overview

This example demonstrates this registration process:

1. Extract and match FPFH features for point cloud pairs.

2. Perform registration, using points derived from feature matching, for non-rigid, rigid, and affine scenarios.

3. Compare execution time and root mean square error (RMSE) between FPFH-enhanced CPD and the CPD baseline.

Non-Rigid Point Cloud Registration

This section demonstrates the registration pipeline for non-rigid transformation.

Load point cloud data into the workspace, and visualize it.

% Load point clouds, and extract moving and fixed point clouds
handData = load('hand3d.mat');
handMoving = handData.moving;
handFixed = handData.fixed;

% Visualize
pcshowpair(handMoving,handFixed,MarkerSize=35);
xlabel("X")
ylabel("Y")
zlabel("Z")
title("Point Clouds Before Registration (Non-Rigid)");
legend({['Moving point cloud:', num2str(handMoving.Count), 'x3'], ...
    ['Fixed point cloud:',num2str(handFixed.Count),'x3']},TextColor='w',Location='southeast');

Extract the FPFH descriptors, and use them to identify corresponding pairs in the moving and fixed point clouds.

% Use tic and toc to measure the elapsed time to compare with baseline
% registration
tic

% Extract the FPFH features from the point clouds
movingFeatures = extractFPFHFeatures(handMoving);
fixedFeatures = extractFPFHFeatures(handFixed);

% Find the matching feature pairs using the exhaustive method. Choose an appropriate
% rejection ratio and matching threshold to minimize the number of matched
% pairs while retaining the structure of the point clouds
[matchingPairs,~] = pcmatchfeatures(fixedFeatures,movingFeatures, ...
    handFixed,handMoving,Method="Exhaustive",RejectRatio=0.7,MatchThreshold=0.1);

% Select the matched points
handFixedFPFH = select(handFixed,matchingPairs(:,1));
handMovingFPFH = select(handMoving,matchingPairs(:,2));

Register the matched points using the CPD algorithm.

% Apply the CPD registration to selected point pairs
tformFPFH = pcregistercpd(handMovingFPFH,handFixedFPFH,Transform="NonRigid");

The output of the registration is a displacement field matrix for the selected points. To apply this to all the points in the moving point cloud, find the nearest feature point for each point and assign the corresponding feature point transformation to the point. Then, calculate the RMSE for the registration and visualize the registered point clouds.

% Find the nearest feature point for every point in moving point cloud
movingPts = handMoving.Location;
matchMoving2Feature = zeros(handMoving.Count,1);

for i = 1:handMoving.Count
    [indices,dists] = findNearestNeighbors(handMovingFPFH,movingPts(i,:),1);
    matchMoving2Feature(i) = indices;
end

% Assign transformation based on the nearest feature points
tformNonRigid = zeros(handMoving.Count,3);

for n = 1:handMoving.Count
    tformNonRigid(n,:) = tformFPFH(matchMoving2Feature(n),:);
end

% Apply transformation and calculate RMSE
handMovingReg = pctransform(handMoving,tformNonRigid);

rmseNonrigidFPFH = cast( ...
        vision.internal.pc.rmse(removeInvalidPoints(handMovingReg),handFixed), ...
        'like',handMovingReg.Location);

% Record execution time
tNonrigidFPFH = toc;

% Visualize point clouds after registration
pcshowpair(handMovingReg,handFixed,MarkerSize=35);
xlabel("X")
ylabel("Y")
zlabel("Z")
title({"Point Clouds After Registration (Non-Rigid)", "RMSE" + num2str(rmseNonrigidFPFH),"Runtime: "+ num2str(tNonrigidFPFH)})
legend({['Moving point cloud:',num2str(handMovingReg.Count),'x3'], ...
    ['Fixed point cloud:',num2str(handFixed.Count),'x3']},TextColor='w',Location='southeast');

Rigid Point Cloud Registration

This section demonstrates the registration pipeline for rigid transformation. Load a point cloud into the workspace, and then use a rigid transformation to create a moving point cloud. Visualize the unregistered point clouds.

% Load point cloud
teapotFixed = pcread("teapot.ply");

% Apply a rigid transformation
rotationAngle = [0 0 55];
translation = [5 0 0];
tformManualRigid = rigidtform3d(rotationAngle,translation);

teapotMovingRigid = pctransform(teapotFixed,tformManualRigid);

% Visualize
pcshowpair(teapotMovingRigid,teapotFixed,MarkerSize=5);
xlabel("X")
ylabel("Y")
zlabel("Z")
title("Point Clouds Before Registration (Rigid)")
legend({['Moving point cloud:',num2str(teapotMovingRigid.Count),'x3'], ...
    ['Fixed point cloud:',num2str(teapotFixed.Count),'x3']}, TextColor='w',Location='southeast');

Extract the FPFH descriptors, and use them to identify corresponding pairs in the moving and fixed point clouds. Register matched points using the CPD algorithm, and apply the transformation to all the points.

tic 

% Find matching point pairs
[teapotFixedFPFH,teapotMovingRigidFPFH] = helperFPFHFeatureMatch(teapotFixed,teapotMovingRigid);

% Register
tformRigid = pcregistercpd(teapotMovingRigidFPFH,teapotFixedFPFH,Transform="rigid");

% Apply the rigid transformation to moving point cloud and calculate rmse
teapotMovingTformed = pctransform(teapotMovingRigid,tformRigid);

rmseRigidFPFH = cast(...
            vision.internal.pc.rmse(removeInvalidPoints(teapotMovingTformed),teapotFixed), ...
            'like',teapotMovingTformed.Location);

tRigidFPFH = toc;

% Visualize point clouds after registration
pcshowpair(teapotMovingTformed,teapotFixed,MarkerSize=5);
xlabel("X")
ylabel("Y")
zlabel("Z")
title({"Point Clouds After Registration (Rigid)","RMSE" + num2str(rmseRigidFPFH),"Runtime:" + num2str(tRigidFPFH)+"sec"})
legend({['Moving point cloud: ',num2str(teapotMovingRigid.Count),'x3'], ...
    ['Fixed point cloud:', num2str(teapotFixed.Count), 'x3']}, TextColor='w',Location='southeast');

Affine Point Cloud Registration

This section demonstrates the registration pipeline for affine transformation. Using the same fixed point cloud as for the rigid transformation, create a moving point cloud by applying an affine transformation. Then, visualize the unregistered point clouds.

% Apply affine transformation to create moving point cloud
[sx,sy,sz] = deal(2,2,2.5);
[tx,ty,tz] = deal(5,5,0);
A = [sx 0 0 tx; 0 sy 0 ty; 0 0 sz tz; 0 0 0 1];
tformManualAffine = affinetform3d(A);

teapotMovingAffine = pctransform(teapotFixed,tformManualAffine);

% Visualize 
pcshowpair(teapotMovingAffine, teapotFixed, MarkerSize=5)
xlabel("X")
ylabel("Y")
zlabel("Z")
title("Point Clouds Before Registration (Affine)")
legend({['Moving point cloud:',num2str(teapotMovingAffine.Count),'x3'], ...
    ['Fixed point cloud:',num2str(teapotFixed.Count),'x3']},TextColor='w',Location='southeast');

Extract the FPFH descriptors, and use them to identify corresponding pairs in the moving and fixed point clouds. Register matched points using the CPD algorithm, and apply the transformation to all the points.

tic

% Find matching point pairs
[teapotFixedFPFH,teapotMovingAffineFPFH] = helperFPFHFeatureMatch(teapotFixed,teapotMovingAffine);

% Register
[tformAffine,~,~] = pcregistercpd(teapotMovingAffineFPFH,teapotFixedFPFH,Transform="Affine");

% Apply affine transformation to moving point cloud and calculate RMSE
movingRegTeapot = pctransform(teapotMovingAffineFPFH,tformAffine);

rmseAffineFPFH = cast( ...
            vision.internal.pc.rmse(removeInvalidPoints(movingRegTeapot),teapotFixed), ...
            'like', movingRegTeapot.Location);

tAffineFPFH = toc;

% Visualize point clouds after registration
pcshowpair(movingRegTeapot,teapotFixed,MarkerSize=5);
xlabel("X")
ylabel("Y")
zlabel("Z")
title({"Point Clouds After Registration (Affine)","RMSE "+ num2str(rmseAffineFPFH),"Runtime:" + num2str(tAffineFPFH)+"sec"})
legend({['Moving point cloud:', num2str(teapotMovingAffine.Count),'x3'], ...
    ['Fixed point cloud:',num2str(teapotFixed.Count),'x3']},TextColor='w',Location='southeast');

Performance Analysis

Run the CPD algorithm to register the moving and fixed point clouds without FPFH descriptors, and get the baseline values for execution time and RMSE.

[tNonrigidBaseline,rmseNonrigidBaseline] = helperCPDBaseline(handMoving,handFixed,"Nonrigid");
[tRigidBaseline,rmseRigidBaseline] = helperCPDBaseline(teapotMovingRigid, teapotFixed,"Rigid");
[tAffineBaseline,rmseAffineBaseline] = helperCPDBaseline(teapotMovingAffine,teapotFixed,"Affine");

Compare the execution times of the baseline CPD and FPFH-enhanced CPD registrations.

tBaseline = [tNonrigidBaseline,tRigidBaseline,tAffineBaseline];
tFPFH = [tNonrigidFPFH,tRigidFPFH,tAffineFPFH];
t = [tBaseline; tFPFH];

rmseBaseline = [rmseNonrigidBaseline,rmseRigidBaseline,rmseAffineBaseline];
rmseFPFH = [rmseNonrigidFPFH,rmseRigidFPFH,rmseAffineFPFH];
rmse = [rmseBaseline; rmseFPFH];

helperFigurePlot(t,rmse);

References

[1] Myronenko, Andriy and Xubo Song. “Point Set Registration: Coherent Point Drift.” IEEE Transactions on Pattern Analysis and Machine Intelligence 32, no. 12 (December 2010): 2262–75. https://doi.org/10.1109/TPAMI.2010.46.

[2] Rusu, Radu Bogdan, Nico Blodow, and Michael Beetz. “Fast Point Feature Histograms (FPFH) for 3D Registration.” In 2009 IEEE International Conference on Robotics and Automation, 3212–17, 2009. https://doi.org/10.1109/ROBOT.2009.5152473.usu, Radu Bogdan, Nico Blodow, and Michael Beetz. "Fast point feature histograms (FPFH) for 3D registration." In 2009 IEEE International Conference on Robotics and Automation, pp. 3212-3217. IEEE, 2009.