前言

在阅读无人机跟踪论文Fast-Tracker时，突然好奇是如何实现目标识别与定位的，发现文中是使用了一种“二维码”AprilTag进行定位。使用ApirlTag，系统可以迅速得到该二维码距离距离自己（摄像头）有多远，处于什么位姿。这是一种稳定的目标识别与定位方法，常用于后续算法，如跟踪算法，的验证。

补充，在现实应用与商业无人机（例如大疆）上，目标的识别通常通过图像识别算法来实现。

关于ApirlTag，其是由密歇根大学的Edwin Olson于2011年发表的，虽然看起来像是二维码，但是于日常中常见的二维码有许多不同。即AprilTag的核心目的不是为了存储大量数据，而是为了在极端的环境（光照不均、距离很远、角度倾斜、部分被遮挡）下，依然能被摄像头瞬间认出来，并精确计算出它在三维空间中的位置和姿态（即 6个自由度：前后、左右、上下，以及俯仰、偏航、滚转）。 所以AprilTag看起来比一般的二维码简单许多。
注意AprilTag不仅是一种标签生成的方式，也是一种标签检测系统

算法框架

对于每一张输入的灰度图，都将进行以下操作

边缘检测

1. 计算像素“梯度”：计算相邻点像素值变化的值与方向
2. 聚类与画线：将变化程度与方向相近的点聚类，然后拟合出一条直线

四边形检测

1. 在上述所有线段中寻找首尾相连、逆时针环绕的四条线段
2. 抗遮挡：允许线段的端点之间存在缺口，即不必真的“首尾相连”

位姿估计

1. 系统提前知道真实的AprilTag是边长已知的正方形，但由于透视，其在照片中可能变成了一个梯形或者菱形
2. 系统通过对比已知的正方形和照片中畸变的四边形，并利用相机参数，可以求出 单应性矩阵 ，类似于变换矩阵，从而可以逆向推断出照片中的ApirlTag的位姿

读取ID

1. AprilTag不仅能推断出标签的位姿，还能识别内部的自带的ID
2. 而AprilTag并不是简单将低于/高于固定灰度值的部分分为白色/黑色，而是建立一个随空间变化的光照模型，即通过相对亮度区分，于是可以应对光照不均匀的问题

纠错防伪

画面中可能存在例如斑马线、黑白格子衫这样的物体，要如何保证系统不会认错标签？

1. 保证汉明距离：系统生成的每一张AprilTag，其黑白格子的图案和其他的标签都有巨大差别，保证即使读错了几个格子也能猜出正确的ID
2. 图案复杂：AprilTag编码在设计之初就淘汰了如纯黑这样几何复杂度低、且易在自然中产生的图案

具体实现

假设输入一张(w, h)的灰度图，其数值为0~255，系统应该如何处理

边缘检测

此处方法与ARTag的检测器相似。是整个过程中最为耗时的部分，通常可以以一半的分辨率执行，提高4倍速度。

计算梯度

首先对于图像上的任意一点 (x,y)，考察它水平方向和垂直方向的亮度差

• 水平差分：$g_x = I(x+1,y) - I(x-1,y)$
• 垂直差分：$g_y = I(x,y+1) - I(x,y-1)$
  从而可以计算出梯度的大小与方向
• 梯度幅值：$M(x,y) = \sqrt{g_x(x,y)^2 + g_y(x,y)^2}$
• 梯度方向：$\theta(x,y) = \operatorname{arctan2}(g_y(x,y), g_x(x,y))$

聚类

经过上述操作，图上每一个像素点p都有了两个物理量，然后使用基于图的聚类方法。
创建一个图，其中每个节点代表一个像素。在相邻像素之间添加边，边权重等于像素在梯度方向上的差异：

$$ W_{i,j} = \theta(p_i) - \theta(p_j) \pmod{2\pi} $$

然后按照权重从小到大去尝试把节点合并为一个“component”。即初始时一个节点代表了一个component，后续可能有多个component的合并。假设现在有两个相邻的component，$n$ 和 $m$，通过以下条件判断是否要将其合并：

$$ D(n \cup m) \le \min(D(n), D(m)) + \frac{K_D}{|n \cup m|} $$ $$ M(n \cup m) \le \min(M(n), M(m)) + \frac{K_M}{|n \cup m|} $$

其中：

• $D(n) = \max_{p \in n} \theta(p) - \min_{p \in n} \theta(p)$ 为方向极差
• $K(n) = \max_{p \in n} M(p) - \min_{p \in n} M(p)$ 为幅值极差
• $|n \cup m|$：代表合并后群落包含的像素总数。
• $K_D$：人为设定的常数。
• 巧妙之处：公式右侧的 $\frac{K_D}{|n \cup m|}$ 是一个动态宽容度项。当群落很小（像素少）时，分母小，宽容度极大，允许像素快速抱团，具有极强的抗噪能力；当群落成长为长线段时（像素多），宽容度迅速趋近于 0，算法变得极其严苛，强制保证最终提取出的线段必须是“笔直且方向均匀”的。

画线

使用最小二乘法，对于一个component的像素拟合出几何直线方程
1.计算加权重心 以像素的梯度幅值 $M$ 作为权重 $w_i$，由于处在“悬崖”边缘的像素更具代表性，赋予其更高权重。设共有 $N$ 个像素，计算加权平均坐标 $(\bar{x}, \bar{y})$：

$$ \bar{x} = \frac{\sum_{i=1}^{N} w_i \cdot x_i}{\sum_{i=1}^{N} w_i}, \quad \bar{y} = \frac{\sum_{i=1}^{N} w_i \cdot y_i}{\sum_{i=1}^{N} w_i} $$

随后将所有坐标平移至重心处（去中心化）：$x_i^{\prime} = x_i - \bar{x}, \quad y_i^{\prime} = y_i - \bar{y}$

2. 构建加权协方差矩阵 利用去中心化后的坐标，构建一个 $2 \times 2$ 的加权协方差矩阵 $\mathbf{C}$ 来描述像素的几何分布形态：

$$ \mathbf{C} = \begin{bmatrix} C_{xx} & C_{xy} \\ C_{xy} & C_{yy} \end{bmatrix} $$

其中矩阵元素的计算公式如下：

$$ C_{xx} = \frac{\sum_{i=1}^{N} w_i \cdot (x_i^{\prime})^2}{\sum_{i=1}^{N} w_i}, \quad C_{yy} = \frac{\sum_{i=1}^{N} w_i \cdot (y_i^{\prime})^2}{\sum_{i=1}^{N} w_i}, \quad C_{xy} = \frac{\sum_{i=1}^{N} w_i \cdot x_i^{\prime} \cdot y_i^{\prime}}{\sum_{i=1}^{N} w_i} $$

3. 求解特征向量得到直线方程 直线的走向（倾斜角 $\theta$）即为该协方差矩阵最大特征值对应的特征向量方向。其解析解可以直接通过下式计算：

$$ \theta = \frac{1}{2} \operatorname{arctan2}\bigl(2 C_{xy}, C_{xx} - C_{yy}\bigr) $$

最终，利用倾斜角 $\theta$ 和重心 $(\bar{x}, \bar{y})$ ，我们可以直接写出没有任何数学奇点的亚像素级直线标准方程 $Ax + By + C = 0$：

$$ A = -\sin\theta, \quad B = \cos\theta, \quad C = -(A\bar{x} + B\bar{y}) $$

四边形检测

使用深度为4的深度优先搜索，并在深度一考虑所有线段，而在深度二至四考虑所有开始距离前一个线段结束位置“足够近”并且遵循逆时针缠绕 顺序的线段。

通过调整“足够接近”阈值来处理对遮挡和分割错误的鲁棒性:通过使阈值变大,可以处理边缘 周围的显着间隙。文中“足够接近”的阈值是线长度的两倍加上五个额外像素。

在找到四边形后边可以得到对应四个角在二维图像中的坐标

位姿估计

在找到了二维照片上的 4 个精确角点后，我们需要将其映射回真实的三维物理空间。这里用到了计算机视觉中最经典的“小孔相机投影模型”。
1. 建立巧妙的世界坐标系 将世界坐标系的原点强行定义在真实 AprilTag 的正中心，并让标签平铺在 $X-Y$ 平面上。这一设定的绝妙之处在于，标签上所有点的物理 $Z$ 坐标全部归零：

$$ Z_w = 0 $$

2. 投影矩阵的“降维打击” 根据相机的透视投影公式，世界坐标 $(X_w, Y_w, Z_w)$ 投影到图像像素坐标 $(u, v)$ 满足以下关系：

$$ s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \mathbf{K} \begin{bmatrix} \mathbf{r_1} & \mathbf{r_2} & \mathbf{r_3} & \mathbf{t} \end{bmatrix} \begin{bmatrix} X_w \\ Y_w \\ Z_w \\ 1 \end{bmatrix} $$

参数说明：

• $s$：透视缩放因子（由于“近大远小”丢失的深度信息）。
• $\mathbf{K}$：相机的 $3 \times 3$ 内参矩阵（包含焦距和光心）。
• $\mathbf{r_1}, \mathbf{r_2}, \mathbf{r_3}$：相机旋转矩阵 $\mathbf{R}$ 的三个列向量。
• $\mathbf{t}$：平移向量 $[t_x, t_y, t_z]^T$，即相机到标签中心的真实物理距离。

由于 $Z_w = 0$，矩阵乘法中涉及 $\mathbf{r_3}$ 的项统统被消灭（乘以 0）。原本 $3 \times 4$ 的复杂空间投影，完美坍缩为一个 $3 \times 3$ 的平面映射矩阵，这就是**单应性矩阵 $\mathbf{H}$**：

$$ s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \mathbf{K} \begin{bmatrix} \mathbf{r_1} & \mathbf{r_2} & \mathbf{t} \end{bmatrix} \begin{bmatrix} X_w \\ Y_w \\ 1 \end{bmatrix} \quad \Longrightarrow \quad \mathbf{H} = \mathbf{K} \begin{bmatrix} \mathbf{r_1} & \mathbf{r_2} & \mathbf{t} \end{bmatrix} $$

(通过四对已知的角点坐标，运用直接线性变换 DLT 算法即可解出 $\mathbf{H}$ 的所有元素。)

解出单应性矩阵 $\mathbf{H}$ 后，我们需要逆向剥离出我们真正想要的平移向量 $\mathbf{t}$（距离）和旋转矩阵 $\mathbf{R}$（姿态）。
3. 消除相机内参干扰 在等式两边同时左乘相机内参矩阵的逆 $\mathbf{K}^{-1}$，我们将计算结果记为三个列向量 $\mathbf{h_1}, \mathbf{h_2}, \mathbf{h_3}$：

$$ \mathbf{K}^{-1}\mathbf{H} = \begin{bmatrix} \mathbf{h_1} & \mathbf{h_2} & \mathbf{h_3} \end{bmatrix} = s \begin{bmatrix} \mathbf{r_1} & \mathbf{r_2} & \mathbf{t} \end{bmatrix} $$

这实际上给出了三个极其关键的等式：

$$ \mathbf{h_1} = s \cdot \mathbf{r_1}, \quad \mathbf{h_2} = s \cdot \mathbf{r_2}, \quad \mathbf{h_3} = s \cdot \mathbf{t} $$

4. 利用刚体物理定理求解缩放因子 $s$ 在真实的物理空间中，旋转矩阵的列向量 $\mathbf{r_1}$ 和 $\mathbf{r_2}$ 代表方向轴，它们的模长（长度）必须严格等于 1（即 $|\mathbf{r_1}| = |\mathbf{r_2}| = 1$）。基于这个物理公理，我们可以直接算出透视缩放因子 $s$。为了减少图像噪点误差，通常取前两列模长的几何平均值：

$$ s = \sqrt{\|\mathbf{h_1}\| \cdot \|\mathbf{h_2}\|} $$

5. 提取平移距离与旋转轴 既然 $s$ 已经求出，终极目标瞬间解开：

• 物理距离（平移向量）：直接拿到了无人机距离目标的绝对 $X, Y, Z$ 坐标！

$$ \mathbf{t} = \frac{\mathbf{h_3}}{s} $$

• 物理姿态（旋转向量）：解出真实的 $X$ 轴和 $Y$ 轴朝向。

$$ \mathbf{r_1} = \frac{\mathbf{h_1}}{s}, \quad \mathbf{r_2} = \frac{\mathbf{h_2}}{s} $$

• 找回丢失的 $Z$ 轴：利用三维几何的右手法则，通过向量的叉乘（Cross Product）无中生有地生成垂直于平面的第三个旋转轴：

$$ \mathbf{r_3} = \mathbf{r_1} \times \mathbf{r_2} $$

至此，我们初步拼凑出了 $3 \times 3$ 的旋转矩阵 $\tilde{\mathbf{R}} = \begin{bmatrix} \mathbf{r_1} & \mathbf{r_2} & \mathbf{r_3} \end{bmatrix}$。

6.旋转矩阵正交化 由于像素离散误差和镜头畸变，我们粗略拼凑出来的 $\tilde{\mathbf{R}}$ 往往并不完美（三个轴可能不绝对正交）。如果将其直接输入无人机飞控，会导致严重的姿态计算错误。
数学解法（极分解）： 我们需要寻找一个在三维旋转群 $SO(3)$ 中，距离 $\tilde{\mathbf{R}}$ 最近的“绝对完美的正交旋转矩阵” $\mathbf{R}^*$。通过对粗糙矩阵 $\tilde{\mathbf{R}}$ 进行奇异值分解（SVD）：

$$ \tilde{\mathbf{R}} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^T $$

将代表拉伸变形的对角阵 $\mathbf{\Sigma}$ 强制替换为单位阵 $\mathbf{I}$，即可得到完全正交的最优旋转矩阵：

$$ \mathbf{R}^* = \mathbf{U} \mathbf{V}^T $$

读取ID

在真实的物理环境中，光照往往是极度不均匀的。比如标签一半暴露在刺眼的阳光下，一半掩盖在浓重的树荫里。此时，阴影区域的“纯白色”像素，其绝对亮度可能比阳光区域的“纯黑色”像素还要暗。如果使用一个全局固定的亮度阈值（如 128）去二值化图像，阴影区的密码将全军覆没。

数学解法：建立空间光照曲面 AprilTag 利用了其本身固有的几何设计：标签的最外圈固定是一层白边，往内固定是一层黑框。这些边框的颜色是绝对已知的。 算法在这些已知的白边和黑框中采样若干像素点 $(x_i, y_i)$ 以及它们的真实灰度值 $I_i$，并在数学上假设光照强度的变化在二维空间中是一个双线性平面（Bilinear Surface）。
为此，算法分别构建了两个连续的二维空间亮度函数：
黑色区域的光照曲面模型：

$$ I_{\text{black}}(x, y) = A_1 x + B_1 xy + C_1 y + D_1 $$

白色区域的光照曲面模型：

$$ I_{\text{white}}(x, y) = A_2 x + B_2 xy + C_2 y + D_2 $$

现在，算法准备去读取标签内部的任意一个数据网格（密码位）。假设该网格中心的像素坐标为 $(x_k, y_k)$。我们不再去和一个死板的固定数字比大小，而是将该坐标分别代入刚刚拟合出的两张曲面方程中，计算出它**专属的动态阈值 $T(x_k, y_k)$**（即黑与白的中间值）：

$$ T(x_k, y_k) = \frac{I_{\text{black}}(x_k, y_k) + I_{\text{white}}(x_k, y_k)}{2} $$

最后，用该坐标点真实的相机传感器亮度 $I_{\text{actual}}(x_k, y_k)$ 与其专属阈值进行比较，直接输出二进制位：

$$ \text{Bit}(x_k, y_k) = \begin{cases} 1 \text{ (白)} & \text{if } I_{\text{actual}}(x_k, y_k) > T(x_k, y_k) \\ 0 \text{ (黑)} & \text{if } I_{\text{actual}}(x_k, y_k) \le T(x_k, y_k) \end{cases} $$

纠错防伪

除了上面所有的检测措施之外，AprilTag在设计时还加入了许多限制

汉明距离纠错

为了保证即使部分被遮挡也能成功读取，在设计时保证了任何两个标签之间有足够的汉明距离，例如36h10一族标签代表了有36个格子，最小汉明距离为10

旋转不变性

AprilTag还限制了任意标签在旋转后与原来的自己也保持最小汉明距离，从而可以迅速读出标签的旋转

几何复杂度

自然界中也可能出现类似的图形，例如一个方形窗户中的光阴。AprilTag设定了每个标签绘制所需要的最少矩形个数，排除了如纯黑色标签这种形式

总结

AprilTag通过数学上的精妙运算，实现了一种快速准确且能够在极端环境运行的视觉定位系统

ApirlTag（2011）在后续还推出了ApirlTag2（2016）、ApirlTag3（2019）。分别实现了更精准快速的检测器、更多样化的标签生成机制。

参考内容

AprilTag