pcl：点云滤波

1、体素滤波下采样

简介：在建图时，从多个视野扫描的点云会存在重复情况，近似点会占用大量空间，并占用较大时间复杂度，对空间中数据进行下采样，优化时间复杂度。

作用：处理大数据点云时，例如点云批准，花费时间巨大，通过体素滤波选取体素中心点代替目标点云，减少匹配时间，实现更快速的曲面重建。

#include <pcl/filters/voxel_grid.h>

//点云下采样
pcl::PointCloud<pcl::PointXYZ>::Ptr downSample(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double leaf) {
	pcl::VoxelGrid<pcl::PointXYZ> sor;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_new(new pcl::PointCloud<pcl::PointXYZ>);

	sor.setInputCloud(cloud);
	sor.setLeafSize(leaf, leaf, leaf);  //大小为2*2*2
	sor.filter(*cloud_new);
	return cloud_new;
}

 2、统计滤波

        统计滤波器主要用于去除明显离群点。 离群点特征在空间中分布稀疏。定义某处点云小于某个密度，既点云无效。计算每个点到其最近的k个点平均距离。则点云中所有点的距离应构成高斯分布。根据给定均值与方差，可剔除方差之外的点。对于图中那些离点云规模较聚集的区域，简称为离群点，统计滤波对这些离群点的处理比较好。

#include <pcl/filters/statistical_outlier_removal.h>//统计滤波器支持头文件    

// 去除离群点
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_leave_Point(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,int K=50,double eps=2.0) {
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;//创建对象
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_new(new pcl::PointCloud<pcl::PointXYZ>);
	sor.setInputCloud(cloud);  //输入点云
	sor.setMeanK(K); //临近点
	sor.setStddevMulThresh(eps); //离群点阈值
	sor.filter(*cloud_new);
	return cloud_new;
}

首先，遍历点云，计算每个点与其最近的k个邻居点之间的平均距离；其次，计算所有平均距离的均值μ与标准差σ，则距离阈值dmax可表示为dmax=μ＋α×σ，α是一个常数，可称为比例系数，它取决于邻居点的数目；最后，再次遍历点云，剔除与k个邻居点的平均距离大于dmax的点。

看着统计滤波的时间比较长，想要用kdtree实现，没想到它的底层就是kdtree实现，这里有我写的kdtree版本的统计滤波，先放这里，以后再做性能优化，通过手写kdtree实现统计滤波和上面算法时间复杂度差不多大。

//基于kdtree 去除离去点
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_leave_Point_KdTree(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int K = 50, double eps = 2.0) {
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud(cloud);        //设置搜索空间
	vector<float> vis(cloud->size());
	double average = 0;
	for (int i = 0; i < cloud->size(); i++) {
		//int K = 2;
		std::vector<int> ptIdxByKNN(K);      //存储查询点近邻索引
		std::vector<float> ptKNN(K); 		//存储近邻点对应距离平方
		kdtree.nearestKSearch(cloud->points[i], K, ptIdxByKNN, ptKNN); //执行K近邻搜索
		vis[i] = 0;
		for (int j = 0; j < K; j++) {
			vis[i] += ptKNN[j];
		}
		vis[i] = vis[i] / K * 1.0;
		average += vis[i];
	}
	average = average / (1.0*vis.size());
	double dmax = 0;
	double rou = 0;
	for (int i = 0; i < vis.size(); i++) {
		rou = (vis[i] - average)*(vis[i] - average);
	}
	rou = sqrt(rou*1.0 / vis.size());
	dmax = average + eps * rou;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_new(new pcl::PointCloud<pcl::PointXYZ>);
	for (int i = 0; i < cloud->size(); i++) {
		if (vis[i]<dmax) {
			cloud_new->push_back(cloud->points[i]);
		}
	}
	cout <<" 新点云"<< cloud_new->size() << endl;
	return cloud_new;
}