update repository with camera position

README.md

@@ -70,6 +70,8 @@ Dữ liệu hiệu chuẩn camera lưu tại tệp .npz. Dữ liệu này bao g
 ### Tính toán
 `python distance_estimation/camera_position.py`
 - Ược lượng vị trí camera trong bản đồ tạo bởi các marker được xác định trước
-- Phương pháp này tính toán vị trí camera từ vector tVec, không sử dụng ma trận nghịch đảo của ma trận xoay (vốn yêu cầu mặt phẳng camera luôn song song với mặt phẳng của marker).
-- Các script này sẽ hiển thị vị trí tương đối của camera so với `marker_0` đặt ở trung tâm vùng hạ cánh. `x, y, z, roll, pitch, yaw` sẽ được hiển thị trên màn hình.
-- Nhấn phím `q` để kết thúc, thoát video
+- Phương pháp này tính toán vị trí tương đối camera so với marker bằng việc nhân ma trận nghịch đảo của phép xoay với rVec với tVec.
+- Lấy `marker_0` làm gốc, tọa độ các marker khác được cài đặt trong file `default_config.py`
+- In ra màn hình kết quả các giá trị `x, y, z, roll, pitch, yaw` - lần lượt tọa độ camera trong hệ tọa độ marker_0 làm gốc, `roll, pitch, yaw` - góc quay camera so với mặt phẳng chứa marker
+- Đặt  `visualization= True` để hiển thị video với tọa độ camera so với `marker_0`. Nhấn phím `q` để kết thúc, thoát video
+- Nếu `visualization=False`, nhấn `Ctrl + C` để thoát.

default_config.py

@@ -30,8 +30,8 @@ def change_resolution(capture: cv2.VideoCapture, new_height):
 # Thiết lập vị trí các marker trên bản đồ
 default_positions = np.array([
     [0., 0., 0.],   # marker 0
-    [12.4, -7.5, 0.], # marker 1
-    [-12.4, -7.5, 0.],  # marker 2
-    [-9.4, 7.4, 0.],   # marker 3
-    [9.4, 4.4, 0.]   # marker 4
+    [11.4, -6.5, 0.], # marker 1
+    [-11.4, -6.5, 0.],  # marker 2
+    [-8.8, 6.5, 0.],   # marker 3
+    [10.2, 5.5, 0.]   # marker 4
 ])

distance_estimation/camera_position.py

@@ -57,7 +57,8 @@ def rotationMatrixEulerAngles(rotMatrix):
 
 while True:
     ret, frame = cap.read()
-
+    center_y, center_x = frame.shape[0]//2, frame.shape[1]//2
+    cv2.drawMarker(frame, (center_x, center_y), (0,0,255), markerType=cv2.MARKER_CROSS, markerSize=10, thickness=1, line_type=cv2.LINE_AA)
     if not ret:
         break
     gray_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
@@ -81,7 +82,7 @@ while True:
 
             # Chuyển đổi rVec thành ma trận quay R
             R, _ = cv2.Rodrigues(rVec[i])
-        
+
             # Tính toán tọa độ các điểm của marker so với camera
             object_points = np.array([
                 [-MARKER_SIZE / 2, MARKER_SIZE / 2, 0],
@@ -99,10 +100,10 @@ while True:
 
 
             R, _ = cv2.Rodrigues(rVec[i])        # Chuyển rVec sang ma trận quay
-            R_inv = np.linalg.inv(R)            # Lấy nghịch đảo của ma trận quay (tức là R^T nếu R là trực giao)
+            R_inv = R.T           # Lấy nghịch đảo của ma trận quay (tức là R^T nếu R là trực giao)
             position = - R_inv @ tVec[i].reshape(3, 1)  # Tính vị trí camera trong hệ marker
             position = position.reshape(3,)
-            # position += default_positions[ids[0]]
+            position += default_positions[ids[0]]
 
             angles = rotationMatrixEulerAngles(R.T)
             markers[f'marker {ids[0]}'] = np.hstack((position, angles))
@@ -114,26 +115,27 @@ while True:
                     top_right,
                     cv2.FONT_HERSHEY_PLAIN,
                     1.3,
-                    (0, 0, 255),
-                    2,
-                    cv2.LINE_AA,
+                    (0, 0, 255), 2, cv2.LINE_AA
                 )
-            if ids[0] == 3:
-                position += np.array([2.5, 2.5, 0])
-            if ids[0] == 4:
-                position += np.array([-2.5, 12.8, 0.])
-
         # Nếu marker trung tâm (marker 0) xuất hiện thì chỉ cần lấy vị trí so với marker này
-        # if 'marker 0' in markers.keys():
-        #     cam_position = markers['marker 0']
-        # # Nếu không, xét trung bình các marker
-        # else:
-        #     cam_position = np.array([0., 0., 0., 0., 0., 0.])
-        #     for v in markers.values():
-        #         cam_position += v
-        #     cam_position /= len(markers)
+        if 'marker 0' in markers.keys():
+            cam_position = markers['marker 0']
+        # Nếu không, xét trung bình các marker
+        else:
+            cam_position = np.array([0., 0., 0., 0., 0., 0.])
+            for v in markers.values():
+                cam_position += v
+            cam_position /= len(markers)
 
         print(','.join(f'{i:.1f}' for i in position.tolist()))
+        cv2.putText(
+            frame,
+            ','.join(f'{i:.1f}' for i in position.tolist()),
+            (10, 20),
+            cv2.FONT_HERSHEY_PLAIN,
+            1.,
+            (0, 255, 0), 1, cv2.LINE_AA
+        )
     if visualization:
         cv2.imshow("frame", frame)
         key = cv2.waitKey(1)