基于PCL和C++实现偏度平衡滤波（SKF)算法

来源：（硕士论文）冯茂林《树木遮挡下的机载Lidar点云建筑物轮廓提取》

原文：偏度平衡滤波SKF (Skewness balancing filtering）算法假定Lidar点云中自然地面点的高程概率密度分布服从正态分布，非地面点会使得Lidar点云中点的高程概率密度分布偏离正态分布，呈现出偏态分布，偏度表示偏离正态分布的程度。为了使这种分布达到正态分布的状态，需要从Lidar点云数据样本中剔除高程较高的[18,111]干扰点云高程概率密度分布的非地面点，从而“校正”数据的偏度使其达到平衡。每次校正，高程最高的点被标记为非地面点剔除，迭代进行，直至偏度约等于0，剩余的点即为地面点。完成滤波分离地面点和非地面点。

 查阅资料时没有理解点云高程如何计算，所以我这里使用的是点云的Z坐标为文中高程。如果有错误，还望指出。

首先，我们需要理解两个公式：
∑(zi-z)^2 =∑(zi^2)-n·z^2                                        (1)

 ∑(zi-z)^3 =∑(zi^3)+2n·z^3-3z·∑(zi^2)                  (2)

n代表点云总数，zi代表第i点，z表示点云z坐标均值。

实现算法得到效果如下，第一图是原始点云，图二是分离的建筑物点云。

 算法步骤：

首先，对点云进行排序，按照z值的大小进行升序排序。

根据（1）（2）公式分别计算出∑(zi^2) = sum_z2 ,∑(zi^3) =sum_z3 , z =z_mean

∑(zi-z)^2 =sum_dz2 = sum_z2 - N * pow(z_mean, 2)

 ∑(zi-z)^3 =sum_dz3 = sum_z3 + 2 * N * pow(z_mean, 3) - 3 * z_mean * sum_z2

最后计算出sk。

注：in_cloud为输入（输出）点云，out_cloud输出点云。程序结束时，in_cloud为地面点云结果，out_cloud为（非地面点云）建筑物点云结果。

算法可能存在其他问题，效果明显和CloudCompare内置CSF算法各有优缺点。CSF保留更多的低矮建筑物，SKF很多低矮建筑直接被过滤掉了。但是杂点更少，边界也很整齐。都存在未完全分离地面点云的情况。

最后希望有大佬来改进这个算法，这里献上代码:

void SkewnessBalancingFilter(PointCloudT::Ptr in_cloud, PointCloudT::Ptr out_cloud) {
	std::sort(in_cloud->points.begin(), in_cloud->points.end(), [](PointT pa, PointT pb)->bool {
		if (pa.z < pb.z) return true;
		else return false;
		});
	double sum_z = 0.0f;
	int N = in_cloud->size();
	for (PointT p : in_cloud->points)  sum_z += p.z;
	double z_mean = sum_z / N;
	double sum_z3 = 0.0f, sum_z2 = 0.0f;
	double sum_dz3 = 0.0f, sum_dz2 = 0.0f;
	for (PointT p : in_cloud->points) {
		double p2 = p.z * p.z;
		sum_z2 += p2;
		sum_z3 += p2 * p.z;
	}
	double sk = 1;
	while (sk > 0.00001) {
		sum_dz2 = sum_z2 - N * pow(z_mean, 2);
		sum_dz3 = sum_z3 + 2 * N * pow(z_mean, 3) - 3 * z_mean * sum_z2;
		double theta = sqrt(sum_dz2 / (N - 1));
		sk = sum_dz3 / (N * pow(theta, 3));
		PointT end = in_cloud->points[N - 1];
		in_cloud->points.erase(in_cloud->points.begin() + N - 1);
		out_cloud->points.push_back(end);
		N = in_cloud->size();
		sum_z -= end.z;
		z_mean = sum_z / N;
		double p2 = end.z * end.z;
		sum_z2 -= p2;
		sum_z3 -= p2 * end.z;
	}
}