如何获得具有已知初始外在参数的相机的 3D 点？

如何解决如何获得具有已知初始外在参数的相机的 3D 点？

我正在估计一个带有 apriltags 的物体的姿态。

我最初为 apriltag 板成功地做到了这一点：

使用标签半径（tag_size/2）找到 3D 点，如代码所示：

ob_pt1 = [-tag_size/2,-tag_size/2,0.0]
ob_pt2 = [ tag_size/2,- tag_size/2,0.0]
ob_pt3 = [ tag_size/2,tag_size/2,0.0]
ob_pt4 = [-tag_size/2,0.0]
ob_pts = ob_pt1 + ob_pt2 + ob_pt3 + ob_pt4
object_pts = np.array(ob_pts).reshape(4,3)

现在我必须估计一个带有 apriltags 的物体的姿势。我现在有固定在物体上的 apriltags 的已知初始姿势（旋转和平移向量）。

我在旋转向量上使用了 Rodrigues 来获得旋转矩阵。 使用这个：

我知道我还必须将平移向量应用于旋转矩阵。这将是 3x4 矩阵，即姿势。 而且我知道 Z 将始终为“0”，但我对如何将其用于 3D 点感到迷茫。

我的问题是：

1. 如何使用这些已知的外在函数来获取此对象的 3D 点？我是否解决了 X 和 Y 并将 Z 应用为 0？我该怎么做？

任何帮助将不胜感激！