【论文笔记】LiDAR and Camera Calibration using Motion Estimated by Sensor Fusion Odometry

一.介绍
传感器融合在机器人和计算机视觉领域得到了广泛的研究。与单传感器系统相比，多传感器融合系统可以完成更高层次的任务。该系统可直接应用于三维环境扫描。例如，通过将摄像机与激光雷达结合，可以对一组图像进行颜色建图，或者估计移动传感系统的精确传感器运动。
本文提出了一种固定相机与激光雷达之间的自动无目标标定方法，该方法基于手眼标定。在我们的方法中，分别估计传感器的运动，并使用估计的运动进行校准。在相同的模型下计算每个传感器的运动，并从每个传感器的运动中导出外部参数。
在我们的方法中，我们采用了基于传感器融合里程计的摄像机运动估计的思想，首先，从无尺度相机运动和确定尺度激光雷达运动中获得初始外部参数；接下来，使用初始外部参数和来自激光雷达的点云，重新计算带尺度的相机运动，然后利用这些运动重新计算外部参数；重计算摄像机运动和外部参数，直到估计收敛为止。
本文的贡献如下：
（1）该方法是第一种在传感器融合的2D-3D标定方法中结合了摄像机运动估计的方法。
（2）我们研究了这种校准方法有效地工作的传感器最佳运动。
（3）输入仅为相机的RGB图像和激光雷达的三维点云，不需要任何其他信息，与其它运动量较少的方法相比，外部参数的估计结果更为精确。

改变传感器位置和方向并使用每个传感器观察到的运动进行校准的方法称为手眼校准。设A和B分别为两个固定传感器观察到的位置和方向的变化，X为两个传感器之间未知的相对位置和方向。进而得到表达式AX = XB，利用该表达式求解X，可以得到传感器之间的外部参数。此外，由于噪声对传感器的影响，在标定方法中同时采用卡尔曼滤波估计传感器的偏差。

二.方法
图2显示了我们方法的概述。这种方法大致分为两步，在初始化阶段，我们使用ICP对准得到LiDAR运动，使用特征点匹配得到摄像机运动，二者结合估计外部参数。然后通过传感器融合里程计交替迭代估计外部参数和摄像机运动。

2.1 初始校准参数估计
2.1.1估计在初始参数估计中各个传感器的运动。
在激光雷达运动估计中，采用了基于ICP算法的高速配准方法，在注视方向上搜索对应点，我们使用一系列点预先在点云上创建网格，并将这些点投影到二维上，然后进行划分。在对齐扫描时，首先将对应点之间距离的阈值设置为较大，然后逐渐减小阈值去消除对应的异常值。
对于摄像机的初始运动估计，使用标准特征点匹配的方法。从匹配结果出发，利用5点算法和RANSAC算法计算摄像机帧间的初始相对位置和姿态。优化是通过最小化投影误差来实现的，使用了一个与极轴平面相关的角度误差度量。
2.1.2基于传感器运动的初始外部参数标定
为了从最初估计的运动中获得两个传感器之间的相对位置和方向，我们使用了一种扩展了正常手眼校准的方法来包括对摄像机运动尺度的估计。设摄像机和激光雷达测量的第i个位置和姿态变化分别为4×4矩阵Ai、Bi，两个传感器之间的外部参数为4×4矩阵X。可以建立AiX=XiB，通过分解它，得到下面两个方程：

设kia和kib为旋转矩阵Ria和Rib的旋转轴，当等式1成立时，下列等式成立。

由于无法计算相机帧之间的平移运动的绝对尺度，公式2用比例因子si写成如下

利用kia，kib系数的SVD对R进行线性求解。但是，要解决旋转问题，需要两个以上的位置和姿势变换，并且变换序列中必须包含不同方向的旋转。在非线性优化中，R通过最小化从式1导出代价函数来优化。

2.2 迭代相机运动估计和来自传感器运动的外参估计
单靠摄像机图像无法获得摄像机运动的尺度信息，外参的旋转分量与尺度信息无关，可以在初始参数估计阶段精确估计。另一方面，当如等式2所示旋转传感器时，可以根据相机和激光雷达的运动量之间的差异来计算外部参数的平移。因此，由于外参数平移分量的精度与摄像机运动估计的精度有着密切的关系，因此很难从无尺度摄像机运动中准确估计外参数。
一旦外部参数被估计，我们就可以利用给定的外部参数X和激光雷达扫描的距离数据来估计带尺度相机的运动Ai。在运动估计之后，使用Ai和Bi序列重新估计外部参数X。由于不需要同时估计Ai的平移分量，因此可以比初始估计更准确地估计X的平移分量然后，通过交替地重复摄像机运动估计和外部参数估计来优化外部参数，直到收敛。
2.2.1 基于距离数据的摄像机运动估计
图3显示了构造2D-3D对应关系的示意图，输入是世界坐标中的点云、在相机1、2的位置拍摄的两个相机图像以及用于将相机1定位到世界坐标的外部参数。首先，使用投影函数将点云中的一个点p投影到相机1中的图像上，其2D-3D对应关系可以描述为如下公式，v是从相机1的中心到相应像素的矢量方向.

然后我们使用KLT跟踪器跟踪p从相机图像1投影到图像2的像素，设v’为从摄像机2中心到被跟踪像素的矢量航向。在建立二维-三维对应关系之后，可以通过最小化投影误差优化相机1和相机2的相对位置和方向。设（v’j，pj）为第i运动中的第j个2D-3D对应，通过最小化角度度量代价函数，可以优化摄像机之间的位置和姿态转换。

2.2.2 参数校准
重新计算相机的位置和姿态转换后，使用相机和激光雷达的运动再次优化外部参数。在每次迭代中，R和t都是线性和非线性求解。在非线性优化中，R由式5优化，t由下式优化

三.二维-三维标定的最佳运动
在传感器之间的外部参数与摄像机运动的交替估计过程中，不可避免地会带有误差的对彼此的位置和姿态进行估计。由于运动估计和外部参数估计也依赖于被测环境和运动次数，因此很难获得精确的收敛条件，然而，可以考虑可能收敛估计的运动。
首先，我们考虑了外部参数误差对摄像机定位的影响以及估计成功的条件。

可以总结出：1）摄像头实际运动a 越小，投影误差越小；2） (α-β)越小，投影误差越小。第一点说明标定时候摄像头运动要小，第二点说明，标定的周围环境深度要变化小，比如墙壁。
另外还发现，增加摄像头运动的旋转角，摄像头运动估计到手眼标定的误差传播会小。