PCL_FPFH特徵提取及直方圖顯示

#include <iostream>
#include <pcl/io/pcd_io.h> 
#include <pcl/point_types.h> 
#include <pcl/visualization/cloud_viewer.h> 
#include <pcl/visualization/histogram_visualizer.h>
#include <pcl/point_types.h>
#include <pcl/features/fpfh.h>
#include <pcl/filters/fast_bilateral.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_plotter.h>
#include <vtkAutoInit.h>  

#define vtkRenderingCore_AUTOINIT 3(vtkInteractionStyle,vtkRenderingFreeType,vtkRenderingOpenGL2) 
VTK_MODULE_INIT(vtkRenderingOpenGL);

int main()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("test_pcd.pcd", *cloud) == -1) //* 讀入PCD格式的文件，如果文件不存在，返回-1  
	{
		PCL_ERROR("Couldn't read file test_pcd.pcd \n"); //文件不存在時，返回錯誤，終止程序。  
		return (-1);
	}

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_fillter(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PassThrough<pcl::PointXYZ> pass;
	pass.setInputCloud(cloud);
	pass.setFilterFieldName("z");
	pass.setFilterLimits(0.0, 1.0);
	pass.filter(*cloud_fillter);

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filltered(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::VoxelGrid<pcl::PointXYZ> sor;
	sor.setInputCloud(cloud_fillter);
	sor.setLeafSize(0.01f, 0.01f, 0.01f);
	sor.filter(*cloud_filltered);

	/*pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_outerfillter(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
	outrem.setInputCloud(cloud_filltered);
	outrem.setRadiusSearch(0.8);
	outrem.setMinNeighborsInRadius(5);
	outrem.filter(*cloud_outerfillter);*/

	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>());
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
	ne.setInputCloud(cloud_filltered);
	ne.setSearchSurface(cloud);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	ne.setSearchMethod(tree);
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
	ne.setRadiusSearch(0.03);
	ne.compute(*cloud_normals);

	pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
	fpfh.setInputCloud(cloud_filltered);
	fpfh.setInputNormals(cloud_normals);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_1(new pcl::search::KdTree<pcl::PointXYZ>);
	fpfh.setSearchMethod(tree_1);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());
	fpfh.setRadiusSearch(0.05);
	fpfh.compute(*fpfhs);


	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));

	pcl::visualization::PCLPlotter plotter;
	plotter.addFeatureHistogram<pcl::FPFHSignature33>(*fpfhs,"fpfh",100);

	int v1(0);
	viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer->setBackgroundColor(0, 0, 0, v1);
	viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud1", v1);


	int v2(0);
	viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);	
	viewer->addPointCloud<pcl::PointXYZ>(cloud_filltered, "sample cloud2", v2);
	viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);
	viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_filltered, cloud_normals, 10, 0.2, "normals", v2);


	viewer->initCameraParameters();


	while (!viewer->wasStopped())
	{
		plotter.plot();
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}


	return 0;
}

關於這部分的原理可以看http://pointclouds.org/documentation/tutorials/fpfh_estimation.php#fpfh-estimation，這個PCL官網解釋的還是很清楚的，其實我們只要知道這個特徵的目的就好了，它是3D特徵描述符的一種，所以它就類似於二維圖像中的sift特徵一樣，可以用來做點雲的識別，後期的VFH特徵其實也是基於這個的。