[MATLAB point cloud processing] calculate and visualize FPFH

FPFH: Fast Point Feature Histogram
Reference link: 1. Talking about the realization principle of FPFH algorithm and its application in point cloud registration
2. The Fast Point Feature Histogram (FPFH) feature
is used in 3D point cloud registration (Point cloud registration) At the time, the common practice is to do rough matching based on features and RANSAC first, and then do a round of fine matching. A feature commonly used in rough matching is the FPFH feature, while fine matching generally uses the ICP (Iterative closest point) method. This article mainly records the FPFH features:

MATLAB related functions

1. extractFPFHFeatures() function

features = extractFPFHFeatures(ptCloudIn)
提取输入点云对象中每个有效点的FPFH描述符。该函数以N X33矩阵的形式返回描述符，其中N是输入点云中的有效点的数量。
features = extractFPFHFeatures(ptCloudIn,indices) 
提取给定索引处的有效点的FPFH描述符
[___,validIndices] = extractFPFHFeatures(___)
返回已提取FPFH描述符的点云中有效点的索引
[___] = extractFPFHFeatures(___,Name,Value)
除了前面语法中的参数组合外，还使用一个或多个名称-值对参数指定选项。描述符的提取可以采用KNN搜索法、半径搜索法或两者的结合。extractFPFHFeatures函数默认使用KNN搜索方法提取描述符。用户可以通过名称-值对参数选择提取方法。例如，NumNeighbors’，8选择KNN搜索方法提取描述符，并将k-nearest neighbor (KNN)搜索方法中需要考虑的最大邻居数设置为8。

Name-value correspondence parameter
NumNeighbors : The number of neighbors for the KNN search method, specified as the comma-separated pair consisting of“NumNeighbors”and a positive integer. The KNN search method is to calculate the distance between a certain point in the point cloud and its adjacent points, and sort them in ascending order. The closest points are considered neighbors. NumNeighbors sets an upper bound on the number of neighbors to consider. Default values ​​are: 50
Radius: The radius considered by the radius search method, specified asRadiusthe comma-separated pair consisting of and a positive real-valued scalar. The radius search method sets a specific radius around a point and selects all adjacent points within the given radius as neighbors. Default: 0.05
Note :
If you specify values ​​for the "NumNeighbors" and "Radius" name-value pair parameters, the extractFPFHFeatures function will perform a KNN search method and then select only sets within the given radius. If you specify larger values ​​for "NumNeighbors" and "Radius", memory usage and computation time will increase.

program:

clc;clear all;close all;
%将点云数据加载到工作区。
ptObj = pcread('E:\程序库\点云\规则点云\bunny.pcd');
%对点云数据进行降采样。
ptCloudIn = pcdownsample(ptObj,'gridAverage',0.005);%平均网格下采样，网格滤波器将点云空间分成小块，并且把每个方块中的点进行平均，生成新的数据。
%提取指定关键索引点的FPFH描述符。
indic=[323,2311];
features=extractFPFHFeatures(ptCloudIn,indic);

%显示点云上的关键点
keypcshow=pointCloud(ptCloudIn.Location(indic,:),'Color',[0,0,1;0,1,1]);%先定义点的坐标及颜色属性
figure
pcshow(ptObj)%点云可视化
title('Selected Indices on Point Cloud')
hold on
pcshow(keypcshow,'MarkerSize',1000)%关键点可视化
hold off

%关键点提取的FPFH描述符。
figure;title('FPFH Descriptors of Selected Indices');
subplot(211);bar(features(1,:),'Facecolor',[0,0,1]);
subplot(212);bar(features(2,:),'Facecolor',[0,1,1]);

operation result:

2.pcmatchfeatures() function

Reference blog: matlab point cloud coarse registration (2) - find matching point pairs based on FPFH features (partial implementation of Fast Global Registration algorithm)

indexPairs = pcmatchfeatures(features1,features2)
在提取的点云特征的输入矩阵之间找到匹配的特征，并在每个特征矩阵中返回它们的索引。
indexPairs = pcmatchfeatures(features1,features2,ptCloud1,ptCloud2)
剔除基于与特征矩阵对应的点云的空间关系信息的模糊特征匹配
[indexPairs,scores] = pcmatchfeatures(___)
使用以前语法中的任何输入参数组合，返回匹配功能之间的归一化欧氏距离.
[___] = pcmatchfeatures(___,Name,Value) 
除了以前语法中的任何参数组合之外，还指定使用一个或多个百-目应参数的选项。例如“MatchThresHolding”，0.03将匹配特性的归一化距离闻值设置为0.03。

3. The select() function in the point cloud

select(): Select points in the point cloud.

ptCloudOut = select(ptCloud,indices)
返回索引对应的点云对象
ptCloudOut = select(___,'OutputSize',outputSize)
返回点云对象 点云对象的大小由outputSize指定。

4. Transformation of 3D point cloud: pctransform()

ptCloudOut = pctransform(ptCloudIn,tform)
将指定的3-D仿射变换tform应用于点云ptCloudin。变换可以是刚性变换或非刚性变换

program:

clc;clear all;close all;
%将点云数据加载到工作区。
ptObj = pcread('E:\程序库\点云\规则点云\bunny.pcd');
%对点云数据进行降采样。
ptCloudIn = pcdownsample(ptObj,'gridAverage',0.005);%平均网格下采样，网格滤波器将点云空间分成小块，并且把每个方块中的点进行平均，生成新的数据。
%提取指定关键索引点的FPFH描述符。
indic=[323,2311];
features=extractFPFHFeatures(ptCloudIn,indic);

%显示点云上的关键点
keypcshow=pointCloud(ptCloudIn.Location(indic,:),'Color',[0,0,1;0,1,1]);%先定义点的坐标及颜色属性
figure
pcshow(ptObj)%点云可视化
title('Selected Indices on Point Cloud')
hold on
pcshow(keypcshow,'MarkerSize',1000)%关键点可视化
hold off

%关键点提取的FPFH描述符。
figure;
subplot(211);bar(features(1,:),'Facecolor',[0,0,1]);
title('FPFH Descriptors of Selected Indices');
subplot(212);bar(features(2,:),'Facecolor',[0,1,1]);


%使用转换矩阵A转换并创建一个新的点云。
theta = pi/4;
rot = [cos(theta) sin(theta) 0; ...
      -sin(theta) cos(theta) 0; ...
               0          0  1];
trans = [0, 0, 0];
tform = rigid3d(rot,trans);   
ptCloudTformed = pctransform(ptCloudIn,tform);
figure;pcshowpair(ptCloudIn,ptCloudTformed);
legend("Original", "Transformed","TextColor",[1 1 0]);
fixedFeature = extractFPFHFeatures(ptCloudIn);
movingFeature = extractFPFHFeatures(ptCloudTformed);
length(movingFeature)
%找到匹配的特征。
[matchingPairs,scores] = pcmatchfeatures(fixedFeature,movingFeature,ptCloudIn,ptCloudTformed);
length(matchingPairs)
%计算匹配的分，值越小表示匹配的越好
mean(scores)

matchedPts1 = select(ptCloudIn,matchingPairs(:,1));%返回matchingPairs(:,1)索引号对应的点云
matchedPts2 = select(ptCloudTformed,matchingPairs(:,2));
figure
pcshowMatchedFeatures(ptCloudIn,ptCloudTformed,matchedPts1,matchedPts2, ...
    "Method","montage")

title("Matched Points")

operation result:

5. estgeotform3d() function: estimate 3D geometric transformation from matching point pairs

tform = estgeotform3d(matchedPoints1,matchedPoints2,transformType)
通过将来自一组三维点matchedPoints1的匹配点中的内变量映射到来自一组三维点matchedPoints2的匹配点中的内变量，估计两组三维点之间的三维几何变换。
[tform,inlierIndex] = estgeotform3d(___)