1.点云滤波算法
1. 前言
在获取点云数据时，由于设备精度、操作者经验、环境因素等带来的影响，点云数据中将不可避免地出现一些噪声点。实际应用中除了这些测量随机误差产生的噪声点之外，由于受到外界干扰如视线遮挡、障碍物等因素的影响，点云数据中往往存在着一些离主体点云较远的离散点，即离群点。
在点云处理流程中滤波处理作为预处理的第一步，往往对后续处理流程影响很大，只有在滤波预处理中将噪声点、离群点、孔洞(孔洞修复)、数据压缩(最小信息损失的海量点云数据压缩处理)等按照后续需求处理，才能够更好地进行配准、特征提取、曲面重建、可视化等后续流程。
PCL 中点云滤波模块提供了很多灵活实用的滤波处理算法，例如双边滤波、高斯滤波、条件滤波、直通滤波、基于随机采样一致性滤波RANSAC等。常见的应用场景如下：
- 点云数据密度不规则需要平滑
- 因遮挡等问题噪声的离群点需要去除
- 数据冗余需要下采样
- 噪声数据需要去除
各种场景对应的滤波方法如图所示：
2. 算法介绍
注：本文所示算法主要用来理解算法处理思路，因涉及到自建库，在您的电脑上可能无法运行。
2.1 半径滤波
半径滤波以某点为中心统计给定半径内点云数量，当数量大于给定值时，则为内点，数量小于给定值则为离群点并移除。此算法运行速度快，依序迭代留下的点一定是最密集的，但是圆的半径和圆内点的数目都需要人工指定。在一定程度上可以用来筛选边缘点。
python代码：第一种为调用pcl库，第二种为根据半径滤波流程实现。第一种优点是速度快。第二种优点是参数设置自由，可以保留强度信息(因为可以返回离群值下标)，而pcl和open3d都无法实现这一点，缺点是效率略低，当radius_search设置过大时效率很低。
在构建kdtree时有两种方法：pcl或者open3d，需要注意的是pcl的半径查找并未适配python，而open3d的knn和半径都可使用，因此若用knn，选pcl，若用radius，选open3d。
def radius_filter(self, radius_search, min_neighbors): outrem = self.cloud.make_RadiusOutlierRemoval() outrem.set_radius_search(radius_search) outrem.set_MinNeighborsInRadius(min_neighbors) cloud_filtered = outrem.filter() return cloud_filtered def radius_filter(self, radius_search, min_neighbors): cloud = points_to_o3d(self.points[:, :3]) kdtree = o3d.geometry.KDTreeFlann(cloud) ln = self.points.shape[0] noise = [] for i in range(ln): [_, inds, _] = kdtree.search_radius_vector_3d(cloud.points[i], radius_search) if np.asarray(inds).shape[0] < min_neighbors: noise.append(i) cloud_filtered = np.delete(self.points, noise, axis=0) return cloud_filtered
在程序中往往需要借助ndarray作为open3d与pcl之间转换的媒介，程序如下：
def points_to_o3d(points): cloud_o3d = o3d.pybind.geometry.PointCloud() cloud_o3d.points = o3d.utility.Vector3dVector(points[:, :3]) return cloud_o3d def points_to_cloud(points): if points.shape[1] == 3: cloud = pcl.PointCloud() elif points.shape[1] == 4: cloud = pcl.PointCloud_PointXYZI() cloud.from_array(np.asarray(points, dtype='float32')) return cloud
2.2 统计滤波
统计滤波对每一点的邻域进行统计分析，基于点到所有邻近点的距离分布特征，过滤掉一些不满足要求的离群点。特点：主要是根据密度去除离群点，对密度差异较大的离群点去除效果较好。
实现步骤：
第一次迭代：
- 查找每一个点的k个邻域点
- 计算每个点到其邻域的距离dij，其中i = [1,...,m]表示共m个点，j = [1,...,k]每个点有k个邻域
- 根据高斯分布d~N(µ,σ)模型化距离参数，计算所有点与邻居的µ（距离的均值），σ（距离的标准差），
- 为每一个点，计算其邻域的距离均值
第二次迭代：
- 遍历所有点，如果其距离的均值大于高斯分布的指定置信度，将其标记为离群点并移除，如
python代码：仅支持XYZ
def statistical_filter(self, mean_k, mul_thresh): fil = self.cloud.make_statistical_outlier_filter() fil.set_mean_k(mean_k) fil.set_std_dev_mul_thresh(mul_thresh) cloud_filtered = fil.filter() return cloud_filtered
2.3 高斯滤波
采用加权平均方式的一种非线性滤波器，在指定域内的权重是根据欧式距离的高斯分布，通过权重加权平均的方式得到当前点的滤波后的点。
特点：利用标准差去噪，适用于呈正态分布的数据，平滑效果较好，但是边缘角点也会被较大的平滑。
2.4 双边滤波
通过取邻近采样点的加权平均来修正当前采样点的位置，在高斯滤波器只考虑空间域点的位置基础上，增加了维度上的权重。
特点：既有效地对空间三维模型表面进行降噪，又可以保持点云数据中的几何特征信息，避免三维点云数据被过渡光滑。一定程度上 弥补了高斯滤波的缺点，但是只适用于有序点云。
2.5 频率滤波
2.5.1 算法简介
频率滤波目的是在去除点云低频信息。低频信息（例如建筑物墙面，地面）往往会对分割产生干扰，高频信息（例如建筑物窗框，路面障碍锥）往往尺度上很小，直接采用基于临近信息的滤波器会将此类信息合并至墙面或路面中。所以DoN算法利用了多尺度空间的思想。在图像处理中，高低频的概念指不同方向上图像灰度的变化，在点云处理中，定义点云法线向量差为点云所表达的信号。但对于复杂地形点云来说，没法分割近邻挨着的物体。
算法流程：
(1). 在小尺度上计算点云法线n1
(2). 在大尺度上计算点云法线n2
(3). (法线n1-法线n2)/2 -----范数控制在0~1
(4). 滤去3中值较小的点
python 代码：
def don_segment(self, threshold, radius_s, radius_l): cloud_o3d = points_to_o3d(self.points[:, :3]) cloud_o3d.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius_s, self.ln)) normals = np.array(cloud_o3d.normals) cloud_o3d.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamHybrid(radius_l, self.ln)) normall = np.asarray(cloud_o3d.normals) don = (normals - normall) / 2 removed = [] for i in range(self.ln): mod = np.linalg.norm(don[i]) if mod < threshold: removed.append(i) return removed
2.5.2 参数说明
DON算法的核心在于对“小尺度”、“大尺度”、“值较小”的描述。DON算法需要两次求解点云法线(求解方法间2.5.3)，两次求解法线中，分别给一较大半径和一较小半径，最大半径/最小半径的值为10时效果较好。
2.5.3 点云法线估计
点云法线估计中最简单的方法就是求以该点为圆心，给定半径内的点构成的曲面在该点的切面，该切面的法线便是该点的法线。这个问题便转化为了最小二乘平面拟合拟合。平面可以由一个点x和其法向量表示n，点到平面的距离为，在最小二乘拟合中di平均值为0。法向量的求解变为求解协方差矩阵C的特征值和特征向量。
求解流程：
遍历P内点p：
- 获得p的邻域(kdtree)
- 计算p的表面法线n
- 检查n的方向是否与视场点一致，不一致则取反。
2.6 降采样算法
均匀采样：对点云数据创建一个三维体素栅格，在每个体素保留一个最接近体素中心的点，代替体素中所有点
体素采样：对点云数据创建一个三维体素栅格，用每个体素重心近似代替体素中的其他点。特点：可以达到向下采样同时不破坏点云本身几何结构的功能，平滑点云间隔。
python代码：
def vox_down_sample(self, x, y, z): filter_vox = self.cloud.make_voxel_grid_filter() # 体素滤波器 filter_vox.set_leaf_size(x, y, z) # 体素的大小，米 cloud_filtered = filter_vox.filter() # 滤波，得到的数据类型为点云 return cloud_filtered
def vox_down_sample_o3d(self, voxel_size): cloud_o3d = self.cloud_to_o3d() cloud_o3d = cloud_o3d.voxel_down_sample(voxel_size) cloud_filtered = pcl.PointCloud() cloud_filtered.from_array(np.asarray(cloud_o3d.points, dtype='float32'))# 滤波，得到的数据类型为点云 return cloud_filtered
PCL与open3d都可实现体素降采样，但PCL的体素降采样算法对体素大小的限制更严格，叶尺寸过小则不降采样(网格总数超过了可容纳的数值上限)，解决办法是先进行直通滤波提取感兴趣区域，然后再进行PCL降采样。若仍希望较大范围的降采样，可以使用open3d。需要注意的是，这两种方法都将导致点云索引重置，导致失去点云强度(pcl可以保留)、帧数、时间戳等所有信息，因此进行处理前需权衡除XYZ外其他信息的重要性。
2.7 指定规则滤波
直通滤波：过滤掉指定维度、值域外的点。
条件滤波：通过设定滤波条件进行滤波，类似于分段函数，判断点云是否在规则的范围中，如果不在则舍弃。
索引提取：提取索引列表所对应的点构成点云。
需要注意的是，这些算法都将导致点云索引重置，导致失去点云强度(pcl可以保留)、帧数、时间戳等所有信息，因此进行处理前需权衡除XYZ外其他信息的重要性。
直通滤波python代码：
def passthrough_filter(self): passthrough = self.cloud.make_passthrough_filter() passthrough.set_filter_field_name("x") passthrough.set_filter_limits(0.0, 0.1) cloud_filtered = passthrough.filter() return cloud_filtered
多直通滤波叠加：相对于使用条件滤波来说可以保留强度信息
def passthrough_filter(self, xl, xh, yl, yh, zl, zh): cloud_filtered = points_to_cloud(self.points) if xh - xl: cloud_filtered = self._pass_axis_filter(cloud_filtered, 'x', xl, xh) if yh - yl: cloud_filtered = self._pass_axis_filter(cloud_filtered, 'y', yl, yh) if zh - zl: cloud_filtered = self._pass_axis_filter(cloud_filtered, 'z', zl, zh) return cloud_filtered.to_array() def _pass_axis_filter(self, cloud, axis, l, h): passthrough = cloud.make_passthrough_filter() passthrough.set_filter_field_name(axis) passthrough.set_filter_limits(l, h) cloud_filtered = passthrough.filter() return cloud_filtered
条件滤波python代码：仅支持XYZ，无法对含强度点云的过滤
def conditional_filter(self): range_cond = self.cloud.make_ConditionAnd() range_cond.add_Comparison2('z', pcl.CythonCompareOp_Type.GT, 0.0) range_cond.add_Comparison2('z', pcl.CythonCompareOp_Type.LT, 0.8) condrem = cloud.make_ConditionalRemoval(range_cond) condrem.set_KeepOrganized(True) cloud_filtered = condrem.filter() return cloud_filtered