Update repository with camera position and fix issue

README.md

@@ -12,18 +12,19 @@ pip install -r requirements.txt
 
 ## Cài đặt các giá trị ban đầu cho marker
 
-Di chuyển tới file [`default_config.py`](default_config.py), thay đổi các giá trị sau
+Di chuyển tới file [`default_config.py`](default_config.py), thay đổi các giá trị sau. Lưu ý kích thước `MARKER_SIZE` và `SQUARE_SIZE` đặt cùng đơn vị (tùy ý centimet, milimet), khi đó kết quả đầu ra về khoảng cách sẽ nhận được giá trị tương ứng
 
 ```python
-MARKER_SIZE=5
-set_resolution='original'
+MARKER_SIZE=5 # kích thước các mã QR-code
+set_resolution='original' # điều chỉnh độ phân giải cho camera, đặt original nếu ko thay đổi
+SQUARE_SIZE = 2.1  # kích thước ô của bàn cờ
 ```
 
 
 
 ## Hiệu chuẩn camera:
 
-Để thực hiện hiệu chuẩn, cần phải chụp ảnh bàn cờ `chessboard.jpg` được in trên giấy với nhiều góc độ khác nhau
+Để thực hiện hiệu chuẩn, cần phải chụp ảnh bàn cờ `chessboard.jpg` được in trên giấy với nhiều góc độ, vị trí khác nhau
 
 `python camera_calibration/capture_images.py`
 
@@ -58,9 +59,17 @@ Dữ liệu hiệu chuẩn camera lưu tại tệp .npz. Dữ liệu này bao g
 - Cài đặt `visualization = True`để in kết quả đầu ra kèm kết quả hình ảnh, nhấn `q` đẻ thoát;
 - Hoặc chạy `visualization = False` để in kết quả đầu ra, nhấn `Ctrl + C`.
 
-## Ước lượng vị trí camera (Hiện không khả dụng):
+## Ước lượng vị trí camera:
+### Chuẩn bị
+- Chuẩn bị marker như trong hình mô tả `map_markers.jpg`.
+- Xác định chuẩn xác vị trí tương đối của các ID1, ID2, ID3, ID4 so với ID0 ở trung tâm (lấy hệ tọa độ mặt phẳng là trục x, y của marker ID0). Nên đặt các marker vùng rìa không đối xứng nhau nhằm tối ưu tính toán tọa độ trong trường hợp bị che khuất. Gắn các marker sao cho các trục x, y, z cùng hướng với nhau.
+<p align="center">
+  <img src="map_markers.jpg" alt="Tọa độ biểu diễn markers" width="600"/>
+</p>
+
+### 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_2 đặ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.
+- 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

camera_calibration/calibration.py

@@ -2,7 +2,12 @@ import cv2
 import os
 import numpy as np
 
+import sys
+import os
+
+sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
 
+from default_config import *
 
 # LỰa chọn độ phân giải bằng cách thay đổi set_resolution
 resolution = 'original'
@@ -10,8 +15,7 @@ resolution = 'original'
 # Checker board size
 CHESS_BOARD_DIM = (9, 6)
 
-# The size of Square in the checker board.
-SQUARE_SIZE = 24  # millimeters
+
 
 # termination criteria
 criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

camera_calibration/capture_images.py

@@ -10,11 +10,12 @@ visualization = True
 CHESS_BOARD_DIM = (9, 6)
 criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
 
-# checking if  images dir is exist not, if not then create images directory
-image_dir_path = f"CAMERA_CALIBRATION/images/{set_resolution}"
-if not os.path.exists(image_dir_path):
-    os.makedirs(image_dir_path)
-
+# nếu tồn tại thư mục ảnh, xóa đi và tạo lại
+image_dir_path = f"camera_calibration/images/{set_resolution}"
+if os.path.exists(image_dir_path):
+    import shutil
+    shutil.rmtree(image_dir_path)
+os.makedirs(image_dir_path)
 
 def detect_checker_board(image, grayImage, criteria, boardDimension):
     ret, corners = cv2.findChessboardCorners(grayImage, boardDimension)

camera_calibration/capture_images_without_showing.py

@@ -9,8 +9,11 @@ from default_config import *
 CHESS_BOARD_DIM = (9, 6)
 criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
 
-image_dir_path = f"CAMERA_CALIBRATION/images/{set_resolution}"
-os.makedirs(image_dir_path, exist_ok=True)
+image_dir_path = f"camera_calibration/images/{set_resolution}"
+if os.path.exists(image_dir_path):
+    import shutil
+    shutil.rmtree(image_dir_path)
+os.makedirs(image_dir_path)
 
 def detect_checker_board(image, grayImage, criteria, boardDimension):
     ret, corners = cv2.findChessboardCorners(grayImage, boardDimension)

default_config.py

@@ -1,5 +1,6 @@
 import cv2
 import os
+import numpy as np
 
 os.environ["QT_QPA_PLATFORM"] = "xcb"
 
@@ -9,6 +10,8 @@ MARKER_SIZE = 5  # centimeters
 set_resolution = 'original'
 RESOLUTION_STANDARDS = {'540p': 540, '720p': 720, '1080p': 1080, 'original': None}
 
+# The size of Square in the checker board.
+SQUARE_SIZE = 2.1  # centimeters
 
 # Thay đổi độ phân giải phù hợp với yêu cầu ban đầu
 def change_resolution(capture: cv2.VideoCapture, new_height):
@@ -23,3 +26,12 @@ def change_resolution(capture: cv2.VideoCapture, new_height):
         new_width = old_width
         new_height = old_height
     return (new_width, 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
+])

distance_estimation/camera_position.py

@@ -11,8 +11,9 @@ sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
 
 from default_config import *
 
+visualization = True
 
-
+# Tải dữ liệu hiệu chuẩn camera, phù hợp với độ phân giải ban đầu
 calib_data_path = f"camera_calibration/calib_data/{set_resolution}/MultiMatrix.npz"
 calib_data = np.load(calib_data_path)
 print('resolution:', set_resolution)
@@ -28,7 +29,7 @@ cap = cv2.VideoCapture(0)
 
 # Thay đổi độ phân giải phù hợp với yêu cầu ban đầu
 new_height = RESOLUTION_STANDARDS[set_resolution]
-new_width, new_height = change_resolution(cap,new_height)    
+new_width, new_height = change_resolution(cap,new_height)
 print(f'Resolution camera {new_width}x{new_height}')
 
 fps = cap.get(cv2.CAP_PROP_FPS)
@@ -70,13 +71,14 @@ while True:
         total_markers = range(0, marker_IDs.size)
         markers = {}
         for ids, corners, i in zip(marker_IDs, marker_corners, total_markers):
+
             corners = corners.reshape(4, 2)
             corners = corners.astype(int)
             top_right = corners[0].ravel()
             top_left = corners[1].ravel()
             bottom_right = corners[2].ravel()
             bottom_left = corners[3].ravel()
-    
+
             # Chuyển đổi rVec thành ma trận quay R
             R, _ = cv2.Rodrigues(rVec[i])
         
@@ -90,42 +92,54 @@ while True:
             marker_points_camera = R @ object_points.T + tVec[i].reshape(3, 1)
             marker_points_camera = marker_points_camera.T
 
-            distance = np.sqrt(
-                tVec[i][0][2] ** 2 + tVec[i][0][0] ** 2 + tVec[i][0][1] ** 2
-            )
+            distance = np.linalg.norm(tVec[i][0])
+
             # Draw the pose of the marker
-            # point = cv2.drawFrameAxes(frame, cam_mat, dist_coef, rVec[i], tVec[i], 4, 4)
-            x = -tVec[i][0][0]
-            y = tVec[i][0][1]
-            z = tVec[i][0][2]
-            pos = np.array([x, y, z])
+            point = cv2.drawFrameAxes(frame, cam_mat, dist_coef, rVec[i], tVec[i], 4, 4)
+
+
+            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)
+            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]]
+
             angles = rotationMatrixEulerAngles(R.T)
-            if ids == 2:
-                markers['marker 2'] = np.hstack((pos,angles))
-            if ids == 0:
-                pos += np.array([-12.3, 9.1, 0])
-                markers['marker 0'] = np.hstack((pos,angles))
-            if ids == 1:
-                pos += np.array([12.3, 9.1, 0.])
-                markers['marker 1'] = np.hstack((pos,angles))
-            if ids == 3:
-                pos += np.array([-7.2, -9.1, 0.])
-                markers['marker 3'] = np.hstack((pos,angles))
-            if ids == 4:
-                pos += np.array([12.3, -5., 0.])
-                markers['marker 4'] = np.hstack((pos,angles))
-        if 'marker 2' in markers.keys():
-            x, y, z, roll, pitch, yaw = np.round(markers['marker 2'], 1)
-        else:
-            cam_position = np.array([0., 0., 0., 0., 0., 0.])
-            for v in markers.values():
-                cam_position += v
-            cam_position /= len(markers)
-            x, y, z, roll, pitch, yaw = np.round(cam_position, 1)
-        print(x, y, z, roll, pitch, yaw)
-    # cv2.imshow("frame", frame)
-    key = cv2.waitKey(1)
-    if key == ord("q"):
-        break
+            markers[f'marker {ids[0]}'] = np.hstack((position, angles))
+
+            if visualization:
+                cv2.putText(
+                    frame,
+                    f"id: {ids[0]}",
+                    top_right,
+                    cv2.FONT_HERSHEY_PLAIN,
+                    1.3,
+                    (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)
+
+        print(','.join(f'{i:.1f}' for i in position.tolist()))
+    if visualization:
+        cv2.imshow("frame", frame)
+        key = cv2.waitKey(1)
+        if key == ord("q"):
+            break
 cap.release()
-cv2.destroyAllWindows()
+
+if visualization:
+    cv2.destroyAllWindows()

distance_estimation/distance_single.py

@@ -7,7 +7,7 @@ import sys
 sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '..')))
 from default_config import *
 
-visualzation = True
+visualization = True
 
 
 calib_data_path = f"camera_calibration/calib_data/{set_resolution}/MultiMatrix.npz"
@@ -45,7 +45,7 @@ while True:
         )
         total_markers = range(0, marker_IDs.size)
         for ids, corners, i in zip(marker_IDs, marker_corners, total_markers):
-            if visualzation:
+            if visualization:
                 cv2.polylines(
                     frame, [corners.astype(np.int32)], True, (0, 255, 255), 4, cv2.LINE_AA
                 )
@@ -62,7 +62,7 @@ while True:
             z = tVec[i][0][2]
 
             # Draw the pose of the marker
-            if visualzation:
+            if visualization:
                 point = cv2.drawFrameAxes(frame, cam_mat, dist_coef, rVec[i], tVec[i], 4, 4)
                 cv2.putText(
                     frame,
@@ -85,12 +85,12 @@ while True:
                     cv2.LINE_AA,
                 )
             print(f"Marker {ids[0]}: x = {x: .1f}, y = {y: .1f} , z = {z: .1f}, distance = {distance: .2f}")
-    if visualzation:
+    if visualization:
         cv2.imshow("frame", frame)
         key = cv2.waitKey(1)
         if key == ord("q"):
             break
 cap.release()
 
-if visualzation:
+if visualization:
     cv2.destroyAllWindows()