1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
| import open3d as o3d
import os
import numpy as np
from pyntcloud import PyntCloud
import random
def voxel_filter(point_cloud, leaf_size, method="rand"):
"""
# 功能：对点云进行voxel滤波
# 输入：
# point_cloud：输入点云
# leaf_size: voxel尺寸
:param point_cloud:
:param leaf_size:
:return:
"""
filtered_points = []
xyz_max = np.max(point_cloud, axis=0)
xyz_min = np.min(point_cloud, axis=0)
# 计算格子的维度
D_xyz = np.floor((xyz_max - xyz_min) / leaf_size) + 1
# 计算每一个点的序号，x方向，y方向和z方向
h_xyz = (point_cloud - xyz_min) // leaf_size
# 计算每一个点应该在第几个格子，格子的序号从x轴的开始，满了就往y轴方向填，再往z轴方向填
# idx_grid = [12345, 21, 0, 1...] 这代表第一个点应该再标号为12345的格子上
idx_grid = h_xyz['x'] + h_xyz['y'] * D_xyz['x'] + h_xyz['z'] * D_xyz['x'] * D_xyz['y']
# 按照格子的标号对原来的点进行排序，可以从中随机选取一个作为格子的点
idx_grid = idx_grid.to_numpy()
idx_i_grid = [(i, idx) for i, idx in enumerate(idx_grid)]
idx_i_grid.sort(key=lambda x: x[1])
# 下面这个排序之后大约就是[0,0,3,3,10,10,10...]，代表第一个和第二个点都在第一个格子里，
# 第三个和第四个点都在第三个格子里，
sort_point = point_cloud.to_numpy()[[i for i, j in idx_i_grid]]
# 再每一个格子里面，随机选择一个点
begin, end = 0, 0
for end in range(1, len(idx_grid)):
if idx_i_grid[end][1] != idx_i_grid[begin][1]:
if method == "rand":
## random
candid = get_point_random(sort_point, begin, end)
elif method == "centroid":
## centroid
candid = get_point_centroid(sort_point, begin, end)
filtered_points.append(candid)
begin = end
# 把点云格式改成array，并对外返回
filtered_points = np.array(filtered_points, dtype=np.float64)
return filtered_points
def get_point_random(data, begin, end):
idx = np.random.choice([i for i in range(begin, end + 1)])
return data[idx]
def get_point_centroid(data, begin, end):
sorted_list = data[begin: end + 1]
return np.mean(sorted_list, axis=0)
def main():
cat_index = 2 # 物体编号，范围是0-39，即对应数据集中40个物体
root_dir = '/home/kong/下载/shenlan/3D数据集/modelnet40_normal_resampled' # 数据集路径
cat = os.listdir(root_dir)
filename = os.path.join(root_dir, cat[cat_index], cat[cat_index] + '_0001.txt') # 默认使用第一个点云
# 加载原始点云
point_cloud_pynt = PyntCloud.from_file(filename,
sep=",",
names=["x", "y", "z", "nx", "ny", "nz"])
# 从点云中获取点，只对点进行处理
points = point_cloud_pynt.points.iloc[:, :3] # get x,y,z cols
print('total points number is:', points.shape[0])
point_cloud_pynt = PyntCloud(points)
# 转成open3d能识别的格式
point_cloud_o3d = point_cloud_pynt.to_instance("open3d", mesh=False)
o3d.visualization.draw_geometries([point_cloud_o3d]) # 显示原始点云
# 调用voxel滤波函数，实现滤波
filtered_cloud = voxel_filter(point_cloud_pynt.points, 0.05, method="centroid")
point_cloud_o3d.points = o3d.utility.Vector3dVector(filtered_cloud)
print("after filter: ", filtered_cloud.shape[0])
# 显示滤波后的点云
o3d.visualization.draw_geometries([point_cloud_o3d])
if __name__ == '__main__':
main()
|
原始点云
下采样点云
方格求平均取点下采样