LVBA：用于RGB点云建图的LiDAR-视觉联合优化

文章：LVBA: LiDAR-Visual Bundle Adjustment for RGB Point Cloud Mapping

作者：Rundong Li, Xiyuan Liu, Haotian Li, Zheng Liu, Jiarong Lin, Yixi Cai, and Fu Zhang

编辑：点云PCL

摘要

带有准确颜色的点云地图在机器人和测绘应用中至关重要。然而，目前用于生成RGB彩色地图的方法主要依赖基于滤波估计或滑动窗口优化的实时定位，这些方法可能在精度和全局一致性上有所欠缺。在本研究中，我们提出了一种新颖的全局LiDAR-视觉联合优化方法，称为LVBA（LiDAR-Visual Bundle Adjustment），以提升RGB点云建图质量。LVBA首先通过全局LiDAR优化调整LiDAR位姿，随后结合点云中的平面特征进行光度视觉优化，从而优化相机位姿。此外为了解决在优化问题中由于地图点遮挡引发的挑战，我们在LVBA中实现了一种创新的LiDAR辅助全局可见性算法。为了验证LVBA的有效性，我们通过与现有最先进方法（如R3LIVE和FAST-LIVO）的对比实验，全面评估了其建图质量。实验结果表明，LVBA能够高效地重建高保真、精确的RGB点云地图，表现优于同类方法。

主要贡献

本文提出了LiDAR-视觉联合优化方法（LVBA），用于优化相机和LiDAR的位姿，LVBA分为两个阶段：首先通过LiDAR联合优化（LiDAR BA）优化LiDAR的位姿，然后利用LiDAR点云提供的几何先验，通过光度联合优化方法优化相机位姿。本文主要贡献：

• 光度联合优化方法，本文提出了一种基于LiDAR点云先验的光度联合优化方法，用于估计相机状态，即使在时间对齐不准确或外参未精细校准的情况下，也能提高点云上色质量。
• LiDAR辅助场景点生成和可见性确定算法：我们提出了一种包含全局相机帧共视性的算法，有助于构建全局光度视觉联合优化问题。
• 开发了一个工具链，用于评估彩色点云地图的精度与一致性。借助该工具链，对LVBA与当前最先进的LiDAR-视觉-惯性建图方法进行了广泛评估。实验结果表明，LVBA在精确和一致地重建彩色点云地图方面优于现有技术（参见图1）。

图1 使用我们的方法优化的RGB点云地图。图中数据采集自香港大学庄月明物理楼，该方法有效优化了LiDAR和相机的位姿，在建图过程中实现了高精度和高一致性。

内容概述

系统概述

本文提出的LiDAR-视觉联合优化（LVBA）系统的概览如图2所示。LVBA以LiDAR扫描数据、相机图像及其在同一世界参考框架下的粗略位姿为输入，这些通常由前端方法提供。系统包含两个主要部分：LiDAR联合优化部分和视觉联合优化部分。首先，LiDAR状态通过LiDAR联合优化方法进行优化，生成最佳LiDAR位姿估计和平面特征。随后利用LiDAR扫描数据和提取的平面特征点作为先验信息，通过迭代的粗到精光度联合优化方法对相机位姿进行优化。

图2. 系统概览，我们的系统由LiDAR联合优化（LiDAR BA）和视觉联合优化（Visual BA）两部分组成。

LiDAR联合优化

在LVBA中采用名为BALM的LiDAR联合优化方法来优化LiDAR的位姿，该方法通过利用从LiDAR点云中提取的边缘和平面特征，将LiDAR联合优化问题进行建模。优化过程的目标是最小化扫描中每个点与其邻近边缘或平面之间的欧几里得距离。我们使用BALM来优化每次LiDAR扫描的6自由度（6 DoF）位姿，同时构建一个包含从LiDAR点中提取的平面特征的体素地图，该地图将在后续的视觉联合优化过程中使用。

基于LiDAR先验的视觉联合优化

视觉联合优化（BA）是LVBA系统的重要部分，它通过生成视觉特征点（称为“场景点”）并将场景点投影到多个图像帧上，最小化它们之间的光度差异来构建优化目标函数。本模块分为几个关键步骤，包括局部场景点的生成和可见性判断、全局场景点的选择和可见性判定，以及光度误差的构建和优化。以下为详细总结：

局部场景点生成：局部场景点的选择以丰富的光度细节为优先，这有助于提升优化约束的有效性：

• 图像帧被划分为网格单元，LiDAR捕获的平面特征点被投影到这些网格中。
• 投影点需满足一定条件（例如视点方向与表面法向量的对齐程度），以保证视图质量。
• 使用DoG（高斯差分）方法计算投影点的强度梯度分数，每个网格中分数最高的点被选为局部场景点。
• 对于纹理不足（如光滑表面）导致分数较低的网格，则不选取点。

局部可见性判断：生成局部场景点后，需确定哪些图像帧可观察到这些点：

• 只考虑参考帧附近的滑动窗口内的目标帧，以简化视差计算和遮挡检测。
• 使用视域（FoV）检查代替复杂的射线追踪，但可能对前景与背景交界处的点存在误判。为此，通过计算参考帧和候选目标帧的光度差异来验证点的可见性。
• 确保视点方向接近点表面法向量，以进一步筛选目标帧。

全局场景点选择与可见性生成：为了提供更多全局约束，全局场景点用于优化相隔较远的相机帧：

• 利用LiDAR扫描数据构建全局可见性体素地图，每个体素记录其可见的相机帧集合。
• 通过选择观察质量最高的局部场景点，将其作为对应体素的全局场景点。观察质量通过点在参考图像上的投影面积来衡量，面积越大，质量越高。
• 依托可见性体素地图，确定全局场景点的候选目标帧集合，并使用与局部可见性类似的方法进一步筛选真实目标帧。

图3. LiDAR辅助的全局可见性地图。

光度误差公式化

• 光度误差通过比较参考帧和目标帧上场景点的光度值差异构建。
• 利用针孔相机模型将场景点投影到参考帧和目标帧，通过单应性变换对场景点周围的图像块进行匹配，确保光度一致性。
• 引入曝光时间和协方差矩阵的优化模型，提升光度误差计算的鲁棒性和精确度。

Levenberg-Marquardt优化

• 通过Levenberg-Marquardt优化算法，最小化所有场景点在所有目标帧中的光度误差。
• 采用逐步细化的优化策略，从图像金字塔的顶层开始逐步优化到原始分辨率，以增强对初始估计不准确的适应能力。
• 在每次迭代中，利用上一级优化结果生成新的场景点并更新可见性判断。

图4. 光度误差公式化

实验

实验设置

在实验中，将我们的 LVBA 与其他先进的 LiDAR-视觉（惯性）传感器融合方法进行了比较，包括 R3LIVE、FAST-LIVO和 Colmap-PCD。我们在三个数据集上进行了评估，分别是 R3LIVE 数据集、FAST-LIVO 数据集和 MaRS-LVIG 。我们对所有三个数据集进行了建图精度评估，并在 MaRS-LVIG 数据集上进行了轨迹精度评估，该数据集提供了地面真实轨迹。对于 Colmap-PCD 和 LVBA，在评估 R3LIVE 数据集和 MaRS-LVIG 数据集时，利用 R3LIVE 提供初步估计，而在评估 FAST-LIVO 数据集时，使用 FAST-LIVO 提供初步估计。由于这些数据的采集频率较高，通常为 10 Hz，对原始数据上的所有帧位姿进行优化对 LVBA 和 Colmap-PCD 这两种全局优化方法来说计算负担较重。为了限制计算负荷，提取了关键帧。此外由于某些序列对于 Colmap-PCD 和 LVBA 来说过于冗长，导致时间和内存开销过大，将这些序列分成了子序列，并将每个子序列的评估结果的平均值作为最终结果。

建图评估

1. 评估方法：为评估优化结果的映射精度和一致性，使用了一种着色与渲染算法。首先，通过 LiDAR 扫描和相机图像创建 RGB-辐射点云。每个 LiDAR 点的辐射值根据其最近的相机帧来计算，使用相对曝光时间 ϵ\epsilonϵ 进行调整。渲染出的图像使用峰值信噪比（PSNR）和结构相似性指数度量（SSIM）评估与原始图像的差异。

2. 评估设置：我们对三个数据集进行了映射精度评估。由于 FAST-LIVO 在 R3LIVE 数据集上的时间同步要求未满足，因此未在该数据集上评估 FAST-LIVO。此外，LVBA 和 Colmap-PCD 仅估计关键帧的位姿，因此仅在关键帧上评估了 R3LIVE 和 FAST-LIVO。为了评估全局场景点选择和相对曝光时间估计的有效性，分别进行了没有全局场景点约束（"w/o GSP"）和没有曝光时间估计（"w/o RET"）的实验。

3. 比较结果：与 R3LIVE 和 FAST-LIVO 相比，LVBA（完整版）在所有测试序列中显著提高了映射精度。R3LIVE 和 FAST-LIVO 基于 ESIKF 框架，无法有效纠正历史误差，导致长序列中的图像颜色模糊。而 LVBA 通过优化所有图像帧的状态估计，确保了全局一致性和较低的颜色模糊。与 Colmap-PCD 相比，LVBA（完整版）在大多数序列中表现更好，尤其是在处理曝光时间变化较大的序列时，LVBA 的表现明显优于 Colmap-PCD。此外，我们的相对曝光时间估计能够有效捕捉实际曝光时间的趋势，尽管模型简化导致估计值不完全准确，但仍表现出较好的估计能力。

图 7. 消融研究的可视化。(a) 和 (b) 显示了相机捕获的原始图像。(a1) 和 (b1) 显示了原始图像的细节。(a2) 和 (b2) 显示了我们完整算法生成的结果。(a3) 和 (b3) 展示了去除全局场景点后的结果。(a4) 和 (b4) 展示了去除相对曝光时间估计后的结果。该序列有效地展示了我们方法中每个组件对整体结果的影响和贡献。

图 8. 估计的相对曝光时间与真值的比较

总结

本文介绍了一种LiDAR-视觉束调整框架，旨在提高位姿估计精度并确保LiDAR-相机平台的全球RGB建图一致性。通过将相机的光度束调整（BA）与几何先验相结合，我们实现了相机位姿与LiDAR点云的高精度对齐。LiDAR辅助可见性判定算法使得该BA方法能够有效地应用于全局。通过严格的评估，我们证明了我们的方法在多个数据集上超越了其他最先进的方法，既提高了位姿估计精度，又确保了建图一致性。展望未来计划结合IMU预积分和更先进的优化技术，进一步提升系统性能。