基于三维点云的机器人杆件目标识别与抓取（三）

本片博文将主要介绍几种基于点云数据的基本特征描述方法，包括：点法向量估计、点特征直方图（PFH）、快速点特征直方图（FPFH），接下来将依次对齐原理、实验及相关代码进行展示与介绍。

特征描述

一、点云特征描述
在点云数据分割、目标识别、位姿配准、表面重建等点云数据处理算法中，点云特征描述起到尤为基础和重要的作用。根据特征的空间尺度进行分类，可分为单点、局部及全局特征。针对点云数据的局部和全局特征，目前已有很多成熟稳定的点云特征描述子对点云数据进行特征描述与表达，如图3-1所示。

二、点云法线特征描述




三、点特征直方图描述



四、快速点特征直方图


**

相关代码

**
一、点云法向量估计

#include <vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL);

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/visualization/pcl_visualizer.h>

需要添加此两个语句，否则报错：Error: no override found for 'vtkActor'

int
main(int argc, char** argv)
{
	//加载点云模型
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	// pcl::PCLPointCloud2 cloud_blob;
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("scene_truss_1.pcd", *cloud) == -1) {

		PCL_ERROR("Could not read file \n");
	}
	//* the data should be available in cloud
	  

	// Normal estimation*
	//法向计算
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	//建立kdtree来进行近邻点集搜索
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
	//为kdtree添加点云数据
	tree->setInputCloud(cloud);

	n.setInputCloud(cloud);
	n.setSearchMethod(tree);
	//点云法向计算时，需要搜索的近邻点大小
	n.setKSearch(20);
	//开始进行法向计算
	n.compute(*normals);
	//* normals should not contain the point normals + surface curvatures

	// Concatenate the XYZ and normal fields*
	//将点云数据与法向信息拼接
	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
	pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);

	cout << "信息拼接完成" << endl;

	/*图形显示模块*/
	//显示设置
	
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));


	int v1(0);
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	//设置背景色
	viewer->setBackgroundColor(0, 0, 0.7, v1);

	int v2(0);
	viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	viewer->setBackgroundColor(0, 0, 0, v2);


	//设置点云颜色，该处为单一颜色设置
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);

	//添加需要显示的点云数据
	viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample cloud", v1);
	//设置点显示大小
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
	viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color);

	//添加需要显示的点云法向。cloud为原始点云模型，normal为法向信息，10表示需要显示法向的点云间隔，即每10个点显示一次法向，５表示法向长度。
	viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, normals, 20, 1, "normals", v2);
	viewer->addCoordinateSystem(0.1);   
	//--------------------
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
	// Finish
	return (0);
}

// NormalEstimation.cpp : Defines the entry point for the console application.
//

二、点特征直方图描述（PFH）

/*

计算法线---计算临近点对角度差值-----直方图--

点特征直方图(PFH)描述子

点特征直方图(Point Feature Histograms)

正如点特征表示法所示，表面法线和曲率估计是某个点周围的几何特征基本表示法。

虽然计算非常快速容易，但是无法获得太多信息，因为它们只使用很少的

几个参数值来近似表示一个点的k邻域的几何特征。然而大部分场景中包含许多特征点，

这些特征点有相同的或者非常相近的特征值，因此采用点特征表示法，

其直接结果就减少了全局的特征信息。



http://www.pclcn.org/study/shownews.php?lang=cn&id=101



通过参数化查询点与邻域点之间的空间差异，并形成一个多维直方图对点的k邻域几何属性进行描述。

直方图所在的高维超空间为特征表示提供了一个可度量的信息空间，

对点云对应曲面的6维姿态来说它具有不变性，

并且在不同的采样密度或邻域的噪音等级下具有鲁棒性。



是基于点与其k邻域之间的关系以及它们的估计法线，

简言之，它考虑估计法线方向之间所有的相互作用，

试图捕获最好的样本表面变化情况，以描述样本的几何特征。



Pq 用红色标注并放在圆球的中间位置，半径为r，

(Pq)的所有k邻元素（即与点Pq的距离小于半径r的所有点）

全部互相连接在一个网络中。最终的PFH描述子通过计

算邻域内所有两点之间关系而得到的直方图，

因此存在一个O(nk^2) 的计算复杂性。





每一点对，原有12个参数，6个坐标值，6个坐标姿态（基于法线）

PHF计算没一点对的 相对坐标角度差值三个值和 坐标点之间的欧氏距离 d

从12个参数减少到4个参数



computePairFeatures (const Eigen::Vector4f &p1, const Eigen::Vector4f &n1,

const Eigen::Vector4f &p2, const Eigen::Vector4f &n2,

float &f1, float &f2, float &f3, float &f4);



为查询点创建最终的PFH表示，所有的四元组将会以某种统计的方式放进直方图中，

这个过程首先把每个特征值范围划分为b个子区间，并统计落在每个子区间的点数目，

因为四分之三的特征在上述中为法线之间的角度计量，

在三角化圆上可以将它们的参数值非常容易地归一到相同的区间内。



默认PFH的实现使用5个区间分类（例如：四个特征值中的每个都使用5个区间来统计），

其中不包括距离（在上文中已经解释过了——但是如果有需要的话，

也可以通过用户调用computePairFeatures方法来获得距离值），

这样就组成了一个125浮点数元素的特征向量（35），

其保存在一个pcl::PFHSignature125的点类型中。





*/



#include <pcl/point_types.h>
#include <pcl/features/pfh.h>
#include <pcl/io/pcd_io.h>//点云文件pcd 读写
#include <pcl/features/normal_3d.h>//法线特征
#include <pcl/visualization/pcl_plotter.h>// 直方图的可视化 方法2
#include <chrono>


using namespace std;


int main(int argc, char** argv)

{

	auto start = std::chrono::steady_clock::now();

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	// 读取点云文件　填充点云对象

	pcl::PCDReader reader;

	reader.read("pillar1000.pcd", *cloud_ptr);

	if (cloud_ptr == NULL) { cout << "pcd file read err" << endl; return -1; }

	cout << "PointCLoud size() " << cloud_ptr->width * cloud_ptr->height

		<< " data points ( " << pcl::getFieldsList(*cloud_ptr) << "." << ")" << endl;



	// =====【2】计算法线========创建法线估计类====================================

	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;

	ne.setInputCloud(cloud_ptr);

	/*

	 法线估计类NormalEstimation的实际计算调用程序内部执行以下操作：

	对点云P中的每个点p

	  1.得到p点的最近邻元素

	  2.计算p点的表面法线n

	  3.检查n的方向是否一致指向视点，如果不是则翻转



	 在PCL内估计一点集对应的协方差矩阵，可以使用以下函数调用实现：

	//定义每个表面小块的3x3协方差矩阵的存储对象

	Eigen::Matrix3fcovariance_matrix;

	//定义一个表面小块的质心坐标16-字节对齐存储对象

	Eigen::Vector4fxyz_centroid;

	//估计质心坐标

	compute3DCentroid(cloud,xyz_centroid);

	//计算3x3协方差矩阵

	computeCovarianceMatrix(cloud,xyz_centroid,covariance_matrix);



	*/

	// 添加搜索算法 kdtree search  最近的几个点 估计平面 协方差矩阵PCA分解 求解法线

	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());

	ne.setSearchMethod(tree);//设置近邻搜索算法 

	// 输出点云 带有法线描述

	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ptr(new pcl::PointCloud<pcl::Normal>);

	pcl::PointCloud<pcl::Normal>& cloud_normals = *cloud_normals_ptr;

	// Use all neighbors in a sphere of radius 3cm

	ne.setRadiusSearch(0.03);//半价内搜索临近点 3cm

	// 计算表面法线特征

	ne.compute(cloud_normals);





	//=======【3】创建PFH估计对象pfh，并将输入点云数据集cloud和法线normals传递给它=================

	pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfh;// phf特征估计其器

	pfh.setInputCloud(cloud_ptr);

	pfh.setInputNormals(cloud_normals_ptr);

	//如果点云是类型为PointNormal,则执行pfh.setInputNormals (cloud);

	//创建一个空的kd树表示法，并把它传递给PFH估计对象。

	//基于已给的输入数据集，建立kdtree

	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>());

	//pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree2 (new pcl::KdTreeFLANN<pcl::PointXYZ> ()); //-- older call for PCL 1.5-

	pfh.setSearchMethod(tree2);//设置近邻搜索算法 

	//输出数据集

	pcl::PointCloud<pcl::PFHSignature125>::Ptr pfh_fe_ptr(new pcl::PointCloud<pcl::PFHSignature125>());//phf特征

   //使用半径在5厘米范围内的所有邻元素。

	//注意：此处使用的半径必须要大于估计表面法线时使用的半径!!!

	pfh.setRadiusSearch(0.05);

	//计算pfh特征值

	pfh.compute(*pfh_fe_ptr);




	cout << "phf feature size : " << pfh_fe_ptr->points.size() << endl;

	// 应该与input cloud->points.size ()有相同的大小，即每个点都有一个pfh特征向量

	auto end = std::chrono::steady_clock::now();
	std::chrono::duration<double, std::milli> elapsed = end - start;  // milli:表示ms；  micro：表示微秒
	std::cout << "time： " << elapsed.count() << "ms" << std::endl;



  // ========直方图可视化=============================


	
	 pcl::visualization::PCLPlotter plotter;

	 plotter.addFeatureHistogram(*pfh_fe_ptr, 0); //设置的很坐标长度，该值越大，则显示的越细致
	 plotter.addFeatureHistogram<pcl::PFHSignature125>(*pfh_fe_ptr, "pfh", 100);
	 plotter.plot();
	 plotter.setWindowSize(800, 600);
	 plotter.spinOnce(30000);
	 plotter.clearPlots();




	return 0;

}

三、快速点特征描述（FPFH）

/*

phf点特征直方图 计算复杂度还是太高

计算法线---计算临近点对角度差值-----直方图--

因此存在一个O(nk^2) 的计算复杂性。

k个点之间相互的点对 k×k条连接线



每一点对，原有12个参数，6个坐标值，6个坐标姿态（基于法线）

PHF计算没一点对的 相对坐标角度差值三个值和 坐标点之间的欧氏距离 d

从12个参数减少到4个参数



快速点特征直方图FPFH（Fast Point Feature Histograms）把算法的计算复杂度降低到了O(nk) ，

但是任然保留了PFH大部分的识别特性。

查询点和周围k个点的连线 的4参数特征

也就是1×k=k个线



默认的FPFH实现使用11个统计子区间（例如：四个特征值中的每个都将它的参数区间分割为11个），

特征直方图被分别计算然后合并得出了浮点值的一个33元素的特征向量，

这些保存在一个pcl::FPFHSignature33点类型中。

计算程序运行时间：

*/


#include <chrono>
#include <pcl/point_types.h>
#include <pcl/features/fpfh.h>
#include <pcl/io/pcd_io.h>//点云文件pcd 读写
#include <pcl/features/normal_3d.h>//法线特征
#include <pcl/visualization/histogram_visualizer.h> //直方图的可视化
#include <pcl/visualization/pcl_plotter.h>// 直方图的可视化 方法2





using namespace std;

//using std::cout;

//using std::endl;

int main(int argc, char** argv)

{

	auto start = std::chrono::steady_clock::now(); //启动记时⏲


	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);

	//读取点云文件　填充点云对象

	pcl::PCDReader reader;

	reader.read("pillar1000.pcd", *cloud_ptr);

	if (cloud_ptr == NULL) { cout << "pcd file read err" << endl; return -1; }

	cout << "PointCLoud size() " << cloud_ptr->width * cloud_ptr->height

		<< " data points ( " << pcl::getFieldsList(*cloud_ptr) << "." << endl;



	// 计算法线========创建法线估计类

	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;

	ne.setInputCloud(cloud_ptr);

	/*

	 法线估计类NormalEstimation的实际计算调用程序内部执行以下操作：

	对点云P中的每个点p

	  1.得到p点的最近邻元素

	  2.计算p点的表面法线n

	  3.检查n的方向是否一致指向视点，如果不是则翻转



	 在PCL内估计一点集对应的协方差矩阵，可以使用以下函数调用实现：

	//定义每个表面小块的3x3协方差矩阵的存储对象

	Eigen::Matrix3fcovariance_matrix;

	//定义一个表面小块的质心坐标16-字节对齐存储对象

	Eigen::Vector4fxyz_centroid;

	//估计质心坐标

	compute3DCentroid(cloud,xyz_centroid);

	//计算3x3协方差矩阵

	computeCovarianceMatrix(cloud,xyz_centroid,covariance_matrix);



	*/

	// 添加搜索算法 kdtree search  最近的几个点 估计平面 协方差矩阵PCA分解 求解法线

	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());

	ne.setSearchMethod(tree);//设置近邻搜索算法 

	// 输出点云 带有法线描述

	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ptr(new pcl::PointCloud<pcl::Normal>);

	pcl::PointCloud<pcl::Normal>& cloud_normals = *cloud_normals_ptr;

	// Use all neighbors in a sphere of radius 3cm

	ne.setRadiusSearch(0.03);//半径内搜索临近点 3cm

	// 计算表面法线特征

	ne.compute(cloud_normals);





	//=======【3】创建FPFH估计对象fpfh, 并将输入点云数据集cloud和法线normals传递给它=================

	  //pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfh;// phf特征估计其器

	pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;

	// pcl::FPFHEstimationOMP<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh;//多核加速

	fpfh.setInputCloud(cloud_ptr);

	fpfh.setInputNormals(cloud_normals_ptr);

	//如果点云是类型为PointNormal,则执行pfh.setInputNormals (cloud);

	//创建一个空的kd树表示法，并把它传递给FPFH估计对象。

	//基于已给的输入数据集，建立kdtree

	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>());

	//pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree2 (new pcl::KdTreeFLANN<pcl::PointXYZ> ()); //-- older call for PCL 1.5-

	fpfh.setSearchMethod(tree2);//设置近邻搜索算法 

	//输出数据集

	//pcl::PointCloud<pcl::PFHSignature125>::Ptr pfh_fe_ptr (new pcl::PointCloud<pcl::PFHSignature125> ());//phf特征

	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_fe_ptr(new pcl::PointCloud<pcl::FPFHSignature33>());//fphf特征

   //使用半径在5厘米范围内的所有邻元素。

	//注意：此处使用的半径必须要大于估计表面法线时使用的半径!!!

	fpfh.setRadiusSearch(0.05);

	//计算fpfh特征值

	fpfh.compute(*fpfh_fe_ptr);





	cout << "fphf feature size : " << fpfh_fe_ptr->points.size() << endl;

	// 应该与input cloud->points.size ()有相同的大小，即每个点都有一个pfh特征向量


 auto end = std::chrono::steady_clock::now(); //终止记时⏲
	 std::chrono::duration<double, std::milli> elapsed = end - start; // std::micro 表示以微秒为时间单位, std::milli 表示以毫秒为时间单位。
	 cout << "time: " << elapsed.count() << "ms" << std::endl;


  // ========直方图可视化=============================


	pcl::visualization::PCLPlotter plotter;

	 plotter.addFeatureHistogram(*fpfh_fe_ptr, 100); //设置的很坐标长度，该值越大，则显示的越细致

	 plotter.plot();
	 plotter.setWindowSize(800, 600);
	 plotter.spinOnce(30000);
	 plotter.clearPlots();

	


	
return 0;

}