点云滤波算法

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 统计滤波

    统计滤波对每一点的邻域进行统计分析，基于点到所有邻近点的距离分布特征，过滤掉一些不满足要求的离群点。特点：主要是根据密度去除离群点，对密度差异较大的离群点去除效果较好。

实现步骤：

第一次迭代：

1. 查找每一个点的k个邻域点
2. 计算每个点到其邻域的距离dij，其中i = [1,...,m]表示共m个点，j = [1,...,k]每个点有k个邻域
3. 根据高斯分布d~N(µ,σ)模型化距离参数，计算所有点与邻居的µ（距离的均值），σ（距离的标准差），
4. 为每一个点，计算其邻域的距离均值

第二次迭代：

1. 遍历所有点，如果其距离的均值大于高斯分布的指定置信度，将其标记为离群点并移除，如

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：

1. 获得p的邻域(kdtree)
2. 计算p的表面法线n
3. 检查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