FPFH特征提取以及匹配（matlab代码免费）

FPFH特征提取时谁提出的，尊重一下原创：

[1] 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, pp. 3212-3217. IEEE, 2009.

地址：
【免费】(2009FPFH)Fast-Point-Feature-Histograms-FPFH-for-3D-registratio资源-CSDN文库https://download.csdn.net/download/Vertira/88910855

在matlab中，已经封装成一个函数即： extractFPFHFeatures(),下面是在matlab中的详细解释

help extractFPFHFeatures
 extractFPFHFeatures - Extract fast point feature histogram (FPFH) descriptors from point cloud
    This MATLAB function extracts FPFH descriptors for each valid point in
    the input point cloud object.

    features = extractFPFHFeatures(ptCloudIn)
    features = extractFPFHFeatures(ptCloudIn,indices)
    features = extractFPFHFeatures(ptCloudIn,row,column)
    [___,validIndices] = extractFPFHFeatures(___)
    [___] = extractFPFHFeatures(___,Name,Value)

    输入参数
        ptCloudIn - Point cloud
            pointCloud object
        indices - Linear indices of selected points
            vector of positive integers
        row - Row indices of selected points
            vector of positive integers
        column - Column indices of selected points
            vector of positive integers

    名称-值参数
        NumNeighbors - Number of neighbors for KNN search
            50 (默认值) | positive integer
        Radius - Radius considered for radius search
            0.05 (默认值) | positive real-valued scalar

    输出参数
        features - FPFH descriptors
            N-by-33 matrix of positive real values
        validIndices - Linear indices of valid points
            vector of positive integers

    打开示例
        Extract FPFH Descriptors at Selected Indices in Point Cloud

    See also pcread, pcdownsample, pcnormals, pcshow, pointCloud

    在 R2020b 中引入
    extractFPFHFeatures 的文档

使用这个函数的方法1：

 features = extractFPFHFeatures(ptCloudIn)

提取输入点云对象中每个有效点的FPFH描述符。该函数以N× 33矩阵的形式返回描述符，其中N是输入点云中的有效点的数量。

方法2：

features = extractFPFHFeatures(ptCloudIn,indices)

提取有效指定indices位置的特征。（这个indices可以是数组）

方法3：

features = extractFPFHFeatures(ptCloudIn,row,column)

在输入的有组织点云ptCloudn的指定的二维索引处提取有效点的FPFH描述符。将点的行索引和列索引分别指定为行和列。

方法4 ；

[___,validIndices] = extractFPFHFeatures(___)

返回已提取FPFH描述符的点云中有效点的索引。

方法5：

 [___] = extractFPFHFeatures(___,Name,Value)

除了前面语法中的参数组合外，还使用一个或多个名称-值对参数指定选项。描述符的提取可以采用KNN搜索法、半径搜索法或两者的结合。extractFPFHFeatures函数默认使用KNN搜索方法提取描述符。用户可以通过名称值对参数选择提取方法。例如，extractFPFHFeatures(___,‘NumNeighbors’,8)，选择KNN搜索方法提取描述符，并将k-nearest neighbor (KNN)搜索方法中需要考虑的最大邻居数设置为8。

输出参数
features：FPFH 描述符，作为正实数值的 N x 33 矩阵返回。N 是函数从中提取 FPFH 描述符的有效点数。每列都包含点云中有效点的 FPFH 描述符。若要另外返回提取点的索引，请使用有效的validIndices输出参数。
validIndices：有效点的线性索引，指定为正整数的向量。向量仅包含函数提取特征的那些点的索引。

代码实现：

%将数据加载到工作区
ptObj = pcread('bunny.ply');
%数据下采样（点太多了，整少点）
ptCloudIn = pcdownsample(ptObj,'gridAverage',0.05);
%提取指定索引出点的FPFH描述符（这里也就是提取的第6565和10000这两个点的FPFH描述符）
keyInds = [6565 10000];
features = extractFPFHFeatures(ptCloudIn,keyInds);
%显示点云上的关键点
ptKeyObj = pointCloud(ptCloudIn.Location(keyInds,:),'Color',[255 0 0;0 0 255]);
figure
pcshow(ptObj)
title('Selected Indices on Point Cloud')
hold on
pcshow(ptKeyObj,'MarkerSize',1000)
hold off
%把上面指定的那两个点的FPFH描述符显示出来
figure
ax1 = subplot(2,1,1);
bar(features(1,:),'FaceColor',[1 0 0])
title('FPFH Descriptors of Selected Indices')
ax2 = subplot(2,1,2);
bar(features(2,:),'FaceColor',[0 0 1])
linkaxes([ax1 ax2],'xy')

运行后的结果

FPFH直方图：（代码中选取的是两个点用来输出FPFH，因此这里展示的是两个点的快速点特征直方图。）

2 根据FPFH特征查找匹配点对

寻找点云之间的匹配特征，算法理论大部分来自论文《Fast global registration》

A 主要函数 pcmatchfeatures

>> help pcmatchfeatures
 pcmatchfeatures - Find matching features between point clouds
    This MATLAB function finds matching features between the input matrices
    of extracted point cloud features and returns their indices within each
    feature matrix.

    indexPairs = pcmatchfeatures(features1,features2)
    indexPairs = pcmatchfeatures(features1,features2,ptCloud1,ptCloud2)
    [indexPairs,scores] = pcmatchfeatures(___)
    [___] = pcmatchfeatures(___,Name,Value)

    输入参数
        features1 - First feature set
            M_{1}-by-N matrix
        features2 - Second feature set
            M_{2}-by-N matrix
        ptCloud1 - First point cloud
            pointCloud object
        ptCloud2 - Second point cloud
            pointCloud object

    名称-值参数
        Method - Matching method
            'Exhaustive' (默认值) | 'Approximate'
        MatchThreshold - Matching threshold
            0.01 (默认值) | scalar in the range (0, 1]
        RejectRatio - Spatial relation threshold
            0.95 (默认值) | scalar in the range (0,1)

    输出参数
        indexPairs - Indices of matched features
            P-by-2 matrix
        scores - Normalized Euclidean distance between matching features
            P-element column vector

    打开示例
        Match Corresponding Features in Point Clouds

    See also pcshowMatchedFeatures, extractFPFHFeatures

    在 R2020b 中引入
    pcmatchfeatures 的文档

输入参数

features1：第一个特征集（就是用FPFH对原点云提取的特征）
features2：第二个特征集（就是用FPFH对被移动的云提取的特征）
ptCloud1：第一个输入点云，指定为pointCloud对象
ptCloud2：第二个输入点云，指定为pointCloud对象
输出参数

indexPairs：匹配特征的索引值（就是找到的特征点是对应原点云的那个点）。返回的是P×2矩阵P是匹配特征的数目。每一行的第一个元素是特征1中的索引值，第二个元素是特征2的索引值。
Scores：归一化欧氏距离特征。返回的一个列向量，所以这个列向量里面的元素代表了匹配特征之间的距离。（所以这个值越小，表示匹配的越好）

代码：

clc;
clear;
%%
%数据载入并预处理

%将点云数据读入工作区。
ptCld = pcread('bunny.ply');
%下采样
ptCloud = pcdownsample(ptCld,'gridAverage',0.4);
%%
%把原点云做一个旋转平移，得到另外一个点云（方法很多。1.这里使用的是仿射矩阵affine3d+pctransform结合。2.也可以直接自己写一个变换矩阵，然后对每一个点遍历分别变换也行 
%3.也可以自己写一个变换矩阵T，然后把xyz数据放在列上全部变成3×n的矩阵，然后在最后一行加一个全1的一行变成4×n的矩阵，然后拿去乘变换矩阵T就行了）

%使用转换矩阵A转换并创建一个新的点云。
A = [cos(pi/6) sin(pi/6) 0 0; ...
    -sin(pi/6) cos(pi/6) 0 0; ...
            0         0  1 0; ...
            10         10 0 1];     
tform = affine3d(A);%仿射变换=线性变换+平移
ptCloudTformed = pctransform(ptCloud,tform);
%%
%可视化两个点云
pcshowpair(ptCloud,ptCloudTformed);

%%
%找两点云相互匹配对应特征。
%1.先使用ExtractFPFHFeature函数，从两个点云中提取FPFH特征
[fixedFeature,fixed_Ind] = extractFPFHFeatures(ptCloud);
[movingFeature,moving_Ind]= extractFPFHFeatures(ptCloudTformed);
%2.使用pcmatchfeatures函数找到两点云FPFH匹配的特征,返回的是点云的点对序列号（以矩阵n×2的形式返回），Score表示
[indexPairs,Score] = pcmatchfeatures(fixedFeature,movingFeature,ptCloud,ptCloudTformed);



%%
%用连接线显示两点云找到的匹配点（用这个函数即可pcshowMatchedFeatures）——这里图想要看的清除，下采样那里把阈值改大一点即可
%1.pcshowMatchedFeatures函数形参的预处理，把输入数据变成pointcloud类格式（形参都是pointcloud格式，但是我不会写pointcloud格式，但是我发现用seclect可以变成这种格式）
movingFeature_Ind = [1:size(movingFeature)]';
fixedFeature_Ind = [1:size(fixedFeature)]';
MovingFeature = select(ptCloud,movingFeature_Ind);%把movingFeature变成pointcloud类形式，才能作为pcshowMatchedFeatures形参的输入
FixedFeature = select(ptCloudTformed,fixedFeature_Ind);

matchedFixedPts = select(ptCloud,indexPairs(:,1));%select实现的是：可以从ptCloud点云中，挑选出为indexPairs里面序列的点，并转成pointcloud的形式。
matchedMovingPts = select(ptCloudTformed,indexPairs(:,2));

%2.用连接线链接匹配点
figure
pcshowMatchedFeatures(MovingFeature,FixedFeature,matchedMovingPts,matchedFixedPts)
% xlim([-40 210])
% ylim([-50 50])
title('Matched Points')

运行结果