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维向量中最重要的那一维 b i n [ i ] bin[i] bin[i],再为 b i n [ i ] bin[i] bin[i]指定一个阈值来判断当前点是否为特征点。

二、FPFH(Fast Point Feature Histograms)

1. 直方图计算

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

针对这个关键问题,FPFH将原来的邻域点完全互连转变为只由目标点 P q P_q Pq与其它邻域点 P i P_i Pi之间的连接,但为了尽量减小因为减少点对连接的数量而造成特征信息的丢失,所以FPFH额外包含了一些在目标点半径 2 r 2r 2r内的邻域点,邻域关系如下图所示。
在这里插入图片描述
与PFH相同,在每个连接的点对间建立一个相对坐标系,示意图如下:
在这里插入图片描述
其中 n n n为点的法线。

FPFH的计算步骤如下:

  • 第一步,对于点云模型中所有点先以半径 r r r邻域(在第一幅图中以红色粗线标出)计算 α , ϕ \alpha,\phi α,ϕ θ \theta θ三个特征算子:
    α = v ⋅ n t , ϕ = u ⋅ P t − P s d , θ = a r c t a n ( w ⋅ n t , u ⋅ n t ) \alpha=v\cdot n_t,\quad \phi=u\cdot \frac{P_t-P_s}{d},\quad \theta=arctan(w \cdot n_t, u\cdot n_t) α=vnt,ϕ=udPtPs,θ=arctan(wnt,unt)在此基础上统计邻域内所有区间的点数量所占百分比得出简化点特征直方图SPFH。值得注意的是,SPFH的的横坐标,也就是特征向量的维度是33个,与PFH的区间划分方式不同。SPFH将上面每个算子根据值域等分成11个区间,然后将这三个算子划分出来的11个区间直接连接起来这样就形成了33个区间,也就是33个维度,这33个维度值的和为 300 % 300\% 300%,而PFH的125维的和为 100 % 100\% 100%。比如,将 α \alpha α这个算子划分了11个区间后分别为 α 1 & α 2 & ⋯ & α 11 \alpha_1\&\alpha_2\&\cdots \&\alpha_{11} α1&α2&&α11,同样的对 ϕ \phi ϕ θ \theta θ这么划分,分别得到 ϕ 1 & ϕ 2 & ⋯ ϕ 11 \phi_1\&\phi_2\&\cdots\phi_{11} ϕ1&ϕ2&ϕ11 θ 1 & θ 2 & ⋯ θ 11 \theta_1\&\theta_2\&\cdots\theta_{11} θ1&θ2&θ11,那么最终连接得到的33维向量的值分别对应所在的统计区间为 ( α 1 , α 2 , ⋯   , α 11 , ϕ 1 , ϕ 2 , ⋯   , ϕ 11 , θ 1 , θ 2 , ⋯   , θ 11 ) (\alpha_1,\alpha_2,\cdots,\alpha_{11},\phi_1,\phi_2,\cdots,\phi_{11},\theta_1,\theta_2,\cdots,\theta_{11}) (α1,α2,,α11,ϕ1,ϕ2,,ϕ11,θ1,θ2,,θ11)
  • 第二步,对于每个点都需要另外包含 2 r 2r 2r半径内的其它一些点。对于新包含进来这些点的在第一步计算中得到的SPFH进行加权并与点 P q P_q Pq本身的SPFH求和,最后得到目标点 P q P_q Pq的FPFH,公式如下:
    F P F H ( P q ) = S P F H ( P q ) + 1 k ∑ i = 1 k 1 ω i ⋅ S P F H ( p i ) FPFH(P_q)=SPFH(P_q)+\frac{1}{k}\sum_{i=1}^{k}\frac{1}{\omega_i}\cdot SPFH(p_i) FPFH(Pq)=SPFH(Pq)+k1i=1kωi1SPFH(pi) 其中, ω i \omega_i ωi为当前点 P q P_q Pq与其近邻点 P i P_i Pi间的欧式距离, k k k P q P_q Pq邻域内的近邻点个数。

根据上述步骤,统计得到的直方图如图所示(以stanford bunny模型为例):
在这里插入图片描述

2. 特征点提取

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

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

根据实验结果可以反向证实,虽然PFH和FPFH设计了这么多特征算子,并把这些算子根据不同规则划分区域进行组合或连接,但实际上,起到最大作用的只有其中的某一个算子的某一个区间(具体最重要的哪个算子可以根据 b i n [ 16 ] bin[16] 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;
}
基于PCL(Point Cloud Library)的点云边界提取可以通过以下步骤实现: 首先,需要加载点云数据,可以从文件中加载或者实时采集。 其次,利用PCL中的NormalEstimation类估计点云数据的法向量。法向量是计算边界的重要依据,能够帮助确定点云中的表面变化。 然后,使用PCL中的BoundaryEstimation类来估计点云的边界。该类会根据法向量和点云数据的几何信息来确定点云的边界,生成一个包含边界索引的输出向量。 最后,可以根据边界的索引,从原始点云数据中提取出边界的信息,包括坐标和其他属性。 以下是一个简单的C++代码示例,演示了如何使用PCL进行点云的边界提取: ```cpp #include <pcl/point_cloud.h> #include <pcl/point_types.h> #include <pcl/io/pcd_io.h> #include <pcl/features/normal_3d.h> #include <pcl/features/boundary.h> int main () { // 读取点云数据 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); pcl::io::loadPCDFile ("cloud.pcd", *cloud); // 估计法向量 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne; ne.setInputCloud (cloud); pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ()); ne.setSearchMethod (tree); pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>); ne.setKSearch (20); ne.compute (*normals); // 边界提取 pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> est; pcl::PointCloud<pcl::Boundary> boundaries; est.setInputCloud (cloud); est.setInputNormals (normals); est.setRadiusSearch (0.02); est.setAngleThreshold (M_PI/4); est.setSearchMethod (tree); est.compute (boundaries); // 提取边界 for (size_t i = 0; i < boundaries.points.size (); ++i) { if (boundaries.points[i].boundary_point) std::cout << "边界索引: " << i << " - " << cloud->points[i].x << " " << cloud->points[i].y << " " << cloud->points[i].z << std::endl; } return (0); } ``` 这段代码首先加载了一个点云数据文件"cloud.pcd",然后进行法向量估计和边界提取,最后输出了边界的坐标信息。通过这个代码示例,可以实现基于PCL的点云边界提取功能。
评论 25
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值