论文阅读 2017 —— Sensor Fusion for Fiducial Tags: Highly Robust Pose Estimation from Single Frame RGBD

这是一篇发表在IROS的一篇论文，用RGBD相机提升码标（Aruco）的位姿精度。尽管在增强现实和计算机视觉中可以应用多种码标，但是在机器人应用中，从这些码标中获得准确的位姿目前仍然是个挑战。难点很清晰：

• 这些码标仅用了RGB相机，而且机器人上的相机的分辨率通常受到限制。换句话说，码标在图像上成像越大或越清晰，精度才能越高。
• 位姿根据码标矩形框的4个点，通过PnP的方法获得，因此，位姿精度直接与点的精度相关。

该论文，基于RGBD相机来测量码标的位姿，通过RGB图像保留了码标的高精度，同时利用深度图使其对尺寸、光照和感知噪声具有更强的鲁棒性。这些改进使得码标适用于现实环境中要求高姿态精度的机器人任务。

1 介绍

码标在位姿估计中常见，码标具有极低的错误率，所以在多种机器人应用中广泛出现。利用码标矩形框的4个顶点，采用PnP的方法可以计算出位姿，但是由于场景噪声的存在，顶点的检测会存在误差，造成位姿的不稳定性，特别是姿态信息，具有很大的误差。

如下图所示，机器人将要执行一项操作任务并重新排列桌子上的对象。Apriltags被用来寻找场景中目标物体的姿态，但是机器人最终无法抓取长方体，因为它的姿态方向是错误的。

为什么旋转角会有如此大的误差？ 下图给出简单的说明，两个立方体，其中一个旋转120°，这时候对应的两个面形状相似（但是仍然有差异，只不过差异很小），这时候角点检测的误差可能覆盖这两种情况。简单来说，在某些姿态下，微小的角点误差可能会导致较大的姿态误差。

下图给出了不同噪声等级下的位姿估计误差结果。4个角点被叠加不同噪声等级。总价可得以下几点

• 噪声越大，误差越高。
• 存在某些旋转角具有较大误差。
• 距离越远，误差越大。

码标的设计仅针对于RGB图像，但是随着传感器的发展，RGBD相机，可以降低噪声的影响。RGB和深度传感器的工作最佳互补。RGB数据可以用来有效地检测特征，深度数据可以保留更好的结构信息。可以利用这一点来改进姿态估计过程，提高定位精度。该论文的主要创新点如下：

• 方法在噪声场景下鲁棒，适用于多个场景。
• 该方法适用于大多数码标检测场景
• 计算连恒效，能够达到实时性需求。

2 方法

2.1 深度平面拟合

确定了码标的4个点，就可以利用深度图信息，获得码标内部的点云，利用极大似然估计的方法拟合出一个平面。（文章的公式写的晦涩难懂，实际上实现起来很简单）。这里的平面方程表示为$a_{1}x+a_{2}y+a_{3}z=d$，$a_1,a_2,a_3,d$是未知参数，之后约束$a_1^2+a_2^2+a_3^2=1$，计算对应的误差最小的参数，即得到拟合出的平面方程参数$(\mathbf{n},d)$。

2.2 初始位姿参数

根据检测出的靶标4个角点的位置，可以直接从深度图获得深度信息，这样，就可以得到4个空间点，将这些点投影到上一节拟合出的平面上，可以得到点集$\mathbf{p}=[p_1,p_2,p_3,p_4]$，这些点是在相机坐标系下表示的。对应的，在码标坐标系下（码标中心点为原点），很容易设置出相应的四个点坐标$\mathbf{q}=[q_1,q_2,q_3,q_4]$。

这样，就可以直接算出基于初始平面的旋转平移矩阵，$\mathbf{R},\mathbf{t}$。

$$[\mathbf{R},\mathbf{t}]=\underset{R\in SO(3),t\in R^3}{\mathrm{argmin}}\sum _{i=1}^n||R{q}_i+t-p_i|{|}^2$$

2.3 位姿提升

深度图是有误差的，因此，拟合出的平面不是最好的，那么什么样的平面是最好的呢， 作者认为，空间中平面上的码标4个点，投影到图像上，应该与检测出的像素位置，误差之和最小，即

$$[{\mathbf{R}}^{\ast },{\mathbf{t}}^{\ast }]=\underset{R^{\ast },t^{\ast }}{\mathrm{argmin}}\sum _{i=1}^n||K[{\mathbf{R}}^{\ast }|{\mathbf{t}}^{\ast }]p_i-y_i|{|}^2$$

这里$y_i$指的是图像上码标的4个点的像素位置。

设置好旋转矩阵的变化范围，求最有值即可，这里求最优值用的是scipy.optimize.least_squares

以上就是该算法的过程，思想还是很简单的，用RGBD提升精度想法还是很新奇的。

3 实验

实验从观察视角，距离和光照三个角度对位姿精度进行了分析，对比算法就是直接在RGB检测码标位姿的算法。

下图是针对观察视角的误差分析。显然，基于RGB的精度是不稳的，而利用深度图误差很小，且很稳定（我觉得精度有点好的过分，距离可能太近的原因，毕竟用的是tof相机，doge）

下图是针对距离的位姿精度分析，显然，深度图更精确，在1.2m之后才开始有显著的误差。

下图是4种不同光源的误差情况，场景越黑，误差越高。


关于计算时间，这个算法需要额外的11ms，因此，完整的检测流程就达到35ms，速度也是可以的。

4 总结

这个算法，应该算是第一个利用RGBD去提升靶标位姿精度的。所以思想很新奇，但实验感觉有点少，更多的细节可能需要跑下源码才能知道。但毕竟是个会议论文，在无人机跟踪降落这个场景应用还是很强的。