feat: some errors exists
This commit is contained in:
parent
5ec9542cfe
commit
eaaa14dfe0
GPG Key ID:
48D331A3D6160354
98
test.py
98
test.py
@ -1,12 +1,12 @@
|
||||
import numpy as np
|
||||
import open3d as o3d
|
||||
import cv2
|
||||
|
||||
# 設定連接到 Android 手機的相機
|
||||
cap = cv2.VideoCapture(0) # 0 表示第一個相機（通常是後置相機），若是前置相機，可以使用 1
|
||||
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1920) # 設定寬度
|
||||
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080) # 設定高度
|
||||
|
||||
object_points = np.array([[0, 0, 0], [26, 0, 0], [0, 18.5, 0], [26, 18.5, 0]], dtype=np.float32)
|
||||
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):
|
||||
@ -65,15 +65,33 @@ def initial_approx(approx):
|
||||
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() # 讀取影片幀
|
||||
|
||||
@ -100,27 +118,35 @@ if __name__ == '__main__':
|
||||
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
|
||||
|
||||
x, y = new_approx[0][0]
|
||||
cv2.circle(frame, (x, y), 15, (0, 0, 255), -1) # 在角點位置畫紅色圓圈
|
||||
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)
|
||||
|
||||
x, y = new_approx[1][0]
|
||||
cv2.circle(frame, (x, y), 30, (0, 0, 255), -1) # 在角點位置畫紅色圓圈
|
||||
print("R:", rotation_matrix)
|
||||
print("t:", translation_vector)
|
||||
print()
|
||||
|
||||
x, y = new_approx[2][0]
|
||||
cv2.circle(frame, (x, y), 15, (255, 0, 0), -1) # 在角點位置畫紅色圓圈
|
||||
|
||||
x, y = new_approx[3][0]
|
||||
cv2.circle(frame, (x, y), 30, (255, 0, 0), -1) # 在角點位置畫紅色圓圈
|
||||
|
||||
cv2.drawContours(frame, [approx], -1, (0, 255, 0), 2) # 繪製輪廓
|
||||
|
||||
# 顯示結果
|
||||
cv2.namedWindow('Paper Detection(edge)', 0)
|
||||
@ -131,9 +157,10 @@ if __name__ == '__main__':
|
||||
cv2.imshow('Paper Detection', frame)
|
||||
|
||||
# 按下 'q' 鍵退出迴圈
|
||||
if cv2.waitKey(33) & 0xFF == ord('q'): # 等待 33ms (1秒 = 1000ms, 1秒顯示幀)
|
||||
key = cv2.waitKey(17) & 0xFF
|
||||
if key == ord('q'): # 等待 33ms (1秒 = 1000ms, 1秒顯示幀)
|
||||
break
|
||||
if cv2.waitKey(33) & 0xFF == ord(' '):
|
||||
if key == ord(' '):
|
||||
print("Clear previous approxes")
|
||||
previous_approx = []
|
||||
|
||||
@ -141,3 +168,34 @@ if __name__ == '__main__':
|
||||
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])
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user