Python点云处理——统计滤波算法

一、算法原理

        与之前的几种滤波算法相比，统计滤波的原理较为抽象和复杂。对三维点云而言，统计滤波和半径滤波一样，以滤除“离群点”为目的。而统计滤波中离群点的定义比半径滤波更为复杂。在统计滤波中，认为点云整体应呈现出较为均匀的分布，即点云内每个点与和它近邻的K个点之间的平均距离不会超过阈值，超过的则视为离群点。而这一阈值由以下公式决定：

        其中，为某个点到所有点距离的平均值，即

        ,

        而表示的是距离的标准差，即

   

        表示的则是自定义的一个标准差系数，用来控制距离标准差对距离阈值的影响。

        因此，进行统计滤波时，需要先计算出所有点之间的平均距离和标准差，得到距离阈值，然后计算每个点与其近邻的K个点的平均距离，如果大于阈值，说明这个点周围的邻近点都比较”分散“，所以视为离群点。

二、代码示例

        下图的代码会生成一片随机点云，然后对其创建KDtree，遍历整个点云，做最近邻搜索，求出每个点的K个近邻点的平均距离，然后计算所有点的平均距离和标准差，滤出符合要求的点，同时用Open3D的内置统计滤波函数，设置一样的参数进行对比，最后在可视化界面和控制台输出中都可以看到两者的效果是一样的。

fig = plt.figure()
ax= fig.add_subplot(131, projection='3d')
ax2= fig.add_subplot(132, projection='3d')
ax3= fig.add_subplot(133, projection='3d')
# -------- Create KDTtree for X1 and X2. --------
point= np.random.rand(100,3)
ratio=0.0001
ax.scatter3D(point[:,0],point[:,1],point[:,2],c='r',marker=".")
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(point)
cl,idx=pcd.remove_statistical_outlier(10,ratio)
point1=point[idx]
ax2.scatter3D(point1[:,0],point1[:,1],point1[:,2],c='g',marker=".")
# tree1 = KDTree(point)
idx1=np.asarray(range(0,len(point)))
idx3=np.empty(1,int)
k_dist = np.zeros(len(point))
tree = sp.spatial.KDTree(point[:,:3])
for i in range(0,len(point)):
    nb=tree.query(point[i,:3],k=10,workers=-1)
    nb_dt = nb[0]
    k_dist[i] = np.sum(nb_dt)
max_distance = np.mean(k_dist) +ratio*np.std(k_dist)
#idx3=np.where(k_dist<max_distance,idx1,-1)
idx3=np.where(k_dist<max_distance)
idx4=np.where(idx3>-1)
idx3=idx3[idx4]
# for i in range(0,len(point)):
#     if k_dist[i]<max_distance:
#         idx3=np.append(idx3,i)
idx3=np.unique(idx3,axis=0)
point3=point[idx3,:]

print(len(point1),len(point3))
ax3.scatter3D(point3[:,0],point3[:,1],point3[:,2],c='b',marker=".")
#
plt.show()

        封装成函数形式的代码： 

def statistical_filter(point,n,std_ratio):
    k_dist = np.zeros(len(point))
    tree = sp.spatial.KDTree(point[:, :3])
    for i in range(0, len(point)):
        nb = tree.query(point[i, :3], k=n, workers=-1)
        nb_dt = nb[0]
        k_dist[i] = np.sum(nb_dt)
    max_distance = np.mean(k_dist) + std_ratio * np.std(k_dist)
    idx=np.where(k_dist<max_distance)
    # idx2=idx[idx1]
    # for i in range(0, len(point)):
    #     if k_dist[i] < max_distance:
    #         idx = np.append(idx,i)
    #numpy的where函数速度远快于遍历
    point = point[idx]
    return point