-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathconvert_to_3d_cloud.py
69 lines (60 loc) · 2.54 KB
/
convert_to_3d_cloud.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
# convert_to_3d_cloud.py
import abd
import numpy as np
import scipy.stats
# Convert the 2D Point Cloud to 3D to make a convex hull
# x is fleft right
# y is forward backward
# z is depth
def convert_to_3d(point_cloud):
# print(point_cloud)
three_dimensional_point_cloud = []
for point in point_cloud:
rotation_angle = point[0]
distance = point[1]
# If this math works I am him - Rahul
lamb_radians = abd.lamb * .0174
rotation_angle_rads = rotation_angle * .0174
y = float(distance * np.cos(lamb_radians)
* np.cos(rotation_angle_rads))
# pi / 2 rads = 1.57
x = float(distance * np.cos(lamb_radians)) * \
np.sin(rotation_angle_rads)
z = float(distance * np.sin(lamb_radians))
three_dimensional_point_cloud.append([x, y, z])
# print(three_dimensional_point_cloud)
z_min = find_z_min(three_dimensional_point_cloud)
new_three_dimensional_point_cloud = []
# Get z mean
z_array = []
for point in three_dimensional_point_cloud:
z_array.append(point[2])
z_mean = np.median(z_array)
z_std = scipy.stats.iqr(z_array)
for i in range(len(three_dimensional_point_cloud)):
# Parameter tweak for volume anal
# print(three_dimensional_point_cloud[i][2])
if (three_dimensional_point_cloud[i][2] < z_mean + 2.5 * z_std) and (three_dimensional_point_cloud[i][2] > z_mean - 3 * z_std):
# if (three_dimensional_point_cloud[i][2] > np.percentile(z_array, 25) - 1.5 * z_std ) and (three_dimensional_point_cloud[i][2] < np.percentile(z_array, 75) + 1.5*z_std):
# three_dimensional_point_cloud[i][2] -= z_min
new_three_dimensional_point_cloud.append(
three_dimensional_point_cloud[i])
# print(new_three_dimensional_point_cloud)
# print(z_mean)
# print(z_std)
find_x(new_three_dimensional_point_cloud)
return new_three_dimensional_point_cloud
def find_z_min(three_dimensional_point_cloud):
current_min = 10000
for point in three_dimensional_point_cloud:
if point[2] < current_min:
current_min = point[2]
return current_min
def find_average_x_of_deepest_points(three_dimensional_point_cloud):
sorted_point_cloud = sorted(
three_dimensional_point_cloud, key=lambda point: point[2])
start_index = int(0.75 * len(sorted_point_cloud))
deepest_points = sorted_point_cloud[start_index:]
total_x = sum(point[0] for point in deepest_points)
average_x = total_x / len(deepest_points)
print(average_x)