change logic to find camera position

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
-    [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
+    [15.0, -21.5, 0.], # marker 1
+    [-22., -21., 0.],  # marker 2
+    [-26.0, 15.5, 0.],   # marker 3
+    [23, 13, 0.]   # marker 4
 ])

distance_estimation/camera_position.py

@@ -99,10 +99,14 @@ while True:
             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 = 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,)
+            # R, _ = cv2.Rodrigues(rVec[i])        # Chuyển rVec sang ma trận quay
+            # 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 =np.array([-tVec[i][0][0], tVec[i][0][1], tVec[i][0][2]])
             position += default_positions[ids[0]]
 
             angles = rotationMatrixEulerAngles(R.T)