如何将相机坐标转换为世界坐标使用OpenCV

Question

示意图图片

使用OpenCV slovepnp func Input world sopto world coortion socoration

R = np.array([[-0.0813445156856268], [-2.5478950926311636], [1.7376856594745234]], dtype=np.float32)
T = np.array([[10.838262901867047], [-6.506593974297687], [60.308121310607724]], dtype=np.float32)

coortiat已经测量了

世界坐标系统点 - ＆gt;相机坐标系点

0, 0 ,0 -> [11.052, -6.596, 60.9]

13, 0, 0 -> [-2.142, -5.628, 61.3]

13, 0, 13.5 -> [-2.668, -17.547, 56.2]  : possible Error 2 cm

如何使用R，T向量转换为世界坐标系？

Answer 1

我找到了转换公式

R = np.array([[-0.0813445156856268], [-2.5478950926311636], [1.7376856594745234]], dtype=np.float32)
T = np.array([[10.838262901867047], [-6.506593974297687], [60.308121310607724]], dtype=np.float32)

world_point = [13, 0, 0]

rvec_matrix = cv2.Rodrigues(R)[0]
rmat = np.matrix(rvec_matrix)
tmat = np.matrix(T)
pmat = np.matrix(np.array([[world_point[0]], [world_point[1]], [world_point[]2]], dtype=np.float32))

# world coordinate to camera coordinate
cam_point = rmat * pmat + tmat
print(cam_point)

# camera coordinate to world coordinate
world_point = rmat ** -1 * (cam_point - tmat)