基于AprilTag的位姿估计方法

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

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

III DECTOR-C Homography and extrinsics estimation

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

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

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

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

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