本文介绍两篇使用语义地图进行视觉定位的论文，两篇论文工程性很强，值得一学。

AVP-SLAM是一篇关于自动泊车的视觉定位方案，收录于 IROS 2020。论文链接为：https://arxiv.org/abs/2007.01813，视频链接为：https://zhuanlan.zhihu.com/p/157340737。

RoadMap是轻量级的室外语义地图视觉定位方案，收录于 ICRA2021。论文链接为：https://arxiv.org/abs/2106.02527，视频链接为：https://zhuanlan.zhihu.com/p/382183817。

---

1.AVP-SLAM

1.1 System Overview

自动泊车视觉定位方案如下图所示，使用到的传感器有4个环视摄像头，1个IMU，2个轮速编码器，轮速编码器和IMU组成里程计，可以据此得到车辆的相对位置。

整个方案分为两部分：

• 第一部分是建立全局语义地图，4个环视摄像头采集到的图像首先投影到鸟瞰图，然后合并成全方位图；神经网络检测语义特征，包括车道线、车位线、地面引导标志、减速带。结合里程计，将语义特征投影到全局坐标，同时使用回环检测最终得到全局图。
• 第二部分是定位，与建图相似，通过鸟瞰图提取语义特征然后与全局图进行匹配来定位。同时，使用EKF将视觉地位与里程计定位进行融合。
  

---

1.2 Methodology

1）全方位图合成

4个环视摄像头如图3所示，每个摄像头的内参和外参都已提前标定。图像中的像素投影到汽车中心坐标地面（$z=0$），投影方程为：
$$\frac{1}{\lambda }\left[\begin{array}{c}{x}^v\\ y^v\\ 1\end{array}\right]={\left[{\mathbf{R}}_c{\mathbf{t}}_c\right]}_{\mathrm{col}:1,2,4}^{-1}{\pi }_c^{-1}\left(\left[\begin{array}{l}u\\ v\\ 1\end{array}\right]\right)$$

其中 ${\pi }_c(.)$ 是摄像头投影模型，${\pi }_c(.{)}^{-1}$ 是逆投影。$[{\mathbf{R}}_t,{\mathbf{t}}_c]$ 是每个摄像头相对汽车中心的外参，$[u,v]$ 是像素坐标，$[x^v,y^v]$ 是对应在汽车中心坐标的像素位置。

逆投影之后，合成全方位图，如下图4所示。全方位图合成方程为：
$$\left[\begin{array}{c}{u}_{ipm}\\ v_{ipm}\\ 1\end{array}\right]={\mathbf{K}}_{ipm}\left[\begin{array}{c}{x}^v\\ y^v\\ 1\end{array}\right]$$

其中 ${\mathbf{K}}_{ipm}$ 是合成全方位图内参。

2）特征检测

下面就是特征检测，这里使用的分割网络为 U-Net，作者专门针对车库场景进行了训练，分割结果如图4所示。

---

3）建图

特征检测之后，有用的语义特征（车位线、引导标志、减速带）位置会转换到3D空间坐标，转换方程为：
$$\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}{l}{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$$

其中 $[{\mathbf{R}}_o,{\mathbf{t}}_o]$ 是从里程计得到的汽车位置，这样就得了局部地图。

由于里程计会随时间漂移，这里使用回环检测进行优化，如图5所示。两个局部地图使用ICP方法进行匹配，如果匹配成功就得到了两个局部图的相对位置，这些相对位置会用在全局位置图优化中。全局优化方程为：

$$\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}$$

其中 $\mathcal{X}=[{\mathbf{r}}_0,{\mathbf{t}}_0,...,{\mathbf{r}}_t,{\mathbf{t}}_t{]}^T$ 是 所有局部图的位置，${\mathbf{z}}_{t,t+1}^o$ 是从里程计得到的局部图 $t$ 和 $t+1$ 相对位置；${\mathbf{z}}_{i,j}^l$ 是回环帧对 $i,j$ 的相对位置。优化方法为高斯牛顿法。

4）定位

定位如图6所示，和建图类似，首先是检测语义特征，检测到的语义特征与全局图进行匹配，匹配方法为：
$${\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}{l}{x}_k^w\\ y_k^w\\ z_k^w\end{array}\right]\Vert }^2$$

其中 $\mathbf{r},\mathbf{t}$ 是当前帧的旋转和平移向量，$\mathcal{S}$ 是当前特征点集合，$[x_k^v,y_k^v,0]$ 是当前特征点在汽车坐标的位置，$[x_k^w,y_k^w,z_k^w]$ 是全局坐标系下与特征匹配的最近点。

在实际应用中，使用里程计进行位置预测，视觉定位结果进行更新。同时在特征检测时，还要检测停车位位置，这里通过检测停车位顶点和车位线位置来实现（如图7所示）。

---

1.3 Experimental Results

实验采集平台：摄像头采集频率为为30Hz，图像精度为 $1280\times 720$ pixels，图像分割处理频率为15Hz。为了评估建图精度，在室外场景中使用RTK-GPS作为真值，结果如表1所示，建图精度比纯里程计和ORB-SLAM2要高。

下面是召回率比较，为了公平比较，使用了里程计辅助ORB-SLAM2建图，结果如表2所示，可以看出，语义特征比几何特征定位更稳定。

然后是地图大小比较， 如表3所示，语义地图比传统描述子地图更高效。最后是定位误差，如表4所示，测量的是汽车中心与车位线之间实际距离以及地图上两者之间距离，两个距离之差为定位误差，总共进行了20次停车测试，可以看到定位误差达到了厘米级。

---

2.RoadMap

文方案探索的是使用轻量化的众包语义地图，取代传统的高精地图，实现视觉定位。

2.1 System Overview

如下图所示，定位方案包括三部分：车端建图、云端建图、终端定位。

• 配备有前视摄像头、RTK-GPS以及其它传感器的高端车辆（如Robo-taxi) 每天收集大量实时数据，通过分割网路从前视图像提取语义特征，然后根据优化的汽车位置将语义特征投影到世界坐标，同时将局部语义地图上传到云端；
• 云端服务器接收多个车辆上传的局部语义地图，合并成全局地图；然后进行地图压缩提取轮廓，接着将压缩的语义地图发送至终端车辆；
• 配置低成本传感器的终端车辆接收云端的语义地图，前视图像提取的语义特征与地图进行匹配定位。
  

---

2.2 Methodology

1）车端建图

首先是图像分割，如图3所示，使用车道线、停车线、道路标志来建立语义地图。

在使用RTK-GPS进行定位时会受到周围遮挡物的影响，因此这里还需要进行位置图优化，如图4和5所示，优化方程为：
$$\underset{{\mathbf{s}}_0\dots {\mathbf{s}}_n}{\min }\left\{\sum _{i\in [1,n]}{\Vert {\mathbf{r}}_o\left({\mathbf{s}}_{i-1},{\mathbf{s}}_i,{\hat{\mathbf{m}}}_{i-1,i}^o\right)\Vert }_{\mathbf{\sigma }}^2+\sum _{i\in \mathcal{G}}{\Vert {\mathbf{r}}_g\left({\mathbf{s}}_{\mathbf{i}},{\hat{\mathbf{m}}}_i^g\right)\Vert }_{\mathbf{\sigma }}^2\right\}$$

其中，$s$ 是位置状态量，${\mathbf{m}}_{i-1,i}^o$ 是里程计测量，${\mathbf{m}}_i^g$ 是GNSS测量，里程计和GNSS残差 ${\mathbf{r}}_o$ 和 ${\mathbf{r}}_g$ 为：
$$\begin{array}{rl}{\mathbf{r}}_o({\mathbf{s}}_{i-1},{\mathbf{s}}_i,{\hat{\mathbf{m}}}_{i-1,i}^o& =\left[\begin{array}{c}\mathbf{R}{\left({\mathbf{q}}_{i-1}\right)}^{-1}\left({\mathbf{p}}_i-{\mathbf{p}}_{i-1}\right)-\delta {\hat{\mathbf{p}}}_{i-1,i}^o\\ {\left[{\mathbf{q}}_i{}^{-1}\cdot {\mathbf{q}}_{i-1}\cdot \delta {\hat{\mathbf{q}}}_{i-1,i}^o\right]}_{xyz}\end{array}\right]\\ {\mathbf{r}}_g\left({\mathbf{s}}_{\mathbf{i}},{\hat{\mathbf{m}}}_i^g\right)& ={\mathbf{p}}_i-{\hat{\mathbf{m}}}_i^g\end{array}$$

最后是建立局部语义地图，与AVP-SLAM类似，即将语义特征根据优化的位置转换到全局坐标。

---

2）云端建图

云端将各个车辆收集来的地图进行合并，得到全局地图，同时提取出地图轮廓，然后再发送给其它量产车。

---

3）终端定位

首先是复原地图，这里是使用同样的语义标签填充轮廓，然后将标签像素投影到世界坐标。定位方法与AVP-SLAM一样，即：
$${\mathbf{q}}^{\ast },{\mathbf{p}}^{\ast }=\underset{\mathbf{q},\mathbf{p}}{\mathrm{argmin}}\sum _{k\in \mathcal{S}}{\Vert \mathbf{R}(\mathbf{q})\left[\begin{array}{c}{x}_k^v\\ y_k^v\\ 0\end{array}\right]+\mathbf{p}-\left[\begin{array}{c}{x}_k^w\\ y_k^w\\ z_k^w\end{array}\right]\Vert }^2$$

---

2.3 Experimental Results

下面是定位精度比较，这里RTK-GPS做真值，可以看出其定位精度比激光雷达还要准。