面向RMUA2022比赛的视觉标签检测及辅助定位系统

面向RMUA2022比赛的视觉标签检测及辅助定位系统 Vision Marker Detector for RMUA 2022

在RMUA2022赛季，我们战队开发了一套利用场地内视觉标签进行辅助定位的系统，主要工作包括检测标签，字母识别，位姿优化。我们的检测系统具备一定的拓展性，除了场地内的英文字母外，还支持自定义标签的识别，亦可方便地应用于Sim2Real挑战赛中。因为比赛中机器人底盘的快速移动带来的相机运动模糊对于检测标签极为不利，标签的角点提取会不可避免地存在噪声，比赛的动态场景中存在标签被遮挡的情况，所以无论从精度，实用性还是鲁棒性上来说，视觉标签定位都与激光雷达定位存在较大差距，我们只将它作为一种辅助性的定位手段，可以用于给定位提供初始化位姿，也可用于定位失效后的重定位。

本项目主要参考了以下工作：

AprilTag: A robust and flexible visual fiducial system

AprilTag 2: Efficient and robust fiducial detection

opencv_contrib/aruco

1.依赖

本项目在以下环境中开发：

• Ubuntu20.04
• ROS noetic
• C++14

第三方库依赖：

• OpenCV 4:https://opencv.org/

• Ceres: https://github.com/ceres-solver/ceres-solver

  安装过程参考：https://blog.csdn.net/weixin_39373577/article/details/81285420

• Csparse：

  sudo apt-get install libsuitesparse-dev

• g2o:https://github.com/RainerKuemmerle/g2o

  安装过程参考：https://www.cnblogs.com/qilai/p/13866017.html

• Sophus:https://github.com/strasdat/Sophus

  安装过程参考：https://www.cnblogs.com/feifanrensheng/p/8619346.html 千万不要git checkout a621ff

• Eigen3:https://eigen.tuxfamily.org/index.php?title=Main_Page

  sudo apt-get install libeigen3-dev

• fmt:https://fmt.dev/8.1.1/index.html

  cd fmt
  mkdir build
  cd build
  cmake ..
  make -j8
  sudo make install

2.编译与使用

直接把功能包放到${your_ws}/src目录下，编译成功即可使用。我们使用了4个迈德威视的工业相机原始图像作为输入，镜头选择无畸变120度广角镜头，用来捕捉机器人周围的全向视野。

3.检测标签

3.1 自适应阈值二值化

首先把图像转成灰度图，为了找到红色和蓝色标签的候选，以多个窗口尺寸大小进行自适应阈值的二值化。

3.2 寻找轮廓

最耗时的一步。

3.3 四边形拟合

对找到的轮廓以大小，倾斜程度，是否靠近图像边缘等条件进行筛选，舍弃一些条件不好的候选。

3.4 去除相似候选

如果两个候选的角点距离之和太小，说明他们很可能表示一个标签，只留下面积最大的那个。

3.5 防止识别到白边

候选有可能是把白边也算上了，亦或是出现大圈套小圈的情况，这是我们所不希望的，_antiWhiteEdge的作用就是根据检测到的具有相同id的结果，把外圈删掉，这样能滤除一些检测到白边的情况。

3.6 角点精细化

opencv的角点精细化就是负优化，不要用

4.字母识别

要完成字母识别首先要建立一套标签的信息库，包括有效位编码，ID，四个角点的世界坐标等。对上一步检测出的标签候选求到实际标签的单应变换矩阵，分别对图像标签的外边（32个点）和内边（24个点）进行采样，计算白色区域和颜色区域的光度，作为解码时比较的依据。对5×5的有效位区域进行采样，与之前计算好的颜色模型光度进行比较，得到编码code，与事先建立好的标签库进行比较。比较时主要计算汉明距离，因为候选角点只能保证逆时针的角点顺序，所以还需要遍历4种旋转的比对，当汉明距离等于0时即为正确解码，退出循环。对于 H I O X N这些具有旋转对称性的字母，需要特殊处理，否则在旋转比对时会出现问题，具体做法为，对他们的角点顺序进行矫正，把左上角的角点作为0点，再进行二次解码，防止误解。

*汉明距离：表示两个（相同长度）字符串对应位置的不同字符的数量

5.优化位姿

全向感知的四个相机都有可能产生对视觉标签的观测（像素坐标），视觉标签在场地内的位置（世界坐标）已知，四个相机的内参和外参已知，我们可以利用这些信息构建BA优化，通过最小化重投影误差来估计车身在世界坐标系下的位姿。在所有相机找到的标签中选择最大的标签，进行PnP解算，得到的位姿结果作为初始位姿，再将所有识别到的标签的3D点、2D点利用g2o进行图优化，得到最终的位姿。具体做法直接参考了《视觉SLAM十四讲》7.7.3和7.8.3小节，只是在其中增加了车身和相机之间的外参。

设一视觉标签的某个角点在世界坐标系下的空间点坐标为$P$，记变换到相机坐标系下的该点坐标为$P’$，将其前三维提取出来：

$$\boldsymbol{P}^{\prime}=(\boldsymbol{T_{bc}}*\boldsymbol{T_{bw}}* \boldsymbol{P})_{1:3}=[X'，Y’，Z']^T$$

构建最小化重投影误差的非线性优化问题：误差$e$为像素坐标误差（2维），是将空间点的世界坐标根据待优化位姿投影到像素坐标系后，与测量值$u,v$求差。我们想直接优化的是$\boldsymbol{T_{wb}}$，即车身在世界坐标系下的位姿，但是为了表达和写代码方便，我们在这里将待优化变量设为$\boldsymbol{T_{bw}}$。

对$\boldsymbol{T_{bw}}$左乘扰动变量$ \delta \boldsymbol{\xi}$，然后考虑$e$的变化关于扰动量的导数，利用链式法则，可以列写如下：

$$\frac{\partial \boldsymbol{e}}{\partial \delta \boldsymbol{\xi}}=\lim _{\delta \boldsymbol{\xi} \rightarrow 0} \frac{\boldsymbol{e}(\delta \boldsymbol{\xi} \oplus \boldsymbol{\xi})-\boldsymbol{e}(\boldsymbol{\xi})}{\delta \boldsymbol{\xi}}=\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P}^{\prime}} \frac{\partial \boldsymbol{P}^{\prime}}{\partial \delta \boldsymbol{\xi}}$$

由于像素坐标与相机坐标之间可以直接通过内参转换：

$$\left\{\begin{array}{l} u=f_{x} \frac{X^{\prime}}{Z^{\prime}}+c_{x}\\ v=f_{y} \frac{Y^{\prime}}{Z^{\prime}}+c_{y} \end{array}\right.$$

易得第一项：

$$\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P}^{\prime}}=-\left[\begin{array}{ccc}\frac{\partial u}{\partial X^{\prime}} & \frac{\partial u}{\partial Y^{\prime}} & \frac{\partial u}{\partial Z^{\prime}} \\ \frac{\partial v}{\partial X^{\prime}} & \frac{\partial v}{\partial Y^{\prime}} & \frac{\partial v}{\partial Z^{\prime}}\end{array}\right]=-\left[\begin{array}{ccc}\frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}}\end{array}\right]$$

第二项为变换后的点关于李代数的导数，根据《十四讲》4.3.5节中的推导，得

$$\frac{\partial(\boldsymbol{T_{bc}}*\boldsymbol{T_{bw}}* \boldsymbol{P})}{\partial \delta \boldsymbol{\xi}}=\boldsymbol{T_{bc}}*\frac{\partial(\boldsymbol{T_{bw}}* \boldsymbol{P})}{\partial \delta \boldsymbol{\xi}}=\boldsymbol{T_{bc}}*(\boldsymbol{T_{bw}}* \boldsymbol{P})^{\odot}=\boldsymbol{T_{bc}}*\left[\begin{array}{cc}\boldsymbol{I} & -\boldsymbol{P}^{\prime \wedge} \\ \mathbf{0}^{\mathrm{T}} & \mathbf{0}^{\mathrm{T}}\end{array}\right]$$

在$\boldsymbol{P}^{\prime}$定义时，我们取出了前三维，于是得:，这是一个3×6的矩阵

$$\frac{\partial \boldsymbol{P}^{\prime}}{\partial \delta \boldsymbol{\xi}}=\boldsymbol{R_{bc}}*\left[\boldsymbol{I},-\boldsymbol{P}^{\prime \wedge}\right]$$

所以最终得：

$$\frac{\partial \boldsymbol{e}}{\partial \delta \boldsymbol{\xi}}=\frac{\partial \boldsymbol{e}}{\partial \boldsymbol{P}^{\prime}} \frac{\partial \boldsymbol{P}^{\prime}}{\partial \delta \boldsymbol{\xi}}=-\left[\begin{array}{ccc}\frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}}\end{array}\right]*\boldsymbol{R_{bc}}*\left[\boldsymbol{I},-\boldsymbol{P}^{\prime \wedge}\right]$$

四个相机找到的标签结果中，最大的那一个（可以认为是最近的）的PnP解算结果来作为优化的初值。