202 lines
6.9 KiB
Python
202 lines
6.9 KiB
Python
import numpy as np
|
||
import open3d as o3d
|
||
import cv2
|
||
|
||
|
||
PAPER_WCS_POINT = np.array([[0, 0, 0], [18.5, 0, 0], [0, 26, 0], [18.5, 26, 0]], dtype=np.float32)
|
||
# PAPER_WCS_POINT = np.array([[0, 0, 0], [26, 0, 0], [0, 18.5, 0], [26, 18.5, 0]], dtype=np.float32)
|
||
CAMERA_MATRIX = np.load('camera_parameters.npy', allow_pickle=True).item()['K']
|
||
DISTORTION_MATRIX = np.load('camera_parameters.npy', allow_pickle=True).item()['dist']
|
||
|
||
def get_new_approx(approx, previous_approx):
|
||
def distance(point, pre_point):
|
||
point = point[0]
|
||
pre_point = pre_point[0]
|
||
d = ((point[0]-pre_point[0]) ** 2) + ((point[1]-pre_point[1]) ** 2)
|
||
|
||
return np.sqrt(d)
|
||
|
||
new_approx = []
|
||
for pre_point in previous_approx:
|
||
min_d, min_index = 1e9, -100
|
||
for index, point in enumerate(approx):
|
||
d = distance(point, pre_point)
|
||
if d < min_d and d<=500:
|
||
min_d = d
|
||
min_index = index
|
||
|
||
if min_index != -100:
|
||
np.delete(approx, min_index)
|
||
new_approx.append(approx[min_index])
|
||
|
||
if len(new_approx) == 4:
|
||
return new_approx
|
||
else:
|
||
return previous_approx
|
||
|
||
def initial_approx(approx):
|
||
distance = []
|
||
for point in approx:
|
||
x, y = point[0]
|
||
distance.append(x**2+y**2)
|
||
min_d, max_d = 1e9, -1
|
||
min_index, max_index = -100, -100
|
||
for index, d in enumerate(distance):
|
||
if d < min_d:
|
||
min_d = d
|
||
min_index = index
|
||
if d > max_d:
|
||
max_d = d
|
||
max_index = index
|
||
new_approx = []
|
||
|
||
remain_index = list(range(4))
|
||
remain_index.remove(max_index)
|
||
remain_index.remove(min_index)
|
||
|
||
if approx[remain_index[0]][0][0] > approx[remain_index[1]][0][0]: # remain 的第一個點的 x 大於第二個點的，代表他在右上角
|
||
second_index = remain_index[0]
|
||
third_index = remain_index[1]
|
||
else:
|
||
second_index = remain_index[1]
|
||
third_index = remain_index[0]
|
||
|
||
new_approx.append(approx[min_index])
|
||
new_approx.append(approx[second_index])
|
||
new_approx.append(approx[third_index])
|
||
new_approx.append(approx[max_index])
|
||
|
||
return new_approx
|
||
|
||
def plot_corner_points(frame, approx):
|
||
y, x = approx[0][0]
|
||
cv2.circle(frame, (x, y), 15, (0, 0, 255), -1) # 在角點位置畫紅色圓圈
|
||
|
||
y, x = approx[1][0]
|
||
cv2.circle(frame, (x, y), 30, (0, 0, 255), -1) # 在角點位置畫紅色圓圈
|
||
|
||
y, x = approx[2][0]
|
||
cv2.circle(frame, (x, y), 15, (255, 0, 0), -1) # 在角點位置畫紅色圓圈
|
||
|
||
y, x = approx[3][0]
|
||
cv2.circle(frame, (x, y), 30, (255, 0, 0), -1) # 在角點位置畫紅色圓圈
|
||
|
||
|
||
if __name__ == '__main__':
|
||
|
||
# 設定連接到 Android 手機的相機
|
||
# cap = cv2.VideoCapture(0) # 0 表示第一個相機（通常是後置相機），若是前置相機，可以使用 1
|
||
cap = cv2.VideoCapture('demo1.mp4')
|
||
|
||
previous_approx = []
|
||
|
||
rotation_vectors = []
|
||
translation_vectors = []
|
||
while True:
|
||
ret, frame = cap.read() # 讀取影片幀
|
||
|
||
if not ret:
|
||
break
|
||
|
||
# 預處理影像（例如：轉為灰度）
|
||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||
gray = cv2.GaussianBlur(gray, (5, 5), 0)
|
||
|
||
# 偵測邊緣
|
||
edges = cv2.Canny(gray, 30, 90)
|
||
dilate_kernel = np.ones((5, 5), np.uint8)
|
||
dilate_edges = cv2.dilate(edges, dilate_kernel, iterations=1)
|
||
|
||
# 偵測輪廓
|
||
contours, _ = cv2.findContours(dilate_edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||
|
||
if contours != []:
|
||
# 找到最大的輪廓
|
||
max_contour = max(contours, key=cv2.contourArea)
|
||
|
||
# 找到輪廓的近似多邊形
|
||
epsilon = 0.05 * cv2.arcLength(max_contour, True)
|
||
approx = cv2.approxPolyDP(max_contour, epsilon, True)
|
||
|
||
approx = approx[:, :, ::-1]
|
||
|
||
# 繪製多邊形
|
||
if len(approx) == 4: # 確保是四個角點
|
||
|
||
# 比對 previous_approx，確認現在找到的四個點是紙張上的哪一點
|
||
if previous_approx == []:
|
||
print("INITIAL")
|
||
previous_approx = initial_approx(approx)
|
||
new_approx = get_new_approx(approx, previous_approx)
|
||
previous_approx = new_approx
|
||
|
||
paper_ccs_point = np.concatenate(new_approx, axis=0, dtype=np.float32)
|
||
|
||
# 畫邊緣 & 四點
|
||
plot_corner_points(frame, new_approx)
|
||
cv2.drawContours(frame, [approx[:, :, ::-1]], -1, (0, 255, 0), 2) # 繪製輪廓
|
||
|
||
# 算 rotaion & translation
|
||
success, rotation_vector, translation_vector = cv2.solvePnP(PAPER_WCS_POINT, paper_ccs_point, \
|
||
CAMERA_MATRIX, DISTORTION_MATRIX)
|
||
rotation_matrix, _ = cv2.Rodrigues(rotation_vector)
|
||
rotation_vectors.append(rotation_matrix)
|
||
translation_vectors.append(translation_vector)
|
||
|
||
print("R:", rotation_matrix)
|
||
print("t:", translation_vector)
|
||
print()
|
||
|
||
|
||
# 顯示結果
|
||
cv2.namedWindow('Paper Detection(edge)', 0)
|
||
cv2.imshow('Paper Detection(edge)', edges)
|
||
cv2.namedWindow('Paper Detection(dilate edge)', 0)
|
||
cv2.imshow('Paper Detection(dilate edge)', dilate_edges)
|
||
cv2.namedWindow('Paper Detection', 0)
|
||
cv2.imshow('Paper Detection', frame)
|
||
|
||
# 按下 'q' 鍵退出迴圈
|
||
key = cv2.waitKey(17) & 0xFF
|
||
if key == ord('q'): # 等待 33ms (1秒 = 1000ms, 1秒顯示幀)
|
||
break
|
||
if key == ord(' '):
|
||
print("Clear previous approxes")
|
||
previous_approx = []
|
||
|
||
cap.release()
|
||
cv2.destroyAllWindows()
|
||
|
||
|
||
|
||
# 創建 Open3D 點雲
|
||
paper_plane = o3d.geometry.LineSet()
|
||
paper_plane.points = o3d.utility.Vector3dVector(PAPER_WCS_POINT)
|
||
paper_plane.lines = o3d.utility.Vector2iVector([[0, 1], [1, 2], [2, 3], [3, 0]])
|
||
|
||
|
||
for index in range(len(rotation_vectors)):
|
||
# 將相機座標轉換為相機中心和相機朝向向量
|
||
R = rotation_vectors[index]
|
||
t = translation_vectors[index]
|
||
cam_center = -R.T.dot(t) # 相機中心位置
|
||
cam_direction = R.T.dot(np.array([0, 0, 1])) # 相機朝向向量
|
||
|
||
# 繪製相機座標系
|
||
mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.6, origin=[0, 0, 0])
|
||
|
||
# 將相機位置加入到點雲中
|
||
cam_point = o3d.geometry.TriangleMesh.create_sphere(radius=0.05)
|
||
cam_point.paint_uniform_color([1, 0, 0]) # 紅色代表相機位置
|
||
cam_point.translate(cam_center) # 放置在相機中心位置
|
||
|
||
# 創建 Open3D 场景
|
||
scene = o3d.geometry.TriangleMesh()
|
||
scene += paper_plane
|
||
scene += mesh_frame
|
||
scene += cam_point
|
||
|
||
# 顯示 Open3D 场景
|
||
o3d.visualization.draw_geometries([scene])
|
||
|