apriltag位姿 python apriltag位姿估计方法

论文：AprilTag: A robust and flexible visual fiducial system

三 检测器-单应矩阵和外参估计

III DECTOR-C Homography and extrinsics estimation

作者计算$3*3$的单应矩阵，该矩阵可以将齐次坐标形式的2D点从Tag坐标系转换到2D图像坐标系（猜测这里的Tag坐标系原点为标签正中心，右下分别为XY，相机正前方为Z。在此坐标系下，Tag的中心点坐标为$[0, 0, 1]^T$, 而且标签在XY方向拓展了1个单位？）。单应矩阵的计算采用DLT（Direct Linear Transform, 直接线性变换）算法。需要主要的是此处的单应矩阵转换的点坐标是齐次坐标形式，所以这些点的坐标仅按比例尺定义（也就是物理坐标与实际相差一个比例尺）。

在计算标签的位置和姿态信息时需要知道：1）相机的焦距；2）标签的物理尺寸。基于DLT计算的$3*3$单应矩阵可以重写为$3*4$的相机内参矩阵$P$（假设已知）和$4*3$的截断外参矩阵$E$。本来外参矩阵典型的是$4*4$，但是由于标签上的每个位置在Tag坐标系下的z向坐标都等于0。因此，将每个Tag坐标重写为Z向为0的2D齐次坐标，而且移除了外参矩阵的第3列，剩余的便是截断外参矩阵。外参矩阵E的的旋转分量表示为$R_{ij}$，位移分量表示为$T_k$。未知的尺度因子为s：

需要注意的是，我们不能直接求解出$E$，因为P是不满秩矩阵。但是可以拓展等式的右手边，写出$h_{ij}$的表达式如下:

这样的话能够比较容易的计算出$R_{ij}$的元素和$T_k$的元素，除了尺度因子s。但是，由于旋转矩阵的列必须是单位向量，所以可以基于此抑制s的幅值。此处有旋转矩阵中的两列信息，可以计算s作为其两列幅值的几何平均值$\sqrt{ab}$。s的符号可以通过要求标签出现在相机的前方来恢复，比如Tz<0的位置。旋转矩阵的第3列可以通过计算已知两列的叉乘计算得出，因为旋转矩阵是正交的。

DLT步骤和上面的归一化步骤不能保证旋转矩阵严格正交。为了对此进行修正，作者计算了矩阵$R$的极分解，其可生产一个误差的Frobenius矩阵范数最小的旋转矩阵。