StatisticalRemoval统计滤波实现

原理：
对每一点的邻域进行统计分析，基于点到所有邻近点的距离分布特征，过滤掉一些不满足要求的离群点。该算法对整个输入进行两次迭代：在第一次迭代中，它将计算每个点到最近k个近邻点的平均距离，得到的结果符合高斯分布。接下来，计算所有这些距离的平均值 μ 和标准差 σ 以确定距离阈值 thresh_d ，且 thresh_d = μ + k·σ。 k为标准差乘数。在下一次迭代中，如果这些点的平均邻域距离分别低于或高于该阈值，则这些点将被分类为内点或离群点。

实现：

/**
 * @description:	计算向量元素的均值
 * @param v			输入向量
 * @return			向量元素的均值
 */
float distavg(std::vector<float>& v)
{
	float sum = 0.0;
	for (int i = 0; i < v.size(); ++i)
	{
		sum += sqrt(v[i]);
	}
	return sum / v.size();
}

/**
 * @description:	计算向量元素的标准差
 * @param v			输入向量
 * @param avg		向量元素的均值
 * @return			向量元素的标准差
 */
float calcsigma(std::vector<float>& v, float& avg)
{
	float sigma = 0.0;
	for (int i = 0; i < v.size(); ++i)
	{
		sigma += pow(v[i] - avg, 2);
	}
	return sqrt(sigma / v.size());
}

/**
 * @description:			统计学滤波
 * @param cloud				输入点云
 * @param cloud_filtered	滤波点云
 * @param nr_k				k邻近点数
 * @param std_mul			标准差乘数
 * @param negative			设置移除或者保留
 */
void statisticalremoval(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered, 
	int nr_k, double std_mul, bool negative = false)
{
	pcl::KdTreeFLANN<pcl::PointXYZ> tree;
	tree.setInputCloud(cloud);
    std::vector<float> avg;
	for (int i = 0; i < cloud->points.size(); ++i)
	{
		std::vector<int> id(nr_k);
		std::vector<float> dist(nr_k);
		tree.nearestKSearch(cloud->points[i], nr_k, id, dist);
		avg.push_back(distavg(dist));
	}

	float u = accumulate(avg.begin(), avg.end(), 0.0) / avg.size(); 
	float sigma = calcsigma(avg, u);
    std::vector<int> index;
	for (int i = 0; i < cloud->points.size(); ++i)
	{
        std::vector<int> id(nr_k);
        std::vector<float> dist(nr_k);
		tree.nearestKSearch(cloud->points[i], nr_k, id, dist);
		float temp_avg = distavg(dist);
		if (temp_avg >= u - std_mul*sigma && temp_avg <= u + std_mul*sigma)
		{
			index.push_back(i);
		}
	}

	boost::shared_ptr<std::vector<int>> index_ptr = boost::make_shared<std::vector<int>>(index);
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(cloud);
	extract.setIndices(index_ptr);
	extract.setNegative(negative);
	extract.filter(*cloud_filtered);
}

代码传送门：https://github.com/taifyang/PCL-algorithm