CamVox: A Low-cost and Accurate Lidar-assisted Visual SLAM System | CamVox：一个低成本、高精度的激光辅助视觉SLAM系统

摘要

将激光雷达结合在基于相机的同时定位和建图(SLAM)是提高整体精度的有效方法，特别是在大规模的户外场景下。低成本激光雷达的最近发展。使我们能够以更低的预算和更高的性能来探索这种SLAM系统。在本文中，我们通过探索Livox激光雷达的独特特征，将Livox激光雷达转化为视觉SLAM(ORB-SLAM2)来提出CamVox。基于激光雷达的非重复性质，我们提出了一种可以在不受控制的场景中工作的自动激光雷达-照相机校准方法。长深度检测范围也有利于更有效的映射。在同一数据集上对CamVox与视觉SLAM(VINS-mono)和激光雷达SLAM(LOAM)进行了比较，以证明其性能。我们在Github上开源了硬件、代码和数据集。

1、前言

本文提出了第一个Livox激光雷达辅助视觉SLAM系统(CamVox)。我们的CamVox SLAM建立在最先进的ORB-SLAM2之上，使用Livox激光雷达作为深度传感器。创新点如下：

1、一种融合激光雷达和照相机输入的预处理步骤。经过仔细的时间同步，非重复扫描激光雷达点的失真被IMU数据校正，并转换为相机帧。

2、与激光雷达点云相比，该云的精度和范围都优于其他深度摄像机，所提出的SLAM系统可以以更高的效率进行大规模映射，并可以在户外强阳光环境下稳健地运行。

3、我们利用Livox激光雷达的非重复扫描特性，在不受控制的场景中在相机和激光雷达之间进行自动校准。

4、根据一些主流框架对CamVox进行了性能评估，并显示了非常精确的轨迹结果。

(a)CamVox机器人的轨迹（b）CamVox的密集RGBD地图 ( c )放大的 (b)的放大和旋转部分，显示出高质量的点云。

2、相关工作

由于丰富的角度分辨率和信息丰富的颜色信息，相机可以通过一个简单的投影模型和束调整来提供惊人的良好的定位和映射性能。最著名的单目相机单激光系统之一是ORB-SLAM[1]。ORB-SLAM通过提取中的ORB特征来跟踪对象图像和使用环闭包检测来优化地图和全局姿态，这通常是快速和稳定的。然而，由于相机未知的绝对深度尺度，它不能精确地恢复真实尺度因子。

为了解决单目相机无法恢复现实世界的规模的问题，Mur等提出了对ORB-SLAM的ORB-SLAM2[4]的改进，增加了立体相机和RGBD相机对深度估计的支持。然而，这两种相机都有缺点，特别是在估计具有长深度的户外物体时。立体声相机需要一个长基线来进行精确的长深度估计，这在现实场景中通常是有限的。此外，两个摄像机之间的校准容易受到机械变化，并将对长深估计精度产生不利影响。RGBD相机通常容易受到10米有限范围的阳光的影响。 融合相机和IMU是另一种常见的解决方案，因为相机可以部分校正IMU积分漂移，校准IMU偏置，而IMU可以克服单目系统的尺度模糊性。秦等人。提出了视觉惯性单眼SLAM (VINS)[2]，这是一种优秀的照相机和IMU融合系统。同样地，坎波斯等人。通过融合照相机和IMU的测量方法，扩展了ORB-SLAM2，并提出了ORB-SLAM3[3]。然而，消费者级IMU只在相对较低的精度下工作，并遭受偏差、噪声和漂移，而高端IMU的昂贵令人望而却步。

另一方面，激光雷达，提供了一个直接的空间测量。Lidar-SLAM框架,其中一项开创性的工作是LOAM。与视觉SLAM相比，由于来自激光雷达点云的精确深度估计，使它在动态环境中更具鲁棒性。然而，由于复杂的扫描机制，激光雷达不能提供像照相机那样多的角度分辨率，因此在隧道或走廊等结构不那么突出的环境中很容易出现故障。 它还缺乏环闭包检测，这使得算法只关注局部精度，而没有全局姿态和地图优化。为了在LOAM中添加环闭合检测，Shan等人提出了一种增强的LOAM算法LeGO-LOAM。与LOAM相比，LeGO-LOAM通过分割和聚类改进了特征提取以提高效率，并增加了环闭合检测来减少长期漂移。将激光雷达和照相机结合在SLAM框架中，成为一个理想的解决方案。在获得具有精确深度信息的点云的同时，可以利用相机的高角和颜色信息。张等人提出的VLOAM[8]，它松散地融合了单目相机和激光雷达。与LOAM类似，声称该估计值足够准确，不需要环闭合。申文等人也试图结合单目相机和激光雷达使用直接方法而不是特征点来估计位姿。此外，它紧密地结合了视觉数据和点云，并输出了估计的姿态。邵氏等人[9]进一步将立体相机、IMU和激光雷达融合在一起。他们利用立体视觉惯性，在户外真实场景中展示了良好的性能。

Lidar通常太昂贵，在实际应用中。Livox推出了一种基于棱镜扫描的新型激光雷达。由于新的扫描方法，成本可以显著降低，以实现大规模的采用。此外，这种新的扫描方法允许非重复的扫描模式，理想地用于在积累时获得相对较高清晰度的点云。即使是100ms的积累，Livox地平线的密度已经高达64条线，并继续增加。这一特性对校准激光雷达和照相机非常有益，因为传统的多线激光雷达缺乏激光线之间空间的精度。棱镜设计还具有一个最大的光孔径的信号接收，并允许远程检测。例如，Livox地平线可以在强阳光下探测到高达260米的地方。凭借如此优越的成本和性能，林等人提出的Livox-LOAM[11]，这是一种适应Livox中部的负荷的增强。基于此，Livox还发布了一个针对Livox地平线的LOAM框架，名为Livox_horizon_loam。

与传统激光雷达相比，三种Livox激光雷达模型的点云密度作为积分时间的函数。

但到目前为止，校准仍然是一项挑战任务，而且没有开源算法可以在不受控的场景中校准激光雷达和摄像机。Livox激光雷达的非重复扫描模式可以提供更简单的解决方案。

3、CamVox框架

本文所提出的CamVox基于ORB-SLAM2(RGBD模型)，具有单独的RGBD输入预处理和不受控制场景下的自动校准方法。该框架利用激光雷达辅助的视觉关键帧来生成局部建图，由于后端在各种束调整(BA)和ORB-SLAM2的轻量级姿态图优化，显示了很高的鲁棒性。

在原始的ORB-SLAM2中，关键点分为两类，近和远，近距离是深度具有高确定性的点，可用于尺度、平移和旋转估计，而远点只用于旋转估计，因此信息较少。由于从Livox激光雷达中获得的密集、长距离和准确的点与相机图像融合，比传统的RGBD相机（由于检测范围)或立体视觉相机(由于基线有限），可以分配更多的近点。因此，相机(ORB检测和跟踪的高角度分辨率)和激光雷达（远程深度测量）的精确测量都可以以紧密耦合的方式利用其优势。

A.软硬件和软件

CamVox的硬件包括一个MV-CE060-10UC卷帘快门相机，一个Livox地平线激光雷达和一个IMU。附加的GPS-RTK（惯性感知µINS）用于地面真相估计。通过10Hz的触发信号与所有这些传感器进行硬同步。每个触发信号输出（10Hz）。激光雷达保持一个时钟（与GPS-RTK同步），并以准确的时间戳连续输出扫描点。同时，IMU输出以200Hz的频率与触发器同步。来自GPS-RTK的数据也被记录下来，用于地面真相比较。安装了英特尔感测D435RGBD相机进行比较。整个系统安装在一个移动的机器人平台上。值得注意的是，Livox地平线激光雷达（800美元)明显低于其他类似性能激光雷达(1万-8万美元），这允许在合理的预算内构建完整的硬件系统。

软件管道在几个并行线程上运行，如图所示。除了来自ORBSLAM2的主要线程外，还增加了一个额外的RGBD输入预处理线程来捕获来自同步相机和激光雷达的数据(IMU修正)，并将其处理成一个统一的RGBD帧。可以触发一个自动校准线程来校准相机和激光雷达，当检测到机器人没有移动或通过人工交互时，它就会自动发生。然后评估校准的结果，并输出潜在的参数更新。
CamVoxSLAM管道。除了ORB-SLAM2主要线程外，还使用RGBD输入预处理线程将激光雷达和相机数据转换为RGBD格式，并可以自动/手动触发自动校准线程

B. 预处理过程

预处理线程从激光雷达中获取原始点，由IMU校正，并根据使用相机的外部校准投影到深度图像中。然后将RGB图像与深度图像组合作为RGBD帧的输出，其中两幅图像的格式大小相同，像素对应，如图所示。3c和图。3d.进一步跟踪线程操作，如ORB特征提取，然后基于输出执行关键点生成。由于激光雷达连续扫描环境，每个数据点都在稍微不同的时间戳下获得，需要从IMU校正。这与一个照相机不同，而一个图像几乎是在一个瞬间获得的。图中展示了这样一个连续运动的例子，当点云在机器人连续运动时被扭曲。

为了纠正这种失真，机器人运动在每个激光雷达点输出时间从IMU位置进行插值，并进行变换激光雷达在触发信号开始时指向激光雷达坐标，这也是捕捉相机图像的时间。

此外，在长途Livox激光雷达的帮助下，我们能够可靠地探测到超过100米的许多与深度相关的相机特征点。相比之下，感应RGBD相机无法探测到10米以上的点，还有阳光噪音。图中清楚地证明了这一点。因此，在CamVox中，我们可以指定那些相关深度小于130米的点的接近关键点。这远远超过立体基线的40倍(ORB-SLAM2)，通常用于商业立体相机的10厘米
##### C. 校准方法
由于激光雷达的远程能力，校准精度在CamVox中至关重要。小的角度不匹配可能导致在大深度产生大的绝对偏差。受控校准目标如棋盘并不总是可用，在随机固定故障或碰撞后可能发生错位。需要在不受控制的场景中开发一种自动校准方法，如果发现更好的校准匹配，则更新参数。由于Livox激光雷达的非重复特性，只要我们能积累几秒钟的扫描点，深度图像就可以像相机图像一样高分辨率。和与相机图像的对应关系很容易找到。因此，我们能够根据场景信息自动在几乎所有的现场场景中进行此校准。当检测到机器人静止时，会设置此自动校准的触发，以消除运动模糊。我们在保持激光雷达点的同时积累几秒钟。相机图像也被捕获。

用示例校准线纹管道。从相机和激光雷达捕获的数据（保持静止并积累几秒钟)用初始校准参数处理，形成三张图像(灰度、反射率和深度，后两个来自激光雷达）。对从这些图像中检测到的边缘进行校准，直到得到一组令人满意的参数。

整体校准算法的结构.首先通过使用反射率和深度值的初始外部参数将密集点云投影到成像平面上，然后进行轮廓提取以与相机图像轮廓进行比较。成本函数由一种改进的ICP算法构造，可由Ceres[17]进行优化，以得到相对更准确的外部校准参数。然后根据之前的值对这些新参数的成本函数进行评估，并决定是否在输入预处理线程上更新外部校准参数。

在将激光雷达点云投影到二维图像上后，我们对所有图像进行直方图均衡，并使用Canny边缘检测器提取边缘。从深度图像和反射率图像中提取的边缘被组合起来，因为它们都来自同一激光雷达，但都是单独的信息。为了提取更可靠的边缘，在这些边缘图像上滤出以下两种边缘。第一种是长度小于200像素的边缘。第二种边缘是杂乱在一起的内部边缘。最后，得到了照相机图像和激光雷达图像中同时存在的一些特征边缘，并根据最近的边缘距离进行边缘匹配。

初始匹配结果显示在图的右下角。其中橙色线是相机图像的边缘，蓝色线是地平线图像的边缘，红线是最近点之间的距离。在这里，我们采用了ICP算法[，并使用K-D树来加快对最近邻点的搜索。然而，有时在一个错误的匹配中，很少的点实际上参与了距离的计算，成本函数的值被困在这个局部最小值内。在这种情况下，我们通过增加一定程度的不匹配来改进ICP中的成本函数。改进后的成本函数如等式所示（5），其中n是激光雷达边缘点在距离阈值内的相机边缘点数，m是最近邻的数量，N是所有相机点的数量，b是称重因子。我们发现b的值10是一个很好的候选作为默认值。请注意，成本函数是每个点的平均值，因此可用于在不同场景中进行水平比较。

4、测试结果

在本节中，我们展示了CamVox的评估结果。具体地说，我们将首先显示自动校准的结果。并评估了为近关键点选择深度阈值的影响。最后，我们与一些主流SLAM框架进行了比较，并给出了时间分析。

A.自动校准结果

我们的自动校准结果如图所示。8.如图所示。8a是当传感器和相机未被校准（错位超过2度）时，RGB图像上激光雷达点的叠加。成本函数的值为7.95。触发自动校准，并校准结果，如图所示。8(b)，其中成本函数值为6.11，而手动完成的最佳校准结果如图所示。8©，其值为5.88。自动校准提供了一个非常接近最佳手动校准的结果。此外，由于类似图像的校准方案，自动校准在大多数不受控制的场景上稳健地工作。在图中评估了几个不同的场景。8(d-f)和成本值在6左右或低于6，我们认为这是相对较好的值。

B.对关键点的深度阈值的评估

因为激光雷达可以探测到260米，所以在融合的框架中有许多关键点，我们可以被描述为接近。这些点非常有利于跟踪和映射。来自图9(a-d)，通过将接近关键点的深度阈值从20米设置到130米，我们看到映射尺度和映射特征的数量都有显著增加。在图中9e我们评估了启动CamVox后前100帧(10FPS)中跟踪的匹配点的时间函数。随着捕获更多的帧，可以观察到特征数的增加，而较大的阈值明显最初跟踪了更多的特性，这有助于启动健壮的本地化和映射。使用CamVox，我们建议并将默认值设置为130米。

C. 运动轨迹的比较

对图所示的SUSTech框架和两个主流SLAM框架的轨迹进行了比较。10和表我使用evo工具。由于精确的校准，丰富的深度相关的视觉特征及其准确性跟踪时，CamVox系统非常接近地面真相，并显著优于其他框架，如livox地平线壤土和VINS-mono。

D. 计时结果

CamVox框架的时序分析如图II所示。该评估是在一个机载计算机系统歧管2C上进行的，它有一个4核的英特尔酷睿i7-8550U处理器。有了这样的系统，CamVox能够实时执行。自动校准大约需要58秒才能完成。因为校准线程只在机器人处于静止状态时偶尔运行，而且以后可能会发生参数的更新，因此这种计算时间不会是实时性能的问题。

5、结论

综而言之，我们提出了CamVox作为一种新的低成本激光雷达辅助视觉SLAM框架，旨在结合两种方法的最好的效果，即相机的最佳角度分辨率和激光雷达的最佳深度和距离。由于Livox激光雷达独特的工作原理，开发出了一种可以在不受控制的场景中执行的自动校准算法。在自动校准精度、深度阈值和轨迹比较方面对该框架进行了评价。它还可以在车载计算机上实时运行。我们希望这个新的框架能够对机器人和传感器的研究非常有用，并为社区提供一个开箱即用的低成本解决方案