feat: complete camera pose & visualization

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])
 
+