feat: some errors exists

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):
@@ -68,12 +68,30 @@ def initial_approx(approx):
 
     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]
+                paper_ccs_point = np.concatenate(new_approx, axis=0, dtype=np.float32)
-                cv2.circle(frame, (x, y), 15, (0, 0, 255), -1)  # 在角點位置畫紅色圓圈
 
-                x, y = new_approx[1][0]
+                # 畫邊緣 & 四點
-                cv2.circle(frame, (x, y), 30, (0, 0, 255), -1)  # 在角點位置畫紅色圓圈
+                plot_corner_points(frame, new_approx)
+                cv2.drawContours(frame, [approx[:, :, ::-1]], -1, (0, 255, 0), 2)  # 繪製輪廓
 
-                x, y = new_approx[2][0]
+                # 算 rotaion & translation
-                cv2.circle(frame, (x, y), 15, (255, 0, 0), -1)  # 在角點位置畫紅色圓圈
+                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[3][0]
+                print("R:", rotation_matrix)
-                cv2.circle(frame, (x, y), 30, (255, 0, 0), -1)  # 在角點位置畫紅色圓圈
+                print("t:", translation_vector)
+                print()
 
-                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])
+