-
Notifications
You must be signed in to change notification settings - Fork 0
/
pointcloudProcessingExample.py
117 lines (80 loc) · 3.21 KB
/
pointcloudProcessingExample.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
# FreeRiderHMC Team
# Ver.3 0702
# Segment the road just by cutting the z values below threshold instead of RANSAC segmentation
# Visualize Each Clusters
# load binary data
# Add sequent visualization
import sys
import os
import numpy as np
import open3d as o3d
import time
from sklearn.cluster import MeanShift
from sklearn.neighbors import KDTree
import clusteringModule as clu
import planeSegmentation as seg
import loadData
vis = o3d.visualization.Visualizer()
vis.create_window()
# Load binary data
path = './2011_09_26/2011_09_26_drive_0005_sync/velodyne_points/data/'
file_list = loadData.load_data(path)
# get points from all lists
for files in file_list:
data = np.fromfile(path+files, dtype = np.float32)
data = data.reshape(-1,4)
data = data[:,0:3]
# Convert numpy into pointcloud
cloud = o3d.geometry.PointCloud()
cloud.points = o3d.utility.Vector3dVector(data)
# Downsampling pointcloud
cloud_downsample = cloud.voxel_down_sample(voxel_size=0.2)
# Convert pcd to numpy array
cloud_downsample = np.asarray(cloud_downsample.points)
# Crop Pointcloud -20m < x < 20m && -20m < y < 20m && z > -1.80m
cloud_downsample = cloud_downsample[((cloud_downsample[:, 0] <= 20))]
cloud_downsample = cloud_downsample[((cloud_downsample[:, 0] >= -20))]
cloud_downsample = cloud_downsample[((cloud_downsample[:, 1] <= 10))]
cloud_downsample = cloud_downsample[((cloud_downsample[:, 1] >= -10))]
# threshold z value cut the road
cloudoutliers = cloud_downsample[((cloud_downsample[:, 2] >= -1.4))] # -1.56
print(len(cloudoutliers))
# Clustering Pointcloud
# adjust the threshold into Clustering
start = time.time()
tree = KDTree(cloudoutliers)
clusters = clu.euclideanCluster(cloudoutliers, tree, 0.35)
print("number of estimated clusters : ", len(clusters))
print("How much time for Clustering")
print(time.time() - start)
clustersCloud_pcd = o3d.geometry.PointCloud()
# Visualize Clusters
for i in range(len(clusters)):
#print(len(clusters[i]))
clusterCloud = cloudoutliers[clusters[i][:],:]
clusterCloud_pcd = o3d.geometry.PointCloud()
clusterCloud_pcd.points = o3d.utility.Vector3dVector(clusterCloud)
if i%3 == 0:
clusterCloud_pcd.paint_uniform_color([1,0,0])
elif i%3 == 1:
clusterCloud_pcd.paint_uniform_color([0,1,0])
else:
clusterCloud_pcd.paint_uniform_color([0,0,1])
#clustersCloud_pcd.points = np.append(clustersCloud_pcd.points,clusterCloud_pcd.points)
vis.add_geometry(clusterCloud_pcd)
vis.run()
#o3d.visualization.draw_geometries([clusterCloud_pcd])
time.sleep(0.5)
# enter key -> next frame
input("Press Enter to continue...")
vis.clear_geometries()
#clusterCloud.paint_uniform_color([0.1, 0.9, 0.1])
# Visualization
#pcd_processed = o3d.geometry.PointCloud()
#pcd_processed.points = o3d.utility.Vector3dVector(cloudoutliers)
'''
# Load a pcd data
pcd_load = o3d.io.read_point_cloud("./kitti_pcd/pcd0000000000.pcd")
pcd_downsample = pcd_load.voxel_down_sample(voxel_size=0.3)
print(pcd_load)
'''