精度LiDAR-Monocular Visual Odometry

大佬讲解
代码解读和链接

论文题目：LIMO: Lidar-Monocular Visual Odometry
论文信息：2018IROS

Abstract

自动驾驶的高级功能在很大程度上取决于车辆的精确运动估计。但是，他们绝大多数都专注于双目成像或纯LIDAR测量。本文提出了一种从激光雷达测量数据中提取摄像机特征轨迹的深度提取算法，并通过基于鲁棒关键帧的束调整估计运动。语义标注用于地标离群点的剔除和加权。
基于LiDAR点云估计对应图片帧中特征点的深度。

LiDAR深度增强的单目VO。包含帧帧位姿估计和滑动窗口BA。

I. INTRODUCTION AND RELATED WORK

VSLAM是使用相机观测环境来同时估计环境的3D结构和相机自运动的问题。最常见的解决方法是BA。但是BA同时优化所有相机位姿和路标，消耗太大。为了实现实时的VSLAM，很多优化算法被提出。VSLAM结构如图所示。包括特征提取，特征预处理，帧帧运动估计，尺度估计，BA和回环检测。由于本文专注于LiDAR辅助的单目里程计，因此不包含回环检测。

常见的VSLAM解决方案依赖双目相机。但是基于双目相机的VSLAM需要准确的相机标定。Krevso等人[12]提出通过分解场学习双目相机的标定偏差。Geiger等人[7]提出了一种在平移估计中的标定误差补偿方法。相比之下，LiDAR不需要额外的外参标定，可以得到准确的点云深度。因此，我们想要组合LiDAR准确的深度估计和相机的强大特征跟踪能力。
我们提出一种从LiDAR中提取图片中特征点的深度的新方法。具体地，我们拒绝不满足局部平面假设的外点，并特殊考虑了地面点。为了达到实时表现，我们仔细设计了先验估计，路标选择和关键帧选择。本文中，我们没有使用任何LiDAR-SLAM算法，因为我们想推动LiDAR深度测量和VO组合的研究边界。

II. BLOCK A AND B: FEATURE EXTRACTION AND PREPROCESSING

• A. 特征提取和预处理
  特征提取包括特征跟踪和特征关联。特征跟踪使用viso2库中使用的方法（它包括非最大抑制，通过流动的异常值排除和子像素优化），在30-40ms内提取2000个特征对应。然后需要剔除动态的特征点。为此，我们先建立语义图片[21]，然后在每个特征点周围寻找语义属于动态的点的个数，如果大于一定阈值，则认为该特征点也属于动态点。

III. BLOCK S: SCALE ESTIMATION

为了估计尺度，需要从LiDAR中提取特征点的深度。在本工作中，我们使用单帧激光点云来获得特征点的深度。虽然这导致更少可用的点云数据由于没有使用累积点云，但是这也避免了对运动估计的依赖。

A. Approach

首先将LiDAR点云投影到对应的相机坐标系中，然后对每个特征点，执行如下步骤：
1）首先寻找该特征点周围的矩形框内的激光点。
2）然后对这些激光点按照深度进行划分。
3）寻找最靠近该特征点的深度区间的点云，拟合平面。
4）我们认为该特征点位于该平面上，根据光心和特征点的连线与平面的交点记为该特征点的深度。
5）检测估计深度的准确性：光心和特征点连线与平面的夹角必须小于某个阈值；拒绝深度高于30m的特征点以防外点。对于地面上的特征点进行特殊处理。
首先从LiDAR点云中提取地面，然后直接利用地面点云拟合平面，而不需要第2和第3步。

B. Selecting the Neighborhood

图中. 矩形框自适应大小。对于每个特征点（绿色），矩形框至少包含两根扫描线的激光点（红点），如中图所示；而不能只包含一根扫描线中的点。
为了提取特征点周围的局部平面：选择领域
有序节点（直接从点云中提取领域）
无序节点（使用LIDAR点在图像上的投影，并在特征点周围的图像平面中选择矩形大小）

C. Foreground Segmentation

基于深度划分的平面拟合。如果不事先将点云按照深度划分区间，拟合的平面可能出错（如左图所示）。点云按照深度划分区间后，减少了拟合错误平面的可能。

D. Plane Fit

从前景分割的点中，我们选择三个面积最大的三角形F∆来稳定估计。 如果F∆的面积太小，我们将不使用深度估计来避免错误估计深度。 然后，我们将平面拟合到F∆，用于深度估计

E. Special Case: Points on Ground Plane

对于与LIDAR垂直轴正交的平面，分割前景并拟合局部平面非常精确。相反，如图所示，由于LIDAR在垂直方向上的分辨率低于在水平方向上的分辨率，因此无法使用该方法估算地平面上的点的深度，

地面平面不能按照深度划分区间的方式提取，因此需要特别处理。
由于道路表面很少是平坦的表面，因此将视线与地平面相交是不准确的。 为了准确估计道路上特征点的深度，我们宁愿分割对应于地面的点而不是对应于前景的点。 我们通过与以前相同的步骤来估计特征点周围的局部平面，但是最小跨度为F∆。 通过仅接受位于地平面附近的局部平面，可以有效地提取异常值。

IV. BLOCK C: FRAME TO FRAME ODOMETRY

为了得到BA的初始化结果，我们首先执行帧间运动估计。由于具有精确的帧间运动，计算时间可以得到补偿，因为先验问题通常要解决的问题要小得多，并且用重构的地标选择地标更精确。
帧间里程计优化如下问题：
$$\underset{x,y,z,\alpha ,\beta ,\gamma }{\mathrm{argmin}}\sum _i{\rho }_{3d\to 2d}\left({\Vert {\phi }_{i,3d\to 2d}\Vert }_2^2\right)+{\rho }_{2d\to 2d}\left({\Vert {\phi }_{i,2d\to 2d}\Vert }_2^2\right)$$
其中 ${\phi }_{i,3d\to 2d}={\bar{p}}_i-\pi (p_i,P(x,y,z$