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