1. INTRODUCTION

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

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

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

该系统采用：

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

2. METHODOLOGY

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

2.1 IPM Image

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

2.2 Feature Detection

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

2.3 Local Mapping

鸟瞰图的坐标与三维空间对应：
[ x v y v 1 ] = K i p m − 1 [ u i p m v i p m 1 ] \left[\begin{array}{c} x^v \\ y^v \\ 1 \end{array}\right]=\mathbf{K}_{i p m}^{-1}\left[\begin{array}{c} u_{i p m} \\ v_{i p m} \\ 1 \end{array}\right] xvyv1 =Kipm−1 uipmvipm1

这时候获得的是相对车身中心坐标，再转到世界坐标系：
[ x w y w z w ] = R o [ x v y v 0 ] + t o \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 xwywzw =Ro xvyv0 +to其中 [ R o t o ] \left[\begin{array}{ll}\mathbf{R}_{\mathrm{o}} & \mathbf{t}_o\end{array}\right] [Roto] 是从里程计内得到的位姿，这些点聚合到一个local map内(如果算力足够，应该将里程计得到的位姿作为初值，如前面得到的本次的local map做个2D的ICP是否更合适？)每隔 30 m 30m 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的位姿，包含三个旋转轴： r = [ r x r y r z ] T \mathbf{r}=\left[\begin{array}{lll}r_x & r_y & r_z\end{array}\right]^T r=[rxryrz]T 和三个平移量： t = [ t x t y t z ] T \mathbf{t}=\left[\begin{array}{lll}t_x & t_y & t_z\end{array}\right]^T t=[txtytz]T。

有两种边：

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

位姿图优化可以写成以下 cost function：
X ∗ = arg ⁡ min ⁡ X ∑ t ∥ f ( r t + 1 , t t + 1 , r t , t t ) − z t , t + 1 o ∥ 2 + ∑ i , j ∈ L ∥ f ( r i , t i , r j , t j ) − z i , j l ∥ 2 \begin{gathered} \mathcal{X}^*=\underset{\mathcal{X}}{\arg \min } \sum_t\left\|f\left(\mathbf{r}_{t+1}, \mathbf{t}_{t+1}, \mathbf{r}_t, \mathbf{t}_t\right)-\mathbf{z}_{t, t+1}^o\right\|^2 \\ +\sum_{i, j \in \mathcal{L}}\left\|f\left(\mathbf{r}_i, \mathbf{t}_i, \mathbf{r}_j, \mathbf{t}_j\right)-\mathbf{z}_{i, j}^l\right\|^2 \end{gathered} X∗=Xargmint∑ f(rt+1,tt+1,rt,tt)−zt,t+1o 2+i,j∈L∑ f(ri,ti,rj,tj)−zi,jl 2其中：
3. X = [ r 0 , t 0 , … , r t , t t ] T \mathcal{X}=\left[\mathbf{r}_0, \mathbf{t}_0, \ldots, \mathbf{r}_t, \mathbf{t}_t\right]^T X=[r0,t0,…,rt,tt]T 是所有 local map 的位姿；
4. z t , t + 1 o \mathbf{z}_{t, t+1}^o zt,t+1o 是从里程计得到的 t t t 与 t + 1 t+1 t+1 间的相对位姿；
5. L \mathcal{L} L 是所有的回环匹配对；
6. z i , j l \mathbf{z}_{i, j}^l zi,jl 是通过ICP的回环得到的回环帧 i i i 与 j j j 之间的相对位姿；
7. 函数 f ( ⋅ ) f(\cdot) f(⋅) 表示两个 local map 间的相对位姿。

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

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

2.6 Localization

基于上面生成的语义地图，再次来到这个停车场时，车辆可以进行定位。与建图过程类似，四个环视图像拼成顶视图后提取语义信息。然后通过将当前特征点与地图进行匹配来估计车辆的当前位姿，如图6所示。估计采用了ICP法，可以写成以下公式：
r ∗ , t ∗ = arg ⁡ min ⁡ r , t ∑ k ∈ S ∥ R ( r ) [ x k v y k v 0 ] + t − [ x k w y k w z k w ] ∥ 2 \mathbf{r}^*, \mathbf{t}^*=\underset{\mathbf{r}, \mathbf{t}}{\arg \min } \sum_{k \in \mathcal{S}}\left\|\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]\right\|^2 r∗,t∗=r,targmink∈S∑ R(r) xkvykv0 +t− xkwykwzkw 2其中：

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

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

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

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

2.7 Parking Spot Detection

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

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

【论文阅读】AVP-SLAM Semantic Visual Mapping and Localization for Autonomous Vehicles in the Parking Lot
文章:AVP-SLAM: Semantic Visual Mapping and Localization for Autonomous Vehicles in the Parking Lot 作者: ...
论文《FLAME:Feature-Likeilhood Based Mapping and Localization for Autonomous Vehicles》阅读总结
摘要准确的车辆定位是自动驾驶导航最关键也是最基本的任务.虽然基于密集3D点云的地图可以进行准确地定位,但是在城市规模的环境中使用时,它们会带来巨大的存储负担和传输负担. 在本文中,我们提出了一种用于 ...
二十五.SLAM中Mapping和Localization区别和思考
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
【论文阅读】RoadMap: A Light-Weight Semantic Map for Visual Localization towards Autonomous Driving
文章:RoadMap: A Light-Weight Semantic Map for Visual Localization towards Autonomous Driving 作者:Tong Q ...
【论文精读】Hybrid Bird‘s-Eye Edge Based Semantic Visual SLAM for Automated Valet Parking
[论文精读]Hybrid Bird's-Eye Edge Based Semantic Visual SLAM for Automated Valet Parking 论文出处 ICRA2021 摘要 ...
论文阅读：（三）DS-SLAM: A Semantic Visual SLAM towards Dynamic Environments
之前看了有关语义分割的SLAM,最近调研下动态场景下基于动态实例剔除的论文. DS-SLAM: A Semantic Visual SLAM towards Dynamic Environments- ...
Semi-supervised Semantic Segmentation with Error Localization Network(基于误差定位网络的半监督语义分割 )
Semi-supervised Semantic Segmentation with Error Localization Network(基于误差定位网络的半监督语义分割 ) Abstract 本文 ...
【论文笔记】Map-Based Localization Method for Autonomous Vehicles Using 3D-LIDAR
[论文笔记]Map-Based Localization Method for Autonomous Vehicles Using 3D-LIDAR ~~~ ~~~~ 精确和稳健的定位是复杂 ...
Semantic Visual Simultaneous Localization and Mapping A Survey
基于视觉传感器的SLAM,又称视觉SLAM(visual SLAM),因其硬件成本低.小场景精度高.能获取丰富的环境信息等优点,近年来成为热门的研究方向. VSLAM的缺点也很明显.一方面,在应对光线 ...

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