Zhou, Yuxuan, Xingxing Li, Shengyu Li, and Xuanbin Wang. “Visual Mapping and Localization System Based on Compact Instance-Level Road Markings With Spatial Uncertainty.” IEEE Robotics and Automation Letters 7, no. 4 (October 2022): 10802–9. https://doi.org/10.1109/LRA.2022.3196470.
3 System Overview
Fig .1是本系统的基本框架，在本系统中汽车被假设为拥有基本的基于GNSS 的全局定位能力，以及基于轮式里程计或视觉-惯性里程计（VIO）的局部导航能力。对于基于视觉的环境感知，制图与定位阶段共享产生语义道路标志的pipeline。对于位姿估计，一个统一的位姿图优化框架被用于制图和定位阶段，将全局定位、局部导航以及地图匹配（限于终端用户）信息结合在一起实现鲁棒准确的定位。
4 Semantic Road Marking Extraction
作者采用一个在Appoloscape 上预训练的语义分割模型，并在自采数据（在武汉市采集的227张图片）上进行细调，采用了以下分割种类：车道线（虚线）、车道线（实线）、指引标志（箭头或菱形）、斑马线、停止线及其他。值得注意的是，道路标志的语义分割可以被集成到车辆系统的感知模块内，不必将其视为一个独立模块。
进行语义分割之后，使用IPM 模型来恢复路标的公制几何信息，流程如Fig. 2所示，经过转换，可以将原始图片中的道路像素转换到相机坐标系中的3D点或带有公制几何信息的IPM 图片中的点。
5 Modelling the Instance-level Road Markings
5.1 Parameterization of the Road Markings
在第4章提到的分割种类中，虽然斑马线形式非常独特，但是由于计算复杂度的问题，其不被用于制图和定位过程。对于其他种类，作者将其分为两种类型：
- 块状patch-like 路标：车道线（虚线），指引标志（箭头、菱形）；
- 线状路标：车道线（实线），停止线。
对于块状路标，经过像素聚类pixel clustering之后，语义点云的中心、主方向以及特征值eigenvalue 通过奇异值分解（SVD）计算得到。
- 中心被视为路标的参考位置；
- 主方向表示路标的方向，可以用于数据关联data association，以及估计汽车的朝向；
- 特征值是以一种高压缩方式表征路标的形状，类似于bounding box，但是由于其考虑点云中的每一个像素，所以对于边缘噪声更不敏感。
对于线状路标，它们被参数化一条包含有任意数量线段的线带：
after pixel-level clustering, the point cloud of the road marking is fitted using a cubic polynomial along the principal direction.
采样点使用特定的间隔（如0.5m）从曲线上获取，线段直接表征着线状路标的局部朝向以及形状，可被用来进行数据关联。
使用上述的参数化之后，原始点云数据即可被舍弃，本参数化方法以损失一些细节局部几何信息为代价，实现了更为紧凑的参数化，而且使得进一步的复杂处理成为可能，如考虑地图中元素的概率属性或者地图匹配过程中的高效迭代。
5.2 Modelling the Uncertainty
由于相机模型的透视本质，其不能像LiDAR一样直接获取环境的准确测距信息；通过IPM 转换可获取周围道路元素的3D 几何信息，但是其准确性对于像素误差、道路不规则以及汽车的姿势改变较为敏感。因此，为了概率制图与定位，进而在复杂环境中获取足够的精确度，需要对IPM 生成元素的空间不确定性进行研究。
5.2.1 Pixel error
像素误差可以由相机模型和语义分割误差造成。
5.2.2 Pitch angle error
在IPM 转换中，作者做了虚拟相机坐标系与地面平行的假设，但由于道路不规则以及车辆的加速刹车等原因，汽车的俯仰角相对于地面会有改变，导致上述假设失效，造成相应的误差。
5.2.3 Road height error
作者假设的虚拟相机坐标系距离地面的高度可能存在误差。
本文中，上述误差因子被假设服从0均值的高斯分布。在给定0.1°的俯仰角误差，0.05m的高度误差以及2像素误差时，IPM 中点的横向与纵向不确定度如Fig .4所示，可以发现：
- 感知距离越近，不确定度越小；
- 纵向不确定度要远大于横向不确定度，已经达到分米级，不可被忽略。
当把道路元素添加至全局地图中后，汽车位姿估计的不确定度也需要被考虑。
5.3 Dealing With Complex Road Conditions
只要上述提到的误差因子较小且服从相关假设，IPM 转换得到的路标概率属性可以通过不确定模型得到处理。然而，现实世界中经常出现与上述假设不符的情况（如Fig. 5所示），导致IPM 产生的系统误差可能会超过米级，不再适用于本文提出的不确定模型，因此需要采取具体措施来解决这些情况。
5.4 Data Association
尽管路标几何信息的大部分细节已经被舍弃了，但受益于路标的规则离散分布，仍然可以进行有效的数据关联。一般将路标的语义类别、参考位置、朝向以及特征值用于数据关联。
在定位阶段，将感知到的路标投影至地图坐标系来寻找最近的匹配，由于作者对路标进行了最小的参数化，方便进行高效的重复搜索来保持多个路标观测的最好一致性。
6 Pose Graph Optimization
本架构在制图与定位阶段都使用了位姿图优化，如Fig. 7所示
Patch-toPatch Factor
将相机感知到的块状路标与全局地图中的元素进行联系，构建一个简单的点对点约束：
式中，$\tilde{p}_{p_j}^{b_k}$ 表示利用IPM 映射产生的路标观测位置， $p_{p_j}^{b_k}$ 表示地图中元素在汽车坐标系中的位置。
Line-to-Line Factor
对于相机感知到的线状路标，其中的采样点与地图中对应线段的距离被作为构建因子：
式中，$\tilde{p}_{l_j, m}^{b_k}$ 表示IPM 产生的线状路标中一个采样点的观测位置， $p_{l_j, z}^{b_k}, p_{l_j, z+1}^{b_k}$ 表示线状地图元素的对应线段在汽车坐标系中的两个顶点。
添加完Fig. 7所示的不同因子之后，即可构建位姿图优化问题来获取汽车位姿的最优估计。值得注意的是，作者在当前系统的部署过程中，为了简化制图过程，地图匹配（patch-to-patch，line-to-line）因子没有被考虑。在制图阶段，需要维护一个全局的位姿图；而对于定位阶段，只需要维持一个几秒的窗口来保证高效性。
在地图辅助定位中，路标的不确定性用来决定地图匹配因子的权重，但是地图中的元素不会进行相应的更新，因为终端用户无权进行地图更新。
7 Experiment
作者在10月13日采集两组数据序列（A1，A2），进行融合制图，采用RTK/INS（战术级INS）的平滑解作为真值；在11月28日采集了用来进行评估的数据序列（U1），如Fig. 9所示，值得注意的是，U1数据序列的采集汽车方向与A1、A2相反。
7.1 On-Vehicle Mapping
在制图过程中，参数化的路标被添加进全局地图中，并基于空间不确定性进行聚集与融合，如Fig. 10所示。
7.2 Multi-Source Map Merging
在数据序列A1与A2分别进行on-vehicle mapping之后，两个地图会进行融合，进而得到一个一致且相对完整的道路地图，如Fig. 11所示。
7.3 Map-Aided Localization
终端汽车中的车载GNSS 利用standard point positioning (SPP) 来实现米级的全局定位，图片与IMU 数据用来进行基于VIO 的局部导航，结合生成的道路地图进行匹配定位，最终利用上述的所有信息进行位姿图优化得到最终的位姿估计。值得注意的是，本架构没有使用边缘化marginalization策略。
考虑到路标的模糊属性，汽车需要观测到足够多的可区分信息才能实现成功的重定位，如Fig. 13所示，
- 由于指示路标（蓝色）更稀疏、更具有区分性，所以它们在地图匹配的第一阶段被使用，以保证在非常有限个候选匹配点对中进行高效匹配；
- 在经过迭代汽车位姿更为精准后，使用更多的观测路标进行地图匹配，以检查地图匹配的一致性并提高定位精度。
本方法与使用稠密点云和iterative closest point (ICP) 方法相比更高效、更灵活，此外，本方法良好的建模空间不确定度有潜力实现更高的定位精度。
