基于AprilTag的位姿估计方法

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

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

III DECTOR-C Homography and extrinsics estimation

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

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

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

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

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