使用 aruco 估计相机的世界位置

Question

在 OpenCV 中，我使用的是 Charuco 板，已校准相机，并使用估计来获取 rvec 和 tvec。 （类似于示例代码）。我正在使用固定板，相机四处移动。我是一个菜鸟，所以如果这是我想念的简单的东西，请多多包涵。

我知道我可以使用 Rodrigues() 从 rvec 获取板方向的 3x3 旋转矩阵，并且我可以使用 worldPos = -Rt() * tvec（在 python 中）将 tvec 值转换为世界坐标获取相机位置

似乎我得到了我需要的一切，但是，如果我物理旋转相机，WorldPos 也会发生巨大变化！我确定这不是因为我尝试了很多次后相机已经移动了。

不知道问题出在哪里，谢谢你的建议。

代码如下（在python中）。

self.retval, self.rvec, self.tvec =   aruco.estimatePoseBoard(self.corners, self.ids, board, self.cameraMatrix, self.distanceCoefficients)
self.dst, jacobian = cv2.Rodrigues(self.rvec)
self.rvec_trs = self.dst.transpose()
self.worldPos = - self.rvec_trs * self.tvec
self.worldPos = [self.worldPos[0][0],self.worldPos[1][1], self.worldPos[2][2]]

Answer 1

最后我在@Micka 的帮助下解决了这个问题。 代码如下（在python中），希望它可以帮助其他有同样问题的人。

self.retval, self.rvec, self.tvec = aruco.estimatePoseBoard(self.corners, self.ids, board, self.cameraMatrix, self.distanceCoefficients
self.dst, jacobian = cv2.Rodrigues(self.rvec)
self.extristics = np.matrix([[self.dst[0][0],self.dst[0][1],self.dst[0][2],self.tvec[0][0]],
                             [self.dst[1][0],self.dst[1][1],self.dst[1][2],self.tvec[1][0]],
                             [self.dst[2][0],self.dst[2][1],self.dst[2][2],self.tvec[2][0]],
                             [0.0, 0.0, 0.0, 1.0]
                ])
self.extristics_I = self.extristics.I  # inverse matrix
self.worldPos = [self.extristics_I[0,3],self.extristics_I[1,3],self.extristics_I[2,3]]

但我仍然不知道为什么 worldPos = -R.t() * tvec 不能得到正确的答案。希望有人可以告诉我。谢谢！