From 6180c34a426efea626fac92c79907dc1bbb07896 Mon Sep 17 00:00:00 2001 From: Ting-Jun Wang Date: Sun, 17 Dec 2023 15:46:22 +0800 Subject: [PATCH] feat: complete camera pose & visualization --- test.py | 135 ++++++++++++++++++++++++++++++++++++++++++++------------ 1 file changed, 107 insertions(+), 28 deletions(-) diff --git a/test.py b/test.py index 6981e44..b073b8b 100644 --- a/test.py +++ b/test.py @@ -4,7 +4,6 @@ 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'] @@ -82,11 +81,109 @@ def plot_corner_points(frame, approx): cv2.circle(frame, (x, y), 30, (255, 0, 0), -1) # 在角點位置畫紅色圓圈 +def visualization(inverse_matrix_M): + box_size = 0.3 + + # initialize visualizer + vis = o3d.visualization.Visualizer() + vis.create_window() + + # 只畫紙張 + paper_points = np.array(PAPER_WCS_POINT) + paper_lines = np.array([[0, 1], [0, 2], [1, 3], [2, 3]]) + paper_line_colors = np.array([[255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0]]) + paper_triangles = np.array([[0, 1, 2], [1, 2, 3], [2, 1, 0], [3, 2, 1]]) + # 畫紙張所有線段 + paper_line_set = o3d.geometry.LineSet() + paper_line_set.points = o3d.utility.Vector3dVector(paper_points) + paper_line_set.lines = o3d.utility.Vector2iVector(paper_lines) + paper_line_set.colors = o3d.utility.Vector3dVector(paper_line_colors) + vis.add_geometry(paper_line_set) + + coordinate_line_set = o3d.geometry.LineSet() + coordinate_line_set.points = o3d.utility.Vector3dVector(np.array([[0, 0, 0],[10, 0, 0], [0, 10, 0], [0, 0, 10]])) + coordinate_line_set.lines = o3d.utility.Vector2iVector(np.array([[0, 1], [0, 2], [0, 3]])) + coordinate_line_set.colors = o3d.utility.Vector3dVector(np.array([[0, 255, 0], [0, 255, 0],[ 0, 255, 0]])) + vis.add_geometry(coordinate_line_set) + + # 化紙張三角形 + paper_triangle_set = o3d.geometry.TriangleMesh() + paper_triangle_set.vertices = o3d.utility.Vector3dVector(paper_points) + paper_triangle_set.triangles = o3d.utility.Vector3iVector(paper_triangles) + vis.add_geometry(paper_triangle_set) + + + # 下面是畫相機 + # 找 金字塔端點 + points = [] + lines = [] + line_colors = [] + triangles = [] + for m in inverse_matrix_M: # 不同位置的 M + start_index = len(points) + for corner in [ + [0, 0, 0, 1], + [-box_size, -box_size, 1, 1], + [-box_size, box_size, 1, 1], + [box_size, box_size, 1, 1], + [box_size, -box_size, 1, 1], + ]: + location = m @ np.array(corner) + location[2] = -location[2] + points.append(location) + + # 方框的線 + lines.append(np.array([ start_index+1, start_index+2 ])) + lines.append(np.array([ start_index+2, start_index+3 ])) + lines.append(np.array([ start_index+3, start_index+4 ])) + lines.append(np.array([ start_index+4, start_index+1 ])) + + # 相機點到方框四點的線 + lines.append(np.array([ start_index, start_index+1 ])) + lines.append(np.array([ start_index, start_index+2 ])) + lines.append(np.array([ start_index, start_index+3 ])) + lines.append(np.array([ start_index, start_index+4 ])) + + # 八條線都是黑色 + for i in range(8): + line_colors.append(np.array([0, 0, 0])) + + # trajectory 紅線 + if start_index != 0: + lines.append(np.array([ start_index, start_index-5 ])) + line_colors.append(np.array([1, 0, 0])) + + # 兩塊三角形組成方框 + triangles.append([ start_index+1, start_index+2, start_index+3 ]) + triangles.append([ start_index+1, start_index+3, start_index+4 ]) + triangles.append([ start_index+3, start_index+2, start_index+1 ]) + triangles.append([ start_index+4, start_index+3, start_index+1 ]) + + points = np.array(points)[:, :-1] + lines = np.array(lines) + line_colors = np.array(line_colors) + triangles = np.array(triangles) + + # 畫所有線段 + line_set = o3d.geometry.LineSet() + line_set.points = o3d.utility.Vector3dVector(points) + line_set.lines = o3d.utility.Vector2iVector(lines) + line_set.colors = o3d.utility.Vector3dVector(line_colors) + vis.add_geometry(line_set) + + # 化三角形 + triangle_set = o3d.geometry.TriangleMesh() + triangle_set.vertices = o3d.utility.Vector3dVector(points) + triangle_set.triangles = o3d.utility.Vector3iVector(triangles) + vis.add_geometry(triangle_set) + + vis.run() + if __name__ == '__main__': # 設定連接到 Android 手機的相機 - # cap = cv2.VideoCapture(0) # 0 表示第一個相機（通常是後置相機），若是前置相機，可以使用 1 - cap = cv2.VideoCapture('demo1.mp4') + cap = cv2.VideoCapture(0) # 0 表示第一個相機（通常是後置相機），若是前置相機，可以使用 1 + # cap = cv2.VideoCapture('demo1.mp4') previous_approx = [] @@ -168,34 +265,16 @@ 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]]) - - + # 畫 camera pose + M_inv = [] 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])) # 相機朝向向量 + rt = np.concatenate((R, t), axis=1) + M = np.concatenate((rt, [[0, 0, 0, 1]]), axis=0) + M_inv.append(np.linalg.inv(M)) -# 繪製相機座標系 - mesh_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.6, origin=[0, 0, 0]) + visualization(M_inv) -# 將相機位置加入到點雲中 - 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]) +