我们提出了一个通过smoothing and mapping的紧耦合的雷达视觉惯性里程计框架，LVI-SAM，能够实时状态估计和建图，且具有很高的精度和鲁棒性。LVI-SAM基于因子图构建，其包含了两个子系统：一个视觉惯性系统和一个雷达惯性系统。这两个子系统以紧耦合的方式设计，其中VIS利用LIS的状态估计促进VIS的初始化。VIS的精度通过使用激光雷达的测量值提取视觉特征点的深度信息进行提高。反之，LIS使用VIS的状态估计作为估计初值进行scan-match。回环检测首先通过VIS进行，然后通过LIS进一步的优化。当两个子系统中的一个子系统出现异常时，LVI-SAM也能正常工作，这提高了其在无纹理和无几何特征环境中的鲁棒性。LVI-SAM在多个平台收集的数据集上进行了广泛的评估。我们的实现已经开源。
前面省略了价值不大的部分。
在本文中，我们提出了一个通过smoothing and mapping的紧耦合的雷达-视觉-惯性里程计框架，LVI-SAM，用于实时的位姿估计和建图。基于因子图，LVI-SAM是由两个子系统组成的，一个视觉惯性系统和一个雷达惯性系统。The two sub-systems can function independently when failure is detected in one of them, or jointly when enough features are detected. VIS进行视觉特征跟踪，并可以选择是否使用雷达帧中的数据提取视觉特征点的深度。视觉里程计数据，是通过优化视觉的重投影误差和IMU的测量值获得的，也能作为激光scan-matching的初值，同时在因子图引入了约束。使用IMU测量值对点云去畸变后，LIS提取边特征和平面特征，并且与在滑窗中保持的特征地图进行匹配。LIS估计的系统状态可以发给VIS促进其的初始化。对于回环检测，首先由VIS识别，接下来由LIS进一步优化。来自于视觉里程计、激光里程计、IMU预积分【20】和回环的约束被加到因子图中进行联合优化。最后，优化后的IMU零偏被用来基于IMU的测量值进行前向传播估计位姿。我们工作的主要贡献可以总结如下：
Our work is novel from a systems standpoint, representing a unique integration of the state-of-the-art in VIO and LIO to achieve an LVIO system offering improved robustness and accuracy.
我们希望我们的系统可以作为一个solid baseline，其他人可以很容易地基于此推进激光雷达视觉惯性里程计的发展。
所提出的激光-视觉-惯性系统，它接收来自三维激光雷达，单目相机和IMU的数据，如图1所示。我们的系统由两个关键的子系统构成：一个视觉惯性系统和一个激光雷达惯性系统。VIS处理图像和IMU数据，不过激光雷达的测量数据也是可选的。视觉里程计是通过最小视觉和IMU测量数据的残差获得的。LIS首先提取激光点云特征，然后通过提取的特征与特征地图进行匹配获取激光里程计。为了实时性特征地图以滑窗的方式维护。最后，状态估计问题实际上可以归结为一个最大后验问题，可以通过使用iSAM2【21】联合优化因子图中的IMU预积分约束、视觉里程计约束和激光里程计约束求解。需要注意的是在LIS中使用的基于多传感器图优化是为了减少数据交换和提高系统效率。
对于VIS，我们采用【8】中processing pipeline，如图2所示。视觉特征点通过角点检测器【22】检测，之后采用Kanade–Lucas–Tomasi算法【23】进行跟踪。VIS初始化之后，基于视觉里程计信息对齐激光帧的点云，得到了一个稀疏的深度图用于特征点深度估计。系统在滑窗中进行BA，其中的系统状态如下：
其中是旋转矩阵，
是位置向量，v是速度，
是IMU零偏，其中
是加速度的零偏，
是角速度的零偏。变换矩阵
表示从body坐标系到视觉坐标系W。在接下俩的章节中，我们给出了详细的步骤关于提高VIS初始化和特征深度估计。由于篇幅限制，我们推荐读者进一步阅读【8】了解细节，比如残差的实现。
基于图优化的VIO通常由于高度的非线性导致在初始化时不收敛。初始化的质量很是依赖于两个因素：初始化时传感器的运动和IMU参数的精度。在实际中，我们发现【8】在传感器以较小的速度或者恒定的速度运动时很容易初始化失败。这是由于当加速度激励不够导致了尺度因子不可观。IMU参数包含了缓慢变化的零偏和白噪声，这会影响到加速度和角速度的测量。在初始化中对于这些参数的良好猜测有助于优化的快速收敛。
为了提高VIS初始化的鲁棒性，我们利用LIS估计的系统状态x和IMU零偏b。因为激光雷达可以直接观测到深度，我们首先初始化LIS并获取到系统状态x和零偏b。然后我们通过插值来把激光帧和视觉帧通过时间戳对齐。需要注意的是，两帧之间的IMU零偏被认为是一个常数。最后使用LIS估计的状态x和零偏b作为VIS初始化中的初始估计，极大的提高了初始化的速度和鲁棒性。关于VIS初始化过程中有无LIS辅助的对比可以参考补充视频。
Upon the initialization of the VIS，我们使用估计的视觉里程计将激光雷达坐标系对齐到相机坐标系。由于现在的3D激光雷达通常点云比较稀疏，因此我们堆叠多个激光雷达帧以获得稠密的深度图。要将视觉特征与深度值关联，我们首先将视觉特征和激光雷达点投影到以相机为中心的单位球体上。然后使用球体上恒定密度的极坐标对激光雷达点进行下采样和存储。通过使用视觉特征的极坐标搜索二维K-D树，在球体上查找视觉特征的最近三个深度点。At last, the feature depth is the length of the line formed by the visual feature and camera center Oc, which intersects the plane formed by the three depth points in Cartesian space. 上述的处理过程如图3（a）所示，其中的特征深度就是虚线的长度。
我们还需要通过检查与三个最近深度点的距离进一步验证特征点的关联深度。This is because stacking lidar frames from different timestamps may result in depth ambiguity from different objects。以上情况的说明如图3（b）所示。在ti时刻观察到的深度点用绿色表示。在tj时刻相机移动到了一个新的位置，并观察到了一个新的深度点（用灰色表示）。However, the depth points at ti, which are circled by a dashed gray line, may still be observable at tj due to lidar frame stacking. 使用来自不同物体的深度点来与视觉特征深度进行关联可能会导致状态估计的不准确。与【17】类似，我们通过检查特征周围深度点之间的最大距离解决上述问题。如果最大距离大于2m，就不对该特征点进行数据关联。
深度图与视觉特征的对齐如图4所示。在图4的（a）中和（c）中，利用视觉里程计对齐的深度图投影到了图像上。在图4的（b）和（d）中，成功关联到深度的视觉特征是绿色的。需要注意的是在图（a）中尽管深度图覆盖了图像的绝大部分，但是在（b）中许多位于窗口角落区域的特征点由于validation check failure 缺乏深度关联。
VIS在剧烈运动、光照变化和纹理缺乏的环境下很容易异常。当机器人进行剧烈运动时或者进入到一个纹理缺乏的环境下，跟踪的特征点会急剧下降。特征点不足导致了优化的不收敛。我们也注意到当VINS异常时估计的IMU零偏会特别的大。因此当IMU零偏超过了阈值或者跟踪的特征点低于阈值时，我们就认为VIS出现了故障。Active failure detection is necessary for our system so that its failure does not corrupt the function of the LIS. 一旦检测到异常时，VIS就立即进行重新初始化并通知LIS。
我们使用DBoW2【24】进行回环检测。对于每个关键帧，我们提取它的BRIEF描述【25】，并将它与之前关键帧提取的描述子进行匹配。DBoW2返回的回环候选关键帧将被发送给LIS并做进一步的验证。
提出的激光雷达惯性系统，继承于【14】，并如图5所示，其维持了一个因子图用于全局优化。其中包含了四种约束：IMU预积分约束，视觉里程计约束，雷达里程计约束和回环检测约束，它们被添加到因子图中进行联合优化。激光雷达里程计约束是从scan-matching推导出来的，where we match the current lidar keyframe to a global feature map。回环检测约束的候选帧首先由VIS提供，然后由scan-matching进一步确认。对于特征地图，我们维护了一个由激光雷达关键帧组成的滑动窗口，这保证了计算的复杂性不至于很高。当机器人的位姿变化超过了一个阀值的时候，选择此时的激光雷达帧作为关键帧。The intermittent lidar frames lying between pairs of keyframes are discarded. 基于新选择的关键帧，机器人的状态x被添加到因子图中作为一个节点。 以这种方式添加关键帧，不仅可以在内存消耗和地图密度之间取得平衡，而且还有助于维护相对稀疏的因子图以进行实时优化。由于篇幅的限制，我们推荐读者阅读【14】了解实现的细节。在接下来的章节中，我们重点关注于提高系统鲁棒性的新措施。
我们发现初始估计在成功scan-matching中扮演非常重要的角色，特别是当传感器进行剧烈运动的时候。 The source of initial guess is different before and after LIS initialization.
在LIS初始化之前，我们假设机器人在初始位置静止。然后我们对IMU的原始数据进行积分，并假设零偏和噪声是零。两个激光雷达关键帧之间的IMU积分后的平移和旋转作为scan-matching的初值估计。我们发现这种方法在初始速度小于10m/s和角速度小于180°/s这种具有挑战的场景下也可以成功的初始化。一旦LIS初始化成功，我们可以估计出因子图中的IMU的零偏、机器人的位姿和速度。然后呢我们把它们发送给VIS以促使其初始化。
当LIS初始化成功后，我们有两个途径获取初始估计：IMU测量值的积分和VIS系统。当VIS里程计可以用的时候我们使用它作为初始估计。当VIS发生异常的时候，我们切换到IMU测量值的积分作为初始估计。以上的这些措施在纹理丰富或者纹理缺乏的环境中均提高了初始估计的精度和鲁棒性。
尽管激光雷达可以得到场景中很远范围内的细节，但是也会在一些场景中导致扫描匹配失败，如图6所示。我们采用【26】的方法进行LIS的异常检测。在scan-matching中非线性优化问题可以构建为迭代求解如下的线性问题：
其中A和b通过针对T线性化获得。The LIS reports failure when the smallest eigenvalue of is smaller than a threshold at the first iteration of optimization。当异常发生时，激光里程计不会添加到因子图中。我们建议读者阅读【26】，了解这些假设所依据的详细分析。
[1] J. Graeter, A. Wilczynski, and M. Lauer, “LIMO: Lidar-Monocular Visual Odometry,” IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 7872–7879, 2018.
[2] Y.-S. Shin, Y. S. Park, and A. Kim, “DVL-SLAM: Sparse Depth Enhanced Direct Visual-LiDAR SLAM,” Autonomous Robots, vol. 44, no. 2, pp. 115–130, 2020.
[3] A. I. Mourikis and S. I. Roumeliotis, “A Multi-state Constraint Kalman Filter for Vision-aided Inertial Navigation,” IEEE International Conference on Robotics and Automation (ICRA), pp. 3565–3572, 2007.
[4] M. Bloesch, S. Omari, M. Hutter, and R. Siegwart, “Robust Visual Inertial Odometry using A Direct EKF-based Approach,” IEEE/RSJ international conference on intelligent robots and systems (IROS), pp. 298–304, 2015.
[5] P. Geneva, K. Eckenhoff, W. Lee, Y. Yang, and G. Huang, “OpenVINS: A Research Platform for Visual-Inertial Estimation,” IROS Workshop on Visual-Inertial Navigation: Challenges and Applications, 2019.
[6] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, “Keyframe-based Visual-Inertial Odometry using Nonlinear Optimization,” The International Journal of Robotics Research, vol. 34, no. 3, pp. 314–334, 2015.
[7] A. Rosinol, M. Abate, Y. Chang, and L. Carlone, “Kimera: an Open-Source Library for Real-Time Metric-Semantic Localization and Mapping.” IEEE, 2020, pp. 1689–1696.
[8] T. Qin, P. Li, and S. Shen, “VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.
[9] J. Delmerico and D. Scaramuzza, “A Benchmark Comparison of Monocular Visual-Inertial Odometry Algorithms for Flying Robots,” IEEE International Conference on Robotics and Automation (ICRA), pp. 2502–2509, 2018.
[10] J. Zhang and S. Singh, “Low-drift and Real-time Lidar Odometry and Mapping,” Autonomous Robots, vol. 41, no. 2, pp. 401–416, 2017.
[11] T. Shan and B. Englot, “LeGO-LOAM: Lightweight and GroundOptimized Lidar Odometry and Mapping on Variable Terrain,” IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4758–4765, 2018.
[12] C. Chen, H. Zhu, M. Li, and S. You, “A Review of Visual-InertialSimultaneous Localization and Mapping from Filtering-based and Optimization-based Perspectives,” Robotics, vol. 7, no. 3, p. 45, 2018.
[13] H. Ye, Y. Chen, and M. Liu, “Tightly Coupled 3D Lidar Inertial Odometry and Mapping,” IEEE International Conference on Robotics and Automation (ICRA), pp. 3144–3150, 2019.
[14] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “LIOSAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping,” IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4758–4765, 2020.
[15] C. Qin, H. Ye, C. E. Pranata, J. Han, S. Zhang, and M. Liu, “LINS: A Lidar-Inertial State Estimator for Robust and Efficient Navigation,” IEEE International Conference on Robotics and Automation (ICRA), pp. 8899–8906, 2020.
[16] C. Debeunne and D. Vivet, “A Review of Visual-LiDAR Fusion based Simultaneous Localization and Mapping,” Sensors, vol. 20, no. 7, p. 2068, 2020.
[17] J. Zhang and S. Singh, “Laser-Visual-Inertial Odometry and Mapping with High Robustness and Low Drift,” Journal of Field Robotics, vol. 35, no. 8, pp. 1242–1264, 2018.
[18] A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, “Vision Meets Robotics: The KITTI Dataset,” The International Journal of Robotics Research, vol. 32, no. 11, pp. 1231–1237, 2013.
[19] X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, “LIC-Fusion: LiDAR-Inertial-Camera Odometry,” arXiv preprint arXiv:1909.04102, 2019.
[20] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-Manifold Preintegration for Real-Time Visual–Inertial Odometry,” IEEE Transactions on Robotics, vol. 33, no. 1, pp. 1–21, 2016.
[21] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, “iSAM2: Incremental Smoothing and Mapping using the Bayes Tree,” The International Journal of Robotics Research, vol. 31, no. 2, pp. 216–235, 2012.
[22] J. Shi et al., “Good Features to Track,” IEEE Conference on Computer Vision and Pattern Recognition, pp. 593–600, 1994.
[23] B. D. Lucas, T. Kanade et al., “An Iterative Image Registration Technique with an Application to Stereo Vision,” 1981.
[24] D. Galvez-L ´ opez and J. D. Tardos, “Bags of Binary Words for ´ Fast Place Recognition in Image Sequences,” IEEE Transactions on Robotics, vol. 28, no. 5, pp. 1188–1197, 2012.
[25] M. Calonder, V. Lepetit, C. Strecha, and P. Fua, “Brief: Binary robust independent elementary features,” European conference on computer vision, pp. 778–792, 2010.
[26] J. Zhang, M. Kaess, and S. Singh, “On Degeneracy of Optimizationbased State Estimation Problems,” IEEE International Conference on Robotics and Automation (ICRA), pp. 809–816, 2016.
