-
Notifications
You must be signed in to change notification settings - Fork 0
/
utils.py
146 lines (113 loc) · 6.03 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
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
# Not my implementation, but I could not find it online again.
# important functions: MinimumBoundingBox
from scipy.spatial import ConvexHull
from math import sqrt
import numpy as np
from math import atan2, cos, sin, pi
from collections import namedtuple
def unit_vector(pt0, pt1):
# returns an unit vector that points in the direction of pt0 to pt1
dis_0_to_1 = sqrt((pt0[0] - pt1[0])**2 + (pt0[1] - pt1[1])**2)
return (pt1[0] - pt0[0]) / dis_0_to_1, \
(pt1[1] - pt0[1]) / dis_0_to_1
def orthogonal_vector(vector):
# from vector returns a orthogonal/perpendicular vector of equal length
return -1 * vector[1], vector[0]
def bounding_area(index, hull):
unit_vector_p = unit_vector(hull[index], hull[index+1])
unit_vector_o = orthogonal_vector(unit_vector_p)
dis_p = tuple(np.dot(unit_vector_p, pt) for pt in hull)
dis_o = tuple(np.dot(unit_vector_o, pt) for pt in hull)
min_p = min(dis_p)
min_o = min(dis_o)
len_p = max(dis_p) - min_p
len_o = max(dis_o) - min_o
return {'area': len_p * len_o,
'length_parallel': len_p,
'length_orthogonal': len_o,
'rectangle_center': (min_p + len_p / 2, min_o + len_o / 2),
'unit_vector': unit_vector_p,
}
def to_xy_coordinates(unit_vector_angle, point):
# returns converted unit vector coordinates in x, y coordinates
angle_orthogonal = unit_vector_angle + pi / 2
return point[0] * cos(unit_vector_angle) + point[1] * cos(angle_orthogonal), \
point[0] * sin(unit_vector_angle) + point[1] * sin(angle_orthogonal)
def rotate_points(center_of_rotation, angle, points):
# Requires: center_of_rotation to be a 2d vector. ex: (1.56, -23.4)
# angle to be in radians
# points to be a list or tuple of points. ex: ((1.56, -23.4), (1.56, -23.4))
# Effects: rotates a point cloud around the center_of_rotation point by angle
rot_points = []
ang = []
for pt in points:
diff = tuple([pt[d] - center_of_rotation[d] for d in range(2)])
diff_angle = atan2(diff[1], diff[0]) + angle
ang.append(diff_angle)
diff_length = sqrt(sum([d**2 for d in diff]))
rot_points.append((center_of_rotation[0] + diff_length * cos(diff_angle),
center_of_rotation[1] + diff_length * sin(diff_angle)))
return rot_points
def rectangle_corners(rectangle):
# Requires: the output of mon_bounding_rectangle
# Effects: returns the corner locations of the bounding rectangle
corner_points = []
for i1 in (.5, -.5):
for i2 in (i1, -1 * i1):
corner_points.append((rectangle['rectangle_center'][0] + i1 * rectangle['length_parallel'],
rectangle['rectangle_center'][1] + i2 * rectangle['length_orthogonal']))
return rotate_points(rectangle['rectangle_center'], rectangle['unit_vector_angle'], corner_points)
BoundingBox = namedtuple('BoundingBox', ('area',
'length_parallel',
'length_orthogonal',
'rectangle_center',
'unit_vector',
'unit_vector_angle',
'corner_points'
)
)
# use this function to find the listed properties of the minimum bounding box of a point cloud
def MinimumBoundingBox(points):
# Requires: points to be a list or tuple of 2D points. ex: ((5, 2), (3, 4), (6, 8))
# needs to be more than 2 points
# Effects: returns a namedtuple that contains:
# area: area of the rectangle
# length_parallel: length of the side that is parallel to unit_vector
# length_orthogonal: length of the side that is orthogonal to unit_vector
# rectangle_center: coordinates of the rectangle center
# (use rectangle_corners to get the corner points of the rectangle)
# unit_vector: direction of the length_parallel side. RADIANS
# (it's orthogonal vector can be found with the orthogonal_vector function
# unit_vector_angle: angle of the unit vector
# corner_points: set that contains the corners of the rectangle
if len(points) <= 2: raise ValueError('More than two points required.')
hull_ordered = [points[index] for index in ConvexHull(points).vertices]
hull_ordered.append(hull_ordered[0])
hull_ordered = tuple(hull_ordered)
min_rectangle = bounding_area(0, hull_ordered)
for i in range(1, len(hull_ordered)-1):
rectangle = bounding_area(i, hull_ordered)
if rectangle['area'] < min_rectangle['area']:
min_rectangle = rectangle
min_rectangle['unit_vector_angle'] = atan2(min_rectangle['unit_vector'][1], min_rectangle['unit_vector'][0])
min_rectangle['rectangle_center'] = to_xy_coordinates(min_rectangle['unit_vector_angle'], min_rectangle['rectangle_center'])
# this is ugly but a quick hack and is being changed in the speedup branch
return BoundingBox(
area = min_rectangle['area'],
length_parallel = min_rectangle['length_parallel'],
length_orthogonal = min_rectangle['length_orthogonal'],
rectangle_center = min_rectangle['rectangle_center'],
unit_vector = min_rectangle['unit_vector'],
unit_vector_angle = min_rectangle['unit_vector_angle'],
corner_points = set(rectangle_corners(min_rectangle))
)
def estimateMinimumAreaBox(pts):
Box = MinimumBoundingBox(pts[:,:2])
l,w, theta = Box.length_parallel, Box.length_orthogonal, Box.unit_vector_angle
h = np.max(pts[:,2]) - np.min(pts[:,2])
center = Box.rectangle_center
corner_pts = np.array(list(Box.corner_points))
corner_pts = np.insert(corner_pts, 4, corner_pts[0], axis=0)
corner_pts = corner_pts[[0,2,1,3,4]]
box = np.array((center[0], center[1], h / 2 + np.min(pts[:,2]), l, w, h, theta))
return box, corner_pts