-
Notifications
You must be signed in to change notification settings - Fork 16
/
utils.py
122 lines (79 loc) · 2.62 KB
/
utils.py
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
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
import numpy as np
import open3d as o3d
def ReadPlyPoint(fname):
""" read point from ply
Args:
fname (str): path to ply file
Returns:
[ndarray]: N x 3 point clouds
"""
pcd = o3d.io.read_point_cloud(fname)
return PCDToNumpy(pcd)
def NumpyToPCD(xyz):
""" convert numpy ndarray to open3D point cloud
Args:
xyz (ndarray):
Returns:
[open3d.geometry.PointCloud]:
"""
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(xyz)
return pcd
def PCDToNumpy(pcd):
""" convert open3D point cloud to numpy ndarray
Args:
pcd (open3d.geometry.PointCloud):
Returns:
[ndarray]:
"""
return np.asarray(pcd.points)
def RemoveNan(points):
""" remove nan value of point clouds
Args:
points (ndarray): N x 3 point clouds
Returns:
[ndarray]: N x 3 point clouds
"""
return points[~np.isnan(points[:, 0])]
def RemoveNoiseStatistical(pc, nb_neighbors=20, std_ratio=2.0):
""" remove point clouds noise using statitical noise removal method
Args:
pc (ndarray): N x 3 point clouds
nb_neighbors (int, optional): Defaults to 20.
std_ratio (float, optional): Defaults to 2.0.
Returns:
[ndarray]: N x 3 point clouds
"""
pcd = NumpyToPCD(pc)
cl, ind = pcd.remove_statistical_outlier(
nb_neighbors=nb_neighbors, std_ratio=std_ratio)
return PCDToNumpy(cl)
def DownSample(pts, voxel_size=0.003):
""" down sample the point clouds
Args:
pts (ndarray): N x 3 input point clouds
voxel_size (float, optional): voxel size. Defaults to 0.003.
Returns:
[ndarray]:
"""
p = NumpyToPCD(pts).voxel_down_sample(voxel_size=voxel_size)
return PCDToNumpy(p)
def PlaneRegression(points, threshold=0.01, init_n=3, iter=1000):
""" plane regression using ransac
Args:
points (ndarray): N x3 point clouds
threshold (float, optional): distance threshold. Defaults to 0.003.
init_n (int, optional): Number of initial points to be considered inliers in each iteration
iter (int, optional): number of iteration. Defaults to 1000.
Returns:
[ndarray, List]: 4 x 1 plane equation weights, List of plane point index
"""
pcd = NumpyToPCD(points)
w, index = pcd.segment_plane(
threshold, init_n, iter)
return w, index
def DrawResult(points, colors):
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)
pcd.colors = o3d.utility.Vector3dVector(colors)
o3d.visualization.draw_geometries([pcd])