如何使用 QR/Aruco 代码（Python CV）将点云从相机框架转换为世界框架？

Question

我想将来自多个深度相机的点云合并在一起。 为了实现这一点，我试图将每个相机帧的点云转换为由 aruco 代码（类似于二维码）定义的单个世界帧。

现在，我只是从一台相机开始。

假设我有

k = #camera intrinsic matrix (3x3)
aruco_world_coords = #coordinates (meters) of the aruco code corners in the real world (4x3) 
                     e.g. [[0,    0,  0],
                           [.1,   0,  0],
                           [.1, -.1,  0],
                           [0,  -.1,  0]]
rgb_image = #camera's color image (width x height x 3)
cam_pc = #point cloud in camera's frame (nx3)

我目前执行以下操作：

import cv2 as cv
import numpy as np
from cv2 import aruco

#######################################################################
#####          Detect Aruco code and extract rvec and tvec       ######
#######################################################################

gray = cv.cvtColor(rgb_image, cv.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
params =  aruco.DetectorParameters_create()

corners, _, _ = aruco.detectMarkers(gray, aruco_dict, parameters=params)

rvec, tvec = cv.solvePnP(QR_world_coords, 
                            corners[0], 
                            K, None, None, None,
                            False, cv.SOLVEPNP_ITERATIVE)

rmatrix, _ = cv.Rodrigues(rvec)

#######################################################################
#####                  Calculate new point cloud                 ######
#######################################################################

new_pc = np.matmul(np.linalg.inv(rmatrix), cam_pc.T-tvec).T

#######################################################################
#####           Same math in a different way to verify result     #####
#######################################################################

'''
t = -rmatrix.T.dot(tvec)
rmatrix_ = rmatrix.T
new_pc = cam_pc.dot(rmatrix_.T) + tvec.squeeze()
'''

#######################################################################
##### Same math, in yet another way, to be super sure it's right  #####
#######################################################################

'''
m = np.hstack([rmatrix, trans])
m = np.vstack([m, np.array([[0,0,0,1]])])
m = np.linalg.inv(m)

cam_pc_homog = np.concatenate([cam_pc, np.ones((pc.shape[0],1))], axis=1)
new_pc = np.matmul(m, cam_pc_homog.T).T
new_pc /= new_pc[:,3].reshape(-1,1)
new_pc = new_pc[:,:3]
'''

然后我在一个场景中显示新的点云，轴在原点 (0,0,0) 处绘制。

我现在希望当我将相机指向 Aruco 代码时，点应该移动，而且确实如此。

然而，我也希望点云现在应该保持静止，即使我旋转相机，因为这些点已经转换到世界框架中，并且世界相对于世界框架没有移动。 这不会发生，当我四处移动相机时，这些点会继续移动（尽管以不同的方式）。

我相信旋转和平移是正确的，我使用它们将轴显示到 rgb_image 中的 aruco 代码上，并且它们位于正确的位置。

任何帮助将不胜感激！