138 lines
4.9 KiB
Python
138 lines
4.9 KiB
Python
import numpy as np
|
|
|
|
|
|
def fit_line(p1, p2):
|
|
# fit a line ax+by+c = 0
|
|
if p1[0] == p1[1]:
|
|
return [1., 0., -p1[0]]
|
|
else:
|
|
[k, b] = np.polyfit(p1, p2, deg=1)
|
|
return [k, -1., b]
|
|
|
|
|
|
def line_cross_point(line1, line2):
|
|
# line1 0= ax+by+c, compute the cross point of line1 and line2
|
|
if line1[0] != 0 and line1[0] == line2[0]:
|
|
print('Cross point does not exist')
|
|
return None
|
|
if line1[0] == 0 and line2[0] == 0:
|
|
print('Cross point does not exist')
|
|
return None
|
|
if line1[1] == 0:
|
|
x = -line1[2]
|
|
y = line2[0] * x + line2[2]
|
|
elif line2[1] == 0:
|
|
x = -line2[2]
|
|
y = line1[0] * x + line1[2]
|
|
else:
|
|
k1, _, b1 = line1
|
|
k2, _, b2 = line2
|
|
x = -(b1-b2)/(k1-k2)
|
|
y = k1*x + b1
|
|
return np.array([x, y], dtype=np.float32)
|
|
|
|
|
|
def line_verticle(line, point):
|
|
# get the verticle line from line across point
|
|
if line[1] == 0:
|
|
verticle = [0, -1, point[1]]
|
|
else:
|
|
if line[0] == 0:
|
|
verticle = [1, 0, -point[0]]
|
|
else:
|
|
verticle = [-1./line[0], -1, point[1] - (-1/line[0] * point[0])]
|
|
return verticle
|
|
|
|
|
|
|
|
def rectangle_from_parallelogram(poly):
|
|
'''
|
|
fit a rectangle from a parallelogram
|
|
:param poly:
|
|
:return:
|
|
'''
|
|
p0, p1, p2, p3 = poly
|
|
angle_p0 = np.arccos(np.dot(p1-p0, p3-p0)/(np.linalg.norm(p0-p1) * np.linalg.norm(p3-p0)))
|
|
if angle_p0 < 0.5 * np.pi:
|
|
if np.linalg.norm(p0 - p1) > np.linalg.norm(p0-p3):
|
|
# p0 and p2
|
|
## p0
|
|
p2p3 = fit_line([p2[0], p3[0]], [p2[1], p3[1]])
|
|
p2p3_verticle = line_verticle(p2p3, p0)
|
|
|
|
new_p3 = line_cross_point(p2p3, p2p3_verticle)
|
|
## p2
|
|
p0p1 = fit_line([p0[0], p1[0]], [p0[1], p1[1]])
|
|
p0p1_verticle = line_verticle(p0p1, p2)
|
|
|
|
new_p1 = line_cross_point(p0p1, p0p1_verticle)
|
|
return np.array([p0, new_p1, p2, new_p3], dtype=np.float32)
|
|
else:
|
|
p1p2 = fit_line([p1[0], p2[0]], [p1[1], p2[1]])
|
|
p1p2_verticle = line_verticle(p1p2, p0)
|
|
|
|
new_p1 = line_cross_point(p1p2, p1p2_verticle)
|
|
p0p3 = fit_line([p0[0], p3[0]], [p0[1], p3[1]])
|
|
p0p3_verticle = line_verticle(p0p3, p2)
|
|
|
|
new_p3 = line_cross_point(p0p3, p0p3_verticle)
|
|
return np.array([p0, new_p1, p2, new_p3], dtype=np.float32)
|
|
else:
|
|
if np.linalg.norm(p0-p1) > np.linalg.norm(p0-p3):
|
|
# p1 and p3
|
|
## p1
|
|
p2p3 = fit_line([p2[0], p3[0]], [p2[1], p3[1]])
|
|
p2p3_verticle = line_verticle(p2p3, p1)
|
|
|
|
new_p2 = line_cross_point(p2p3, p2p3_verticle)
|
|
## p3
|
|
p0p1 = fit_line([p0[0], p1[0]], [p0[1], p1[1]])
|
|
p0p1_verticle = line_verticle(p0p1, p3)
|
|
|
|
new_p0 = line_cross_point(p0p1, p0p1_verticle)
|
|
return np.array([new_p0, p1, new_p2, p3], dtype=np.float32)
|
|
else:
|
|
p0p3 = fit_line([p0[0], p3[0]], [p0[1], p3[1]])
|
|
p0p3_verticle = line_verticle(p0p3, p1)
|
|
|
|
new_p0 = line_cross_point(p0p3, p0p3_verticle)
|
|
p1p2 = fit_line([p1[0], p2[0]], [p1[1], p2[1]])
|
|
p1p2_verticle = line_verticle(p1p2, p3)
|
|
|
|
new_p2 = line_cross_point(p1p2, p1p2_verticle)
|
|
return np.array([new_p0, p1, new_p2, p3], dtype=np.float32)
|
|
|
|
|
|
def sort_rectangle(poly):
|
|
# sort the four coordinates of the polygon, points in poly should be sorted clockwise
|
|
# First find the lowest point
|
|
p_lowest = np.argmax(poly[:, 1])
|
|
if np.count_nonzero(poly[:, 1] == poly[p_lowest, 1]) == 2:
|
|
# 底边平行于X轴, 那么p0为左上角 - if the bottom line is parallel to x-axis, then p0 must be the upper-left corner
|
|
p0_index = np.argmin(np.sum(poly, axis=1))
|
|
p1_index = (p0_index + 1) % 4
|
|
p2_index = (p0_index + 2) % 4
|
|
p3_index = (p0_index + 3) % 4
|
|
return poly[[p0_index, p1_index, p2_index, p3_index]], 0.
|
|
else:
|
|
# 找到最低点右边的点 - find the point that sits right to the lowest point
|
|
p_lowest_right = (p_lowest - 1) % 4
|
|
p_lowest_left = (p_lowest + 1) % 4
|
|
angle = np.arctan(-(poly[p_lowest][1] - poly[p_lowest_right][1])/(poly[p_lowest][0] - poly[p_lowest_right][0]))
|
|
# assert angle > 0
|
|
# if angle <= 0:
|
|
# print(angle, poly[p_lowest], poly[p_lowest_right])
|
|
if angle/np.pi * 180 > 45:
|
|
# 这个点为p2 - this point is p2
|
|
p2_index = p_lowest
|
|
p1_index = (p2_index - 1) % 4
|
|
p0_index = (p2_index - 2) % 4
|
|
p3_index = (p2_index + 1) % 4
|
|
return poly[[p0_index, p1_index, p2_index, p3_index]], -(np.pi/2 - angle)
|
|
else:
|
|
# 这个点为p3 - this point is p3
|
|
p3_index = p_lowest
|
|
p0_index = (p3_index + 1) % 4
|
|
p1_index = (p3_index + 2) % 4
|
|
p2_index = (p3_index + 3) % 4
|
|
return poly[[p0_index, p1_index, p2_index, p3_index]], angle |