PCL提取3D点云模型特征（2.0 PFH点特征直方图 ）附完整代码

一、概述

之前在做本科毕业设计的时候，就用到了 PFH（Point Feature Histogram）和 FPFH（Fast Point Feature Histogram），这两个方法都是Rusu这个大佬提出来的。最开始他提出了PFH作为一个signature来描述点云邻域内的属性，利用这种性质来对点运做registration。但是PFH存在过多的冗余计算，导致效率很低，在此基础上，他又提出了FPFH来提升计算效率。在刚开始接触到这两个特征描述子之前，对这两个方法一无所知，在网上也找了很多资料和帖子，但都看的云里雾里，没办法只能去看Rusu的原始论文，其中PFH的论文为：Persistent Point Feature Histograms for 3D Point Clouds，FPFH的论文为：Fast Point Feature Histograms (FPFH) for 3D Registration。在PFH的原始论文中，提出了4个算子，包括三个角度算子和1一个距离算子，但是在后来的工作中，将PFH和FPFH方法都集成到了PCL中，对PFH的又有所改进，只保留了三个角度算子，并由原来的16维signature向量增加到了125维，具体如何改进见下文。

另外，在目前为止我所有看过的资料中都没有提到如何利用PFH或者FPFH来检测出点云表面的特征点，而我看了原始论文之后好像也并没有了解到具体应该如何根据计算出的直方图检测出特征点（可能并没有完全看明白）。但是在我的摸索之下，还是让我找到了根据特征直方图找出特征点的办法。所以，在本文中提到的特征提取方法可能并不严谨，但个人感觉，提取出的特征点效果基本符合要求。

二、PFH（Point Feature Histograms）

1. 直方图计算

PFH刚开始是只有16维的特征向量，在之后才增加到了125维，因此，在这里我将以16维进行方法的描述，理解了16维的向量之后，125维就可以类比理解了。

由于PFH综合考虑了目标点邻域内每一对点的属性，并划分出了16个特征区间，可以将每一个特征区间（论文中叫bin）看作是目标点的某一维属性，那么这样就一共有了16维度的属性。

我们知道，对于任何事物的描述，都可以从不同角度，不同方面来进行描述，比如描述一个西瓜，它的属性就可以有 $[甜度、水分、颜色、声音]$ 这些属性，那么这就可以利用一个4维向量来描述一个西瓜，所以同样地，我们可以构造一个16维的向量来描述一个点周围邻域点的属性，而这16维的向量直观的画出来，那就是我们所说的PFH了。

PFH可以很好地应对不同的采样密度和近邻的噪声水平，对点云表面的细节特征把握较好。下图显示了PFH中目标点$P_q$的邻域影响图，用红色标记的即为目标点$P_q$，在虚线内即为$P_q$和其在半径$r$内的所有$k$个近邻完全相互连接。

如下图所示：

对每一对点$(P_t,P_s)$计算如下4个特征算子（3个角度算子，1个距离算子）：
$$f_1=<v,n_t>,f_2=||P_t-P_s||,f_3=<u,P_t-P_s>/f_2,f_4=atan(<w,n_t>,<u,n_t>)$$
然后根据这4个算子的值域，分别将其划分为2个等分区间，并将这4个特征算子的等分区间完全组合。在这里$f_1,f_3,f_4$虽然表示的是两个向量的夹角，但在接下来的处理中，我们将其默认为该夹角的余弦值。由于两个向量的夹角范围是$[0°,180°]$所以这3个算子的值域为$[-1,1]$，而$f_2$的值域为$[0,2r]$。如此，不同算子间的区间组合公式为：
$$idx=\sum _{i=1}^{i\le 4}step(s_i,f_i)\cdot 2^{i-1}$$上面这个公式指出了内部具体实现时，16维向量的每一个维度的索引与区间组合的关系，其中 $step(s,f):0iff<sor1$。在实现时，为了将每个特征算子等分成两个区间，对于$f_1,f_3$和$f_4$来说，$s=0$，而对于$f_2$，$s=r$。

可能光看上面的公式对不同算子间的区间组合还是不能完全理解。假设 $f_1$ 算子划分成了$A_1\&B_1$两个区间，同样，其他三个算子则分别划分出了$A_2\&B_2,A_3\&B_3,A_4\&B_4$,那么组合起来就有$[A_1{A}_2{A}_3{A}_4,B_1{A}_2{A}_3{A}_4,A_1{B}_2{A}_3{A}_4,\cdots ,B_1{B}_2{B}_3{B}_4]$一共$2^4=16$种组合了。然后，对每个点的邻域内，统计落入每个组合区间内的点的数量占邻域内所有点数量的百分比，将这个百分比作为每一个维度的属性值，这样，对于点云中每个点都能计算出一个16维的向量或者叫做直方图来描述这个点。

以上是PFH原始论文中的工作，只构造了16维的向量，而改进后，是将上面所提的$f_1,f_2,f_3,f_4$这4个算子保留了$f_1,f_3,f_4$这3个，然后将这3个算子由原来的2等分变成了5等分，这样一来，16维的向量就被提高到了$5^3=125$维的向量了。利用PCL自带的直方图可视化方法对这125维的向量可视化结果如下所示（利用stanford bunny模型举例说明，不同点云的直方图肯定不一样，同一点云中的不同点间的直方图也不一样）：

2. 特征点提取

在论文中提到，可以利用$KL-divergence$来对点云进行配准，我的理解是，也可以利用这个方法来对点云表面的特征点进行提取。$KL-divergence$的计算公式如下：
$$KL-divergence=\sum _{i=1}^{125}(p_i^f-{\mu }_i)\cdot ln\frac{p_i^f}{{\mu }_i}$$其中，$p_i^f$表示第$i$个组合区间的直方图即第$i$维的属性值，${\mu }_i$表示第$i$个组合区间的平均直方图即对点云中所有点的第$i$个组合区间的值求和平均。计算出的$KL-divergence$越大，则目标点为特征点的概率越大。

虽然说是这样说，但我并没有去具体实现这个方法，我觉得这个方法还是比较麻烦，并且最终计算出来的这个$KL-divergence$并不能直接判断目标点是否为特征点，仍然需要人为的指定一个概率阈值。所以我自己找了一个方法。

上面说过，对于任意物体的描述都可以利用一个包含不同属性的向量来描述，例如西瓜的 $[甜度、水分、颜色、声音、大小、\cdots ]$等等这些属性，而在这些属性中，肯定有某一部分甚至某一个属性是最至关重要的，比如 $[甜度、大小]$ 这两个属性对西瓜来说最重要（我认为是这样的，大吗，大就是好瓜）。同样的，对于PFH来说，肯定也存在这样的一个性质，在这16维的属性中，可能只有某几个或某一个组合区间对特征点的识别贡献最大。

基于上面的想法，我绘制了不同模型的PFH，观察这些图我发现，在第63（索引为62）个组合区间内的值都异常的高，

所以我尝试了一下，在计算出了每个点的PFH之后，直接对第63个组合区间的值$bin[62]$指定一个阈值$\epsilon$，当$bin[62]<\epsilon$时，认为目标点为特征点，并标记为红色。实验结果如下：

三、完整代码

配置环境为PCL1.8.0+VS2015，保存的文件为.ply文件，可以利用meshlab软件打开。将需要进行特征检测的.ply点云文件放到工程目录下，输出的特征检测结果也保存在同目录下。

#include 
#include 
#include   //法线估计类头文件
#include 
//#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;


//以.ply文件格式保存点云文件
void savefile(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_result, pcl::PointCloud<pcl::Normal>::Ptr cloud_normals, string filename, string method)
{
	ofstream fout(method + "_" + filename);
	fout << "ply" << endl << "format ascii 1.0" << endl
		<< "element vertex " + std::to_string(cloud_result->size()) << endl
		<< "property float x" << endl << "property float y" << endl << "property float z" << endl
		<< "property float nx" << endl << "property float ny" << endl << "property float nz" << endl
		<< "property uchar red" << endl << "property uchar green" << endl << "property uchar blue" << endl
		<< "end_header" << endl;
	for (int i = 0; i < cloud_result->points.size(); i++)
	{
		fout << cloud_result->points[i].x
			<<" "<< cloud_result->points[i].y
			<< " " << cloud_result->points[i].z
			<< " " << cloud_normals->points[i].normal_x
			<< " " << cloud_normals->points[i].normal_y
			<< " " << cloud_normals->points[i].normal_z
			<<" "<<std::to_string(cloud_result->at(i).r)<<" "<<std::to_string(cloud_result->at(i).g)<<" "<<std::to_string(cloud_result->at(i).b)
			<< endl;
	}
	fout.close();
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr computePFH(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_origin, pcl::PointCloud<pcl::Normal>::Ptr cloud_normals, string filename)
{
	float radius; 
	int k_number;
	cout << "input radius/k_nubmer of pfh_search: ";
	//cin >> radius;
	cin >> k_number;
	pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfh; //声明pfh类对象
	pfh.setInputCloud(cloud_origin); //设置输入点云
	pfh.setInputNormals(cloud_normals); //设置输入点云的法线
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	pfh.setSearchMethod(tree); //设置近邻搜索方式
	pcl::PointCloud<pcl::PFHSignature125>::Ptr pfh_fe_ptr(new pcl::PointCloud<pcl::PFHSignature125>()); //声明一个数据结构，来存储每个点的pfh
	//pfh.setRadiusSearch(radius);
	pfh.setKSearch(k_number); //设置近邻个数
	pfh.compute(*pfh_fe_ptr); //计算每个点的pfh

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_b(new pcl::PointCloud<pcl::PointXYZRGB>);
	cloud_b->resize(cloud_origin->size());
	for (int i = 0; i < cloud_origin->points.size(); i++)
	{
		cloud_b->points[i].x = cloud_origin->points[i].x;
		cloud_b->points[i].y = cloud_origin->points[i].y;
		cloud_b->points[i].z = cloud_origin->points[i].z;
	}

	Eigen::Vector3d d, n1, n2;
	double d_x, d_y, d_z;

	for (int i = 0; i < pfh_fe_ptr->points.size(); i++)
	{
		if (pfh_fe_ptr->points[i].histogram[62] < 50) //设置第63个组合区间的阈值为50，将特征点标记为红色
		{
			cloud_b->at(i).r = 255;
			cloud_b->at(i).g = 0;
			cloud_b->at(i).b = 0;
		}
		else
		{
			cloud_b->at(i).r = 100;
			cloud_b->at(i).g = 100;
			cloud_b->at(i).b = 100;
		}
	}
	savefile(cloud_b, cloud_normals, filename, "PFH"); //将点云以.ply文件格式保存
	return cloud_b;
}

int main()
{
	string filename;
	cout << "input filename: ";
	cin >> filename;
	filename += ".ply";
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOrigin(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PLYReader reader;
	reader.read(filename, *cloudOrigin);
	reader.read(filename, *cloud_normals);
	computePFH(cloudOrigin, cloud_normals, "bunny.ply");
	return 0;
}