如何获得具有已知初始外部特征的摄影机的3D点？

Question

我正在估算一个物体的姿势，并将apriltags附加到它们上

我最初为apriltag板成功地完成了这项工作：

使用标签半径（标签尺寸/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,  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. 如何使用这些已知的外部对象获取此对象的三维点？我解X和Y，然后把Z作为0吗？我该怎么做呢

任何帮助都将不胜感激