提出一种基于单目视觉的定位方法，在城市环境中使用道路标记进行定位。相对与特征点法道路标记对时间，季节，视角和光照变化更稳健。定位问题的本质是匹配传感器的观测与已知的先验地图。提出来一种在3D Lidar map中的单目相机定位方法。
- 提出语义车道标记定位，避免特征点局限
- 提出单目相机在3D Lidar Map中的定位方法
- 提出几何形状匹配方法，而不是光度匹配。
- 在图像空间使用Chamfer Distance 方法匹配形状
主要由三个误差函数用来定位，分别是倒角匹配，对极约束和里程计约束。
倒角匹配：
\[
\mathcal{C}_{\mathrm{chf}}\left(\mathbf{P}_{k}\right)=\sum_{\mathbf{X} \in \mathcal{M}_{k}} \mathbf{C}_{k}\left(\pi\left(\mathbf{P}_{k}, \mathbf{X}\right)\right)
\]
倒角匹配使用距离变换方法进行形状对齐。先将地图投影到图像平面，再通过倒角匹配方法匹配。
对极约束：
对于两帧图像特征点，对极约束表示为：
\[
\tilde{\mathbf{x}}_{i, k-1}^{T} \mathbf{F} \tilde{\mathbf{x}}_{i, k}=0
\]
对于已知位姿，fundamental 矩阵为:
\[
\mathbf{F}=K^{-T}\left[{ }^{k-1} \mathbf{t}_{k}\right]_{\times}{ }^{k-1} \mathbf{R}_{k} K^{-1}
\]
因此对于点对，对极约束可以表示为：
\[
\mathcal{C}_{\text {epi }}\left(\mathbf{P}_{k-1}, \mathbf{P}_{k}\right)=\sum_{i} \tilde{\mathbf{x}}_{i, k-1}^{T} \mathbf{F} \tilde{\mathbf{x}}_{i, k}
\]
里程计约束
两帧之间里程计具有观测，可以进行位姿约束：
\[
\mathcal{C}_{\text {odm }}\left(\mathbf{P}_{k-1}, \mathbf{P}_{k}\right)=\left(d_{k}-\left.\right|^{k-1} \mathbf{t}_{k} \mid\right)^{2}
\]
因此约束方程为：
\[
\mathcal{C}\left(\mathbf{P}_{k}\right)=\mathcal{C}_{\text {chf }}\left(\mathbf{P}_{k}\right)+\mathcal{C}_{\text {epi }}\left(\mathbf{P}_{k-1}, \mathbf{P}_{k}\right)+\mathcal{C}_{\text {odm }}\left(\mathbf{P}_{k-1}, \mathbf{P}_{k}\right)
\]
使用非线性优化方法进行求解。
Reference
[1] Monocular Localization in Urban Environments using Road Markings [2] http://datasets.honda-ri.com/roadmark