基於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矩陣範數最小的旋轉矩陣。