我正在估计一个带有 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, 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 点感到迷茫。
我的问题是：
- 如何使用这些已知的外部参数来获取该对象的 3D 点？我是否解决了 X 和 Y 并将 Z 应用为 0？我该怎么做呢？
任何帮助将不胜感激！