PCL提取3D点云模型特征（3.0 FPFH快速点特征直方图）附完整代码

一、概述

上一篇博客解释了PFH是什么以及如何利用PFH来提取点云的特征，那么讲了PFH（PCL提取3D点云模型特征（2.0 PFH点特征直方图 ）附完整代码）之后肯定是要接着说FPFH的。本来想着把FPFH的内容和PFH放在一起的，但是感觉如果放在一起写再加上最后的代码（毕竟每篇博客能加代码的我都会把代码贴出来，毕竟talk is cheap, show me your code嘛），就感觉一篇博客的内容就太多了，不仅写起来累，看起来也累，所以就分开吧。

与PFH方法类似，在目前所有网上看到的资料中，都没有看到如何利用统计出来的FPFH来提取出点云模型的特征点。基于对PFH如何提取特征点的思想，我对FPFH也做了同样的操作，找出FPFH的33维向量中最重要的那一维$bin[i]$，再为$bin[i]$指定一个阈值来判断当前点是否为特征点。

二、FPFH（Fast Point Feature Histograms）

1. 直方图计算

FPFH的全称是Fast Point Feature Histograms，顾名思义，这个方法就是基于PFH做的，而改进的地方就是在于计算效率更加高了。PFH统计的是半径$r$（实际上在实现的过程中，$r$近邻可以是$k$近邻）内所有点完全相互连接得到的点对信息，这样，在计算相邻目标点的PFH时，一定会有部分甚至大部分的邻域会重叠，而两两相互连接的点在计算前一个目标点的PFH时已经计算过了，这样就会造成大量的重复计算，降低了PFH方法的计算效率。

针对这个关键问题，FPFH将原来的邻域点完全互连转变为只由目标点$P_q$与其它邻域点$P_i$之间的连接，但为了尽量减小因为减少点对连接的数量而造成特征信息的丢失，所以FPFH额外包含了一些在目标点半径$2r$内的邻域点，邻域关系如下图所示。

与PFH相同，在每个连接的点对间建立一个相对坐标系，示意图如下：

其中$n$为点的法线。

FPFH的计算步骤如下：

• 第一步，对于点云模型中所有点先以半径$r$邻域（在第一幅图中以红色粗线标出）计算$\alpha ,\varphi$和$\theta$三个特征算子：
  $$\alpha =v\cdot n_t,\varphi =u\cdot \frac{P_t-P_s}{d},\theta =arctan(w\cdot n_t,u\cdot n_t)$$在此基础上统计邻域内所有区间的点数量所占百分比得出简化点特征直方图SPFH。值得注意的是，SPFH的的横坐标，也就是特征向量的维度是33个，与PFH的区间划分方式不同。SPFH将上面每个算子根据值域等分成11个区间，然后将这三个算子划分出来的11个区间直接连接起来这样就形成了33个区间，也就是33个维度，这33个维度值的和为$300\%$，而PFH的125维的和为$100\%$。比如，将$\alpha$这个算子划分了11个区间后分别为${\alpha }_1\&{\alpha }_2\&\cdots \&{\alpha }_{11}$，同样的对$\varphi$和$\theta$这么划分，分别得到${\varphi }_1\&{\varphi }_2\&\cdots {\varphi }_{11}$和${\theta }_1\&{\theta }_2\&\cdots {\theta }_{11}$，那么最终连接得到的33维向量的值分别对应所在的统计区间为$({\alpha }_1,{\alpha }_2,\cdots ,{\alpha }_{11},{\varphi }_1,{\varphi }_2,\cdots ,{\varphi }_{11},{\theta }_1,{\theta }_2,\cdots ,{\theta }_{11})$。
• 第二步，对于每个点都需要另外包含$2r$半径内的其它一些点。对于新包含进来这些点的在第一步计算中得到的SPFH进行加权并与点$P_q$本身的SPFH求和，最后得到目标点$P_q$的FPFH，公式如下：
  $$FPFH(P_q)=SPFH(P_q)+\frac{1}{k}\sum _{i=1}^k\frac{1}{{\omega }_i}\cdot SPFH(p_i)$$ 其中，${\omega }_i$为当前点$P_q$与其近邻点$P_i$间的欧式距离，$k$为$P_q$邻域内的近邻点个数。

根据上述步骤，统计得到的直方图如图所示（以stanford bunny模型为例）：

2. 特征点提取

在上一篇博客中说到，对于PFH是通过观察直方图找到了$bin[62]$这个维度值异常的高，从而确定第63个维度对点云模型的特征检测具有至关重要的作用。如果同样的基于这种思想，对FPFH进行同样的操作，显然是不可行的，因为FPFH并没有对三个算子的区间进行组合统计，而是直接简单连接，这样，在这个三个算子的区间统计中，哪一个$bin$的值是最高的这并不是确定的。

但是，根据PFH找到的第63个$bin$，而PFH共有125个组合区间，那么是否存在这样一个关系：在所有的组合区间中，处于中间的那个维度是最重要的。FPFH中共有33个区间，那么是否同样的处于中间的第17个维度即$bin[16]$是最重要的。带着这样的想法进行了实验，发现结果确实如此。根据这个想法，对点云中所有点进行遍历，判断每个点的FPFH的$bin[16]$是否大于某个阈值$\epsilon$，如果$bin[16]<\epsilon$，则$P_q$为特征点。

根据实验结果可以反向证实，虽然PFH和FPFH设计了这么多特征算子，并把这些算子根据不同规则划分区域进行组合或连接，但实际上，起到最大作用的只有其中的某一个算子的某一个区间（具体最重要的哪个算子可以根据$bin[16]$对应的哪个算子得出）

三、实验结果

利用Stanford Bunny模型进行实验，实验结果如下：

四、完整代码

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

#include <pcl/io/io.h>
#include <pcl/io/ply_io.h>
#include <pcl/features/integral_image_normal.h>  //法线估计类头文件
#include <pcl/visualization/cloud_viewer.h>
//#include <pcl/visualization/histogram_visualizer.h>
#include <pcl/visualization/pcl_plotter.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/point_types.h>
#include <pcl/features/pfh.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/vfh.h>
#include <pcl/features/boundary.h>
#include <pcl/features/integral_image_normal.h>
#include <iostream>
#include <ctime>
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 computeFPFH(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_origin, pcl::PointCloud<pcl::Normal>::Ptr cloud_normals, string filename)
{
	float radius;
	int k_number;
	int pf_num = 0;
	cout << "input radius/k_nubmer of fpfh_search: ";
	//cin >> radius;
	cin >> k_number;
	clock_t start, finish;
	start = clock();
	pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
	fpfh.setInputCloud(cloud_origin);
	fpfh.setInputNormals(cloud_normals);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	fpfh.setSearchMethod(tree);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_fe_ptr(new pcl::PointCloud<pcl::FPFHSignature33>());
	//pfh.setRadiusSearch(radius);
	fpfh.setKSearch(k_number);
	fpfh.compute(*fpfh_fe_ptr);

	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 < fpfh_fe_ptr->points.size(); i++)
	{
		if (fpfh_fe_ptr->points[i].histogram[16] < 50)
		{
			pf_num++;
			cloud_b->at(i).r = 255;
			cloud_b->at(i).g = 0;
			cloud_b->at(i).b = 0;
		}
		else
		{
			cloud_b->at(i).r = 130;
			cloud_b->at(i).g = 130;
			cloud_b->at(i).b = 130;
		}
	}
	finish = clock();
	cout << "total time is: " << finish - start << endl;
	cout << "pf_num is: " << pf_num << endl;
	savefile(cloud_b, cloud_normals, filename, "FPFH");
	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);
	computeFPFH(cloudOrigin, cloud_normals, "bunny.ply");
	return 0;
}