how do i transform camera coordinate to world coordinate use opencv

Question

schematic diagram picture

use opencv slovePNP func input world coordinate four point get R and T

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

The following corresponding data have been measured

World coordinate system point -> camera coordinate system point

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

How do I convert the camera coordinate system to the world coordinate system using R,T vector?

Answer 1

I have found the conversion formula

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)