matlab——FPFH特征提取以及匹配点对

一.FPFH特征提取

1 函数功能概述

A 参考文献

[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.

B 主要函数

重载函数1

 features = extractFPFHFeatures(ptCloudIn)

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

• ptCloudIn：输入的点云
  重载函数2

features = extractFPFHFeatures(ptCloudIn,indices)

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

• indices：选定点的索引

重载函数3

features = extractFPFHFeatures(ptCloudIn,row,column)

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

• row：有序点云中选定点的行索引，指定为正整数的向量。行向量和列向量的长度必须相同。数据类型：Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64
• column：有序点云中选定点的列索引，指定为正整数的向量。行向量和列向量的长度必须相同。数据类型同上
  重载函数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：有效点的线性索引，指定为正整数的向量。向量仅包含函数提取特征的那些点的索引。

C 代码实现

%将数据加载到工作区
ptObj = pcread('teapot.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，因此这里展示的是两个点的快速点特征直方图。

参考资料：
在matalab命令行中，输入help extractFPFHFeatures。显示如下：

这里面的数据集（teapot.ply自带了），代码都在文档里，可以直接运行的。

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

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

A 主要函数

重载函数
在matlab命令行窗口输入help pcmatchfeatures后，可以查看有几个重载函数。

输入参数

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

输出参数

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

B 代码实现

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

%将点云数据读入工作区。
ptCld = pcread('teapot.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')

结果展示

注意：这里顺便也想学习一下这个找到的匹配点之间的连线pcshowMatchedFeatures函数（为了更清楚的看打这个连线，在代码预处理的下采样pcdownsample中，第三个参数我设置的比较大）。

第一张是原来两个点云数据集原图，第二张是找到的匹配点对应是在各自点云形成的矩阵中的序列号，第三张就是找到的特征点连线图。