[论文阅读]Road Mapping and Localization using Sparse Semantic Visual Features

IEEE ROBOTICS AND AUTOMATION LETTERS 2021 文章链接

速读

1.1 论文试图解决什么问题？这是否是一个新的问题？
将道路中的语义信息用于解决自动驾驶定位这一问题。不是新问题。
1.2 有哪些相关研究？如何归类？谁是这一课题在领域内值得关注的研究员？
相关研究提及较少
1.3 文章的贡献是什么？
提出了一个提取、建模优化语义道路元素的视觉SLAM方法，结合深度学习模型提取语义。
①一个深度学习模块用于语义信息提取；
②道路语义的参数化（用关键点对道路线和路标建模、用分段三次样条曲线参数化车道线）并用其设计了相关的loss function
③用这些信息建立了完整的SLAM系统；
1.4 文章解决方案的关键是什么？
基于特征点和语义信息将SLAM中使用的信息从像素级向物体级扩展
1.5 实验如何设计？实验结果足够论证其效果吗？
①网络的训练上通过人工标注数据集中的图片作为训练数据；
②训练网络的对三种语义目标的分类准确率、召回率、像素误差做实验测试；
定位实验：一个基于类似特征点描述的传统方法作为baseline，尽量保持两个方法条件一致，不相同的地方做出了声明，主要比较绝对轨迹的translational root-mean-squared-error (T.RMSE)
③Self-Relocalization：地图大小和T.RMSE两个维度上的比较说明语义地图占用更小空间的情况下由高质量在线定位效果；
④Cross-Relocalization:还添加了一个baseline：Maplab。所有数据完成定位，鲁棒性好、准确性好、轻量
⑤Modular and Efficiency Analysis：更细节的比较，loose tracking和tight tracking两种情况的运行实验，三种语义对象使用的特征点数量实验，各个部分使用时间说明；
⑥Stability of Semantic Mapping：建图阶段的位姿结果实验；
1.6 数据集是什么？
两个真实的数据集：
一个公开的数据集KAIST：使用26, 28, 38, 39四条城市环境的左相机+IMU+GPS数据；
一个自己采集的数据集：于杭州采集共两条，设备为10Hz ON AR0144 stereo camera, a 200Hz Bosch BMI 088 IMU and a 10Hz u-blox F9 RTK-GPS，一条长4.7km一条长3.2km
1.7 还会存在什么问题
①好像没有对路口的建模

②还有那些语义信息可以使用
③对极端天气情况下，语义信息能够雨天下的有效定位（视觉上的模糊）、雪天下的定位（地面上语义信息消失）

---

以下是精读部分

2 系统框架

与使用类似框架的已有结果不同的是作者使用多种语义信息

框架分为离线构建语义地图+在线定位，具有批处理和滑动窗口策略的紧耦合状态优化；
语义实例被分为三类：地面上的物体（路灯、路标）、地面元素（左转右转标记）、线（车道线）

3 各部分功能

3.1 Selection of Road Features

被选定为语义标志的元素：
①道路上方的路灯和交通标志，稳定且足够被前放相机捕获；
②路面上的醒目标志，占据画面大部分的路面信息，虽然大多时候被遮挡；
实线和虚线车道线，实线提供方向约束，虚线矩形块的角点作为一系列点标记；

3.2 Detection of Road Features

两步的检测模块：
①物体级的检测得到道路上方和路面上的语义信息框，以及车道轮廓上的采样像素
②沿着虚线车道检测虚线车道的角点

为了减少前端的重复工作使用无锚检测方法CenterNet（an anchor free detection method CenterNet）

使用DLA-34和DCN module作为特征提取部分。后面这一部分没看懂，好像是设计不同的loss function得到各种具有不同意义的H，然后通过对H的聚类得到属于哪一类语义信息？
①Object boxes and keypoints：在CenterNet的基础基础上提出六个head用于检测这类语义；
②Lanes：$H_7$输出左右边界点和上下边界点，$H_8$是个左右边界点 or 上下边界点 or 都不是的三分类，$H_9$用于降低位置特征的维度，最后通过DBSCAN聚类；
Dashed lane corners：给定64*64的图像检测角点练成线段表示虚线块，$H_{10}$计算角度上的differences，$H_{11}$计算长度上的differences；

3.3 Feature Tracking for Semantic Entities

对于地面上的标志和车道线等地面物体ground objects追踪，在IMU积分得到的位姿$T^{\prime }$下的重投影误差如下，相较于常见的追踪多了一个有趣的约束:
$$p^{\prime }=\pi (T^{\prime }\cdot {\pi }^{\prime }(p,z_p)),withG\cdot {\pi }^{\prime }(p,z_p)=0$$表示特征点是在某个确定平面上的，其中：
① ${\pi }^{\prime }(p,z_p)$表示像素点反投影到世界坐标系下的坐标
② $T^{\prime }$表示由IMU积分得到的两帧间相对位姿
③ $G=[\theta ,\varphi ,d]$表示一个世界坐标系下的平面，其物理意义为ground objects像素点所在的地面，对于$G$上面的点$x$有：
$$G\cdot x\triangleq [cos\theta cos\varphi ,cos\theta sin\varphi ,sin\theta {]}^{T}x+d=0$$如何匹配：
使用Hungarian matching strategy，对像素进行物体级（instance-wise）和像素级（pixel-wise）的匹配
① instance-wise：在规则物体用多边形面表示和车道线用5像素宽的线段表示，计算交集intersection-over-union （ IOU ）
② pixel-wise：计算重投影误差
IOU比例应>50% + 重投影误差应<5

对于垂直平面上的路灯等vertical objects，使用光流法跟踪由GFTT extractor和FREAK descriptor提取的特征点来检测这类语义目标，由于语义检测得到的目标一般在空中，所以检测框内会包含目标之外的其他特征点，需要做离群点筛选

3.4 Representation and Initialization of Road Lanes

控制点到模拟车道线
用一系列控制点构造的分段三次Catmull-Rom样条曲线（the piecewise cubic Catmull-Rom spline curves）表示车道线的左右轮廓。分段三次Catmull-Rom样条曲线就是用四个点$C_{k-1},C_k,C_{k+1},C_{k+2}$模拟一段由$C_k$到$C_{k+1}$的曲线，其中$C_{k-1}$和$C_{k+2}$控制曲线两端点的朝向，具体公式如下：
$$C(t^{\prime })={\left[\begin{array}{c}{t}^{\prime 3}\\ t^{\prime 2}\\ t^{\prime }\\ 1\end{array}\right]}^T\left[\begin{array}{cccc}-\tau & 2-\tau & \tau -2& \tau \\ 2\tau & \tau -3& 3-2\tau & -\tau \\ -\tau & 0& \tau & 0\\ 0& 1& 0& 0\end{array}\right]{\left[\begin{array}{c}{C}_{k-1}\\ C_k\\ C_{k+1}\\ C_{k+2}\end{array}\right]}^T$$
其中$\tau =0.5$表示曲线的在控制点间的弯曲程度，$t^{\prime }\in [0,1]$表示$C_k$到$C_{k+1}$的某一个中间位置。

由此我们即可将$C_k,k\in [1,M+2]$个控制点分成$M-1$组，并用其描述出经过$C_k,k\in [2,M+1]$这些控制点且两端点方向由$C_1$和$C_{M+2}$控制的曲线。

控制点的生成
首先使用3得到一系列车道线上的点组成的点集$U_{c,j}\{P_{cj}\}$，然后从中随机取N个样本点组成点集$C_k,k\in [2,N+1]$，用这个点集构造样条曲线$C(t^{\prime })$模拟车道线，误差为：
$$\begin{array}{rl}& \underset{C_k}{\mathrm{argmin}}\sum _{c,j}||P_{cj}-C(t_{cj})|{|}_{x_{\Omega }}^2+\sum _{k=3}^{N+1}|||C_k-C_{k-1}|-{\lambda }_1|{|}_{y_{\Omega }}^2\\ & with<C_1,{\beta }_1>\leftarrow \{\begin{array}{r}|C_2-C_1|=|C_3-C_2|,\\ {\beta }_1\cdot N_2=\tau (C_3-C_1)\end{array}\end{array}$$
①左边项表示拟合的曲线和观测到的曲线上的点构成的误差，其中$t_{cj}$是通过求解式子$C^{\prime }(t_{cj})(C(t_{cj}-P_{cj}))=0$得到；
②右边项表示正则项，保证均匀采样控制点，即两个控制点间距离最好相差${\lambda }_1$；
with项用于初始化确定曲线初始方向的线外点$C_1$。$|C_2-C_1|=|C_3-C_2|$表示$C_1$在以$C_2$为圆心，$|C_3-C_2|$为半径的球面上；第二个式子没有看懂；如果$C_1$有多个结果的话选择和$C_3$在反方向的；
④点$C_{N+2}$的确定和类似；

车道线的生成步骤
①随机选择一个初始点；
②递归地在点的两边$[0.5{\lambda }_1,1.5{\lambda }_1]$内随机选取点；
为每个曲线生成最多500组点集，选取各个点集生成的候选曲线中误差项最小的那条

3.5 State Estimator Design

符号	意义	符号	意义
a	地面或垂直物体	i	物体a在c第几个像素，表示为$(\cdot {)}_{a^i}^c$
b	曲线	k	曲线b的第几个控制点，表示为$(\cdot {)}_{b^k}$
c	照片帧	m	曲线b在c的第几个采样点，表示为$(\cdot {)}_{b^m}^c$
j	$j\in \{m,n\}$	n	曲线b的角点，表示为$(\cdot {)}_{b^n}^c$

提出5种优化变量
①关键点i在地面或者直立的物体a中的3D位置$P_{a^i}\in {\mathbb{R}}_3$；
②3中提到的平面表示$G$；
直立物体在全局坐标系下的垂直平面表示为$V_a(\phi ,e)\cdot x\triangleq [cos\phi ,sin\phi ,0{]}^{T}x+e=0$，其中$\phi \in [0,2\pi ],e\in {\mathbb{R}}_3$，它相较于2少一个维度；
④曲线b的控制点$C_{b^k}\in {\mathbb{R}}_3$；
⑤曲线b上被帧c观测的像素点$p_{b_j}^c$相关的一个动态参数$t_{b^j}^c\in \mathbb{R}$，表示曲线上哪个地方（上面有提到），其作用为确保曲线投影的正确性；

五种变量和原相机位姿$T_c$构成三种约束：

①Points observation factors：常规的重投影误差
$${}^1{f}_{a_i}^c=||p_{a^i}^c-\pi (T_c^{-1}\cdot P_{a^i})|{|}_{{}^1{\Omega }_{a_i}^c}^2$$②Spline observation factors：曲线观测误差，即曲线上的边沿的点和角点$p_{b_j}^c$的投影误差
$${}^2{f}_{b_j}^c=||p_{b^j}^c-\pi (T_c^{-1}\cdot C_b(t_{b_j}^c))|{|}_{{}^2{\Omega }_{b_j}^c}^2$$Coplanar prior factors：平面先验残差，${}^3{f}_{a^i}$为水平平面，${}^3{f}_x$为竖直平面，点在平面上的话残差项应该为0
$$\begin{array}{rl}& {}^3{f}_{a^i}=||V_a\cdot P_{a^i}|{|}_{{}^3{\Omega }_{a^i}}^2,\\ & {}^3{f}_x=||G\cdot (T_c^{-1}\cdot P_x^c)|{|}_{{}^3{\Omega }_x^c}^2,x\in \{a^i,b^j\}\end{array}$$对式子中三个$\Omega$的详细意义和具体使用没有整理清楚

状态估计步骤
①初始化优化变量（这里理解不一定对，得对照代码）：
1)用GNSS-VIO轨迹中恢复的位姿三角化特征点；
2)将三角化后的地面点转化到观测帧，做3D平面拟合；
3)用已有点对垂直语义对象做2D曲线拟合，得到$V_a(\phi ,e)$，然后用于对检测的2D box中的特征点的检测；
4)有了2)后用3计算车道线；
5)求解样条4)的过程中将求解得到的3D association parameter分配给$t_{b^j}^c$，然后使用独立的非线性优化refine。(后续具体描述没有看懂)
②离线建图：
所有变量都初始化后，用推导的视觉惯导里程计因子图优化，所有的关键帧和检测到的优化变量参与最终的BA优化
在线定位：固定上面①和②式子中的语义要素（语义上的3D点和样条曲线的控制点等），以此来对定位增加约束，实现语义信息在定位中的使用。因为③式是用构建语义时的共面约束，在定位时不再使用。

3.6 Re-Identification and Feature Merging

Re-Identification：使用3D到3D的语义关联而不是词袋，因为这类信息是稀疏的且在外观上难以辨别。将质心小于5m的三角化物体认为是用一个物体并使用Hungarian strategy合并。对于伴随的GFTT点，使用FREAK描述子在多帧间投票，使用union-find algorithm合并再进行全局状态优化

3.7 Data Structure of Semantic Maps

语义地图$\mathcal{M}$=set(观测帧$\mathcal{O}$，语义路标$\mathcal{L}$，共视图$\mathcal{G}$(连接$\mathcal{O}$和$\mathcal{L}$))；
观测$c$保存全局位姿$T_c$和GNSS测量值；
垂直的语义路标保存语义信息、点的3D位置
不存储描述子

3.8 Localization Based on Semantic Maps

四个阶段
①map uninitialized：地图到当前位姿的转换为止$T_{\mathcal{M}}$，用GPS得到一小段距离（小于30m）的粗略位置信息，得到观测帧${\mathcal{O}}_{\mathcal{M}}\subset \mathcal{O}$，并转入②阶段；
②local search：获得标签${\mathcal{L}}_{\mathcal{M}}=\mathcal{G}({\mathcal{O}}_{\mathcal{M}})$，建立2D-3D的PnP-Ransac，初始化$T_{\mathcal{M}}$并转入④阶段；
④loose tracking和tight tracking：两个tracking阶段设定的阈值不同，都往滑动窗口添加reconstructed associations，并用5中的①和②优化，初始位姿$T_c$转化为$T_{c\mathcal{M}}$和$T_{\mathcal{M}}$两个变量来优化。将所有map 元素投影到当前帧，用一个准则确定投影关系
$${}^y{f}_x^c<||{\sigma }_2|{|}_{{}^y{\Omega }_x^c}^2,x\in \{a^i,b^j\},y\in \{1,2\}$$阈值$\sigma$在loose和tight中对deep特征点和classical 特征点的对应值是不同的