AVP-SLAM论文阅读笔记

这篇文章秦通大神今年新发的论文《AVP-SLAM: Semantic Visual Mapping and Localization for Autonomous Vehicles in the Parking Lot》

主要使用的特征有：车道线，停车线，路面指示标记等语义信息。
算法主要分为两个模块：地图构建与视觉定位。

地图构建

IPM

输入的图像分别来自前视、后视、两个侧视，由于需要检测的语义特征基本都位于路面上，作者这里采样了逆投影变换，将来自四个广角摄像头的图像合成一张IPM图像。

${\pi }^{-1}$ 是逆投影变换，omni模型和我们常用的pinhole模型不太一样，具体投影关系可以参考这篇论文：
Omnidirectional Camera
$[R_c,t_c]$是相机与车体之间的外参，可以将车体坐标系内三维点变换到相机坐标系，取逆就是把相机坐标系变换到车体坐标系。我还是觉得用$T_{body\_camera}$这样的符号定义更直观一些，要不然总记不清是谁到谁。
下一步是将四个IPM图像合成一个IPM图像，效果图如下：


IPM变换其实可以理解为有一个虚拟相机从车上面朝下看。

特征提取

这一步实际上就是和传统的语义分割没啥区别，作者使用了U-Net，对合成IPM图像的每一个像素输出一个标签，标签主要有lanes, parking lines, guide signs, speed bumps, free space, obstacles, and walls。看看效果吧

局部建图

在本文中，作者每30米建一个小的地图，其实就是把原来在车体坐标系下的语义特征变换到世界坐标系。这个变换不是算的，是假设系统里还有另外一个里程计会实时的输出车体的位姿。看公式吧：

这里$K_{ipm}$是我们从上往下看虚拟相机的内参，没啥好说的，把像素点变换到相机平面。下一个公式就比较有意思了

这里其实是假设所有的语义特征都处于一个平面上（路面），举一个不恰当的例子，假设你现在只睁左眼，低头往下看，你知道自己多高，也知道你自己眼睛的内参，以你脚底板为原点建立坐标系，向前为x轴，向右为y轴，向上为z轴，即使是一个眼睛，你也可以知道你脚下所有物体在这个坐标系上的坐标，当前这里必须满足一个前提，就是所有物体都在一个平面上。和相机标定过程类似，相机标定的时候所有标定板上的角点z不是也设置为0。
这个公式里$[R_O,t_o]$是从其他里程计里获取的车体相对于世界坐标系的位姿。

回环检测和全局建图

单纯只靠局部地图，最后建成的地图一部分路标可能会有重叠，作者使用ICP计算每个局部地图和周围的局部地图之间的相对变换，然后使用pose graph做全局优化，需要优化的变量是每个局部地图的位姿，主要有两种边，第一种是二元边，残差是两个局部地图的相对位姿和回环检测出来的相对位姿做差；第二种是一元边，是每个局部地图的位姿和前面提到$R_o$的差，这个一元边主要是保证优化后的位姿和里程计的位姿差距别太大。

语义定位

首先还是IPM和语义分割，然后是通过ICP找到当前这个局部地图和前节构建好的全局地图做匹配，可以得到当前车辆相对于地图的位姿，作者提出了两个初始化的方案，第一种方案是在地图上标注停车场的位置，这样在进入停车场的时候，直接用入口位置做初始化；第二种用GPS做初始化。初始化好了之后，就直接使用里程计的输出做初始化。
由于有时候可能看到的特征比较少，这里作者提出使用EKF把这个语义定位的位姿和里程计的位姿做融合，这样还能保证输出的位姿保持平滑。

参考

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