AVP-SLAM 小结: Semantic Visual Mapping and Localization for Autonomous Vehicles in the Parking Lot

Reference:

1. AVP-SLAM: Semantic Visual Mapping and Localization for Autonomous Vehicles in the Parking Lot

1. INTRODUCTION

基于视觉的传统定位方式有以下挑战：

1. 室内和地下停车场大多由无纹理的墙壁、杆子和地面组成。特征检测和匹配不稳定。传统的可视化方法容易出现跟踪丢失的问题；
2. 不同的车辆可能在不同的时间停放至停车场，这使得外观发生了很大的变化。

因此在AVP-SLAM这个系统中，采用了语义特征：引导标志、停车线和减速带等等-------这些通常出现在停车场。与传统的几何特征相比，这些语义特征对视角和光照变化具有长期稳定和鲁棒性。

该系统采用：

1. $4$ 个环视相机-----增加感知距离；
2. IMU+Wheel encoders-----构成里程计，提供相对位姿，但是会有累积误差。

2. METHODOLOGY

大致的步骤就是下面这个样子，分成两个部分MAPPING+LOCALIZATION：

2.1 IPM Image

说白了就是将图片从四路鱼眼拼成一个顶视图(鸟瞰图)，如上图所示。拼的过程写了一堆转换公式，很简单就不再提炼出来了。

2.2 Feature Detection

使用了卷积神经网络做语义特征检测，文中使用的是 U-Net。这个网络是用在停车场捕获的图像进行专门训练的，它将像素分类为车道、停车线、引导标志、减速带、可行驶区域、障碍物和墙壁。在这些类别中，停车线、导向标志、减速带是鲜明而稳定的特征，用于做定位。停车线也被用于停车点检测。可行驶区域和障碍物被用于规划。

2.3 Local Mapping

鸟瞰图的坐标与三维空间对应：
$$\left[\begin{array}{c}{x}^v\\ y^v\\ 1\end{array}\right]={\mathbf{K}}_{ipm}^{-1}\left[\begin{array}{c}{u}_{ipm}\\ v_{ipm}\\ 1\end{array}\right]$$

这时候获得的是相对车身中心坐标，再转到世界坐标系：
$$\left[\begin{array}{c}{x}^w\\ y^w\\ z^w\end{array}\right]={\mathbf{R}}_{\mathbf{o}}\left[\begin{array}{c}{x}^v\\ y^v\\ 0\end{array}\right]+{\mathbf{t}}_o$$其中 $\left[\begin{array}{ll}{\mathbf{R}}_{\mathrm{o}}& {\mathbf{t}}_o\end{array}\right]$ 是从里程计内得到的位姿，这些点聚合到一个local map内(如果算力足够，应该将里程计得到的位姿作为初值，如前面得到的本次的local map做个2D的ICP是否更合适？)每隔 $30m$ 建一张 local map。

2.4 Loop Detection

由于里程计的漂移时间较长，我们通过检测回环来消除漂移。对于最新的local map，我们将其与其他周边local map进行比较。采用ICP对两幅local map进行匹配(这里的ICP用的哪一种处理多个类别？)。如果两个local map匹配成功，则得到这两个local map之间的相对位姿。此相对位姿将用于全局位姿图优化以校正漂移。

比如 Fig. 5(a) 与 Fig. 5(b) 两个 local map 要是直接合并重叠的效果如 Fig. 5© 一样不是太好，但要是用这个值作为初始值优化出一个回环检测的相对位姿，那效果就很 nice 了，如 Fig. 5(d) 所示。

2.5 Global Optimization

在回环检测发生后，对位姿图进行全局优化，消除累积漂移，保持整个图的一致性。在此位姿图中，node是每个local map的位姿，包含三个旋转轴：$\mathbf{r}={\left[\begin{array}{lll}{r}_x& r_y& r_z\end{array}\right]}^T$ 和三个平移量：$\mathbf{t}={\left[\begin{array}{lll}{t}_x& t_y& t_z\end{array}\right]}^T$。

有两种边：

1. 里程计边：它通过里程计测量来约束两个连续的local map；
2. 回环边：约束回环的 local maps。

位姿图优化可以写成以下 cost function：
$$\begin{array}{c}{\mathcal{X}}^{\ast }=\underset{\mathcal{X}}{\mathrm{argmin}}\sum _t{\Vert f\left({\mathbf{r}}_{t+1},{\mathbf{t}}_{t+1},{\mathbf{r}}_t,{\mathbf{t}}_t\right)-{\mathbf{z}}_{t,t+1}^o\Vert }^2\\ +\sum _{i,j\in \mathcal{L}}{\Vert f\left({\mathbf{r}}_i,{\mathbf{t}}_i,{\mathbf{r}}_j,{\mathbf{t}}_j\right)-{\mathbf{z}}_{i,j}^l\Vert }^2\end{array}$$其中：
3. $\mathcal{X}={\left[{\mathbf{r}}_0,{\mathbf{t}}_0,\dots ,{\mathbf{r}}_t,{\mathbf{t}}_t\right]}^T$ 是所有 local map 的位姿；
4. ${\mathbf{z}}_{t,t+1}^o$ 是从里程计得到的 $t$ 与 $t+1$ 间的相对位姿；
5. $\mathcal{L}$ 是所有的回环匹配对；
6. ${\mathbf{z}}_{i,j}^l$ 是通过ICP的回环得到的回环帧 $i$ 与 $j$ 之间的相对位姿；
7. 函数 $f(\cdot )$ 表示两个 local map 间的相对位姿。

优化使用的是 Gauss-Newton 方法。

在全局位姿优化后，通过更新位姿将local map堆叠在一起。通过这种方式，可以生成全局一致的map。

2.6 Localization

基于上面生成的语义地图，再次来到这个停车场时，车辆可以进行定位。与建图过程类似，四个环视图像拼成顶视图后提取语义信息。然后通过将当前特征点与地图进行匹配来估计车辆的当前位姿，如图6所示。估计采用了ICP法，可以写成以下公式：
$${\mathbf{r}}^{\ast },{\mathbf{t}}^{\ast }=\underset{\mathbf{r},\mathbf{t}}{\mathrm{argmin}}\sum _{k\in \mathcal{S}}{\Vert \mathbf{R}(\mathbf{r})\left[\begin{array}{c}{x}_k^v\\ y_k^v\\ 0\end{array}\right]+\mathbf{t}-\left[\begin{array}{c}{x}_k^w\\ y_k^w\\ z_k^w\end{array}\right]\Vert }^2$$其中：

1. $r$ 和 $t$ 是当前帧的三维旋转和平移向量；
2. $\mathcal{S}$ 是当前特征点集合；
3. $\left[\begin{array}{lll}{x}_k^v& y_k^v& 0\end{array}\right]$ 是在车辆坐标系下的当前特征；
4. $\left[\begin{array}{lll}{x}_k^w& y_k^w& z_k^w\end{array}\right]$ 是地图中该特征在全局坐标下的最近点。

值得注意的是，良好的初始猜测对ICP来说至关重要。在开始阶段有两种初始化策略(感觉两种方法都不是特别方便，某些角度来说和描述子做融合是否更为合理？)：

1. 在地图上标出停车场的入口处。因此车辆直接在停车场入口处进行初始化；
2. 可以在进入地下停车场之前使用GPS作为初始位姿(需要初始位置和姿态)。车辆在地图上定位后，不再使用GPS。在初始化之后，使用里程计的预测作为初始猜测。

纹理区域的定位是准确的。虽然多路环视相机尽可能地增加了感知范围，但停车场中也存在一些极少纹理的区域。为了克服这一问题，最后采用了一种融合里程计和视觉定位结果的EKF框架。在该滤波器中，里程计用于预测，视觉定位结果用于更新。该滤波器不仅增加了系统的鲁棒性，而且平滑了估计的轨迹。

2.7 Parking Spot Detection

由于停车线和停车线角点是从IPM图像(Fig. 4(b) 中的白点和黄线)检测出来的，因此很容易自动检测停车点。角点被用来预测停车点的位置。如果停车线与预测的停车点匹配得很好，那么这个预测就被认为是正确的。停车点检测的结果如 Fig. 7 所示。这些停车点被标记在地图上，用于自动停车任务。