点云粗配准算法 python 点云配准代码matlab

FPFH：快速点特征直方图
参考链接：1.浅谈FPFH算法实现原理及其在点云配准中的应用 2.Fast Point Feature Histogram (FPFH)特征 在做三维点云匹配（Point cloud registration）的时候，通用的做法是先做基于特征及RANSAC的粗匹配，然后再做一轮精匹配。粗匹配常使用的一种特征是FPFH特征，而精匹配一般使用的是ICP（Iterative closest point）的方法。本文主要是记录FPFH特征：

MATLAB相关函数

1.extractFPFHFeatures（）函数

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。

名称-值对应参数
NumNeighbors: KNN搜索方法的邻居数量，指定为逗号分隔对，由“NumNeighbors”和一个正整数组成。KNN搜索方法是计算点云中某一点与其相邻点之间的距离，并对其进行升序排序。最近的点被认为是邻居。NumNeighbors设置要考虑的邻居数量的上限。默认值为: 50
Radius: 半径搜索方法考虑的半径，指定为由Radius和一个正实值标量组成的逗号分隔对。半径搜索方法设置一个特定的半径围绕一个点，并选择在给定半径内的所有相邻点作为邻居。默认值为: 0.05
注意:
如果为“NumNeighbors”和“Radius”名称-值对参数指定值，extractFPFHFeatures函数将执行KNN搜索方法，然后只选择给定半径内的集合。如果为“NumNeighbors”和“Radius”指定较大的值，则内存占用和计算时间会增加。

程序：

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]);

运行结果：

2.pcmatchfeatures() 函数

参考博客：matlab 点云粗配准(2)—— 根据FPFH特征查找匹配点对(Fast Global Registration算法的部分实现)

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

3.点云中的select()函数

select()：选择点云中的点。

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

4.三维点云的变换 ： pctransform（）

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

程序：

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")

运行结果：

5.estgeotform3d（）函数：从匹配点对估计三维几何变换

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