# PO-VINS：一种高效且鲁棒的、带激光雷达增强的纯姿态视觉惯性状态估计器 唐海亮 $^{ID}$、张体生 $^{ID}$、王立强 $^{ID}$、王冠 $^{ID}$、牛晓吉 $^{ID}$ 摘要—采用纯姿态视觉表示进行姿态调整，已被证明与束调整（BA）等效，同时能显著提升计算效率。然而，在用于实时导航的正常配置下，纯姿态解决方案尚未在紧耦合视觉惯性状态估计器（VISE）中得到充分考虑。与此同时，激光雷达能够为视觉地标提供精确的深度估计，从而增强VISE的鲁棒性。因此，我们提出了一种紧耦合的激光雷达增强型VISE，命名为PO-VINS，它是一种适用于自主应用的高效且鲁棒的里程计框架。该系统集成了纯姿态解决方案以提高状态估计效率，同时利用轻量级的激光雷达增强技术来提升鲁棒性和精度。具体而言，基于纯姿态视觉表示，我们推导出了解析深度不确定性，用于剔除激光雷达深度异常值。此外，我们在纯姿态框架下提出了基于多状态约束（MSC）的激光雷达深度测量（LDM）模型，以平衡效率与鲁棒性。纯姿态视觉数据、LDM以及IMU预积分测量结果在因子图优化（FGO）框架下被紧密集成，从而实现高效且精准的状态估计。在私有和公共数据集上的大量实验结果表明，所提出的PO-VINS在精度上优于或可与最先进（SOTA）方法相媲美。与基线方法LE-VINS相比，PO-VINS在私有数据集上的状态估计效率在笔记本电脑（Intel i7-13700H）上提升了33%，在机载ARM计算机（NVIDIA Xavier）上提升了56%。此外，通过采用所提出的不确定性驱动的异常值剔除方法和基于MSC的激光雷达深度测量模型，PO-VINS比LE-VINS具有更高的精度和鲁棒性。 收稿日期：2025年7月18日；修订日期：2025年10月16日；录用日期：2025年10月21日。发表日期：2025年11月13日；当前版本日期：2025年11月21日。本研究部分得到了国家自然科学基金项目（批准号：42374034和42504027）、湖北省重点研发计划项目（批准号：2024BAB024）、湖北省重大专项（JD）（批准号：2023BAA02602）以及工业和信息化部高质量发展项目（批准号：2024-182）的支持。负责协调审稿流程的副编辑是Alvaro Hernandez博士。（通讯作者：张体生。） 唐海亮、张体生、牛晓吉隶属于武汉大学卫星导航定位技术研究中心、湖北时空信息与定位导航技术创新中心、湖北珞珈实验室，武汉430079，中国（电子邮箱：；；）。 王立强、王冠隶属于武汉大学卫星导航定位技术研究中心，武汉430079，中国（电子邮箱：；）。 数字对象标识符：10.1109/TIM.2025.3632451 关键词—因子图优化（FGO）、多传感器融合导航、纯姿态状态估计、视觉惯性导航。 ## 术语表 $p^{p}$ 坐标 $(u, v)$ 在图像像素坐标系中的表示。 $p^{u}$ 归一化相机坐标系（u坐标系）下的坐标，其深度为单位值。 $p^{r}$ 激光雷达点在激光雷达测距坐标系（r坐标系）中的表示。 q、R、$\phi$ 分别表示姿态四元数、旋转矩阵和旋转向量。 ⊗ 表示四元数乘法。 Log、Exp 分别表示四元数与旋转向量之间的转换关系。 $p_{wb}^{w}, q_{b}^{w}$ IMU位姿在机体坐标系（b坐标系）中相对于世界坐标系（w坐标系）的表示。 $v_{wb}^{w}$ IMU在世界坐标系中的速度。 $b_{g}, b_{a}$ 陀螺仪和加速度计的偏差。 $p_{bc}^{b}, q_{c}^{b}$ 相机与IMU之间的外部参数。 $p_{rc}^{r}, q_{c}^{r}$ 相机与激光雷达之间的外部参数。 $t_{d}$ 相机数据与IMU数据之间的时间延迟参数。 X、x 表示状态向量。 ATE 表示绝对平移误差。 BA 表示光束法平差。 FGO 表示因子图优化。 GNSS 表示全球导航卫星系统。 IMU 表示惯性测量单元。 INS 表示惯性导航系统。 LDM 表示激光雷达深度测量。 LVINS 表示激光雷达-视觉-惯性导航系统。 MEMS 表示微机电系统。 MSC 表示多状态约束。 RTE 表示相对平移误差。 SLAM 表示同步定位与地图构建。 VINS 表示视觉-惯性导航系统。 VISE 表示视觉-惯性状态估计器。 VRM 表示视觉重投影测量。 ### I. 引言 由于成本更低、体积更小，VINS在移动机器人和自动驾驶车辆中被广泛应用。 --- [^1]。单目相机无法估计自身运动的绝对尺度，从而导致单目相机的尺度不确定性[^2]。利用低成本的MEMS-IMU可以获取自身运动的尺度信息。同时，通过帧间数据关联得到的视觉测量结果，也能估计出IMU的偏差和比例因子误差[^3]。此外，与基于激光雷达的方法相比，基于视觉的方法成本更低且精度满足要求。因此，紧耦合的VINS能够在无GNSS信号的环境中实现可靠且精确的相对定位[^4]。 通常，视觉特征[^5]、[^6]会被逐帧检测并跟踪以获取测量值；或者直接将原始像素[^7]和图像块[^8]作为测量值。结合前端获取的视觉测量值与IMU测量值，便可进行状态估计[^9]，从而确定物体的姿态和速度。传统上，由于计算成本较低，基于滤波的方法——包括扩展卡尔曼滤波器（EKF）[^10]、[^11]以及迭代扩展卡尔曼滤波器（IEKF）[^12]——常被用于解决VINS中的状态估计问题。其中，基于多状态约束卡尔曼滤波器（MSCKF）的VINS[^10]、[^11]、[^13]、[^14]表现相当出色。事实证明，在VINS中FGO[^15]的精度要高于滤波方法[^16]。这是因为通过多次迭代可显著减小视觉测量的非线性误差。因此，基于优化的VINS受到了广泛关注，其中包括基于特征的方法[^17]、[^18]、[^19]、[^20]以及直接法[^7]、[^21]、[^22]。 尽管基于优化的VINS会消耗大量计算资源，但凭借计算机技术的快速发展，它仍能实现出色的实时性能，比如VINS-Mono[^17]。通常，人们采用BA[^2]、[^9]来同时估计姿态和视觉地标参数。然而，视觉地标的高维参数空间会大幅增加VINS中状态估计问题的求解复杂度，相比之下姿态参数的维度要低得多。研究人员已经研究了提升BA效率的新方法，例如增量式BA[^23]、[^24]、固定滞后平滑器或滑动窗口优化器[^17]、[^25]，以及保持稀疏性[^26]。即便如此，估计视觉地标的参数空间仍需耗费过多计算资源。在一些边缘设备上，降低计算成本至关重要，尤其是当可用于定位任务的处理器核心数量极少时。 近期，双视图成像几何已被证明等价于一组仅涉及姿态的约束，这类约束能够将相机姿态与视觉地标参数解耦 [^27]。这一研究成果随后被拓展至多视图成像几何场景 [^28]。姿态估计过程采用姿态调整而非光束法平差（BA）来实现。此外，已获取的相机姿态可用来解析重构出视觉地标参数 [^28]。具体而言，姿态调整是通过将视觉地标参数显式表示为姿态和特征测量值来实现的。仅基于姿态的解决方案表明，在三维场景重建中计算效率可提升2到4个数量级 [^28]。PIPO-SLAM [^29] 是一款基于ORB-SLAM3 [^20] 的视觉-惯性SLAM系统，其在局部建图线程中采用局部姿态调整而非BA。当局部建图优化涉及超过100个相机姿态和1000个地标时，所采用的局部姿态调整能显著降低计算和内存开销 [^29]。需要注意的是，PIPO-SLAM [^29] 中的局部姿态调整属于纯视觉优化，与 [^28] 中的情况一致，并非紧耦合的视觉-惯性优化。此外，使用100个相机和1000个地标并非实时导航场景中的常规配置。因此，仅基于姿态的解决方案尚未在面向实时导航的典型配置的紧耦合视觉-惯性同步定位与地图构建（VISE）系统中得到充分考量。具体而言，仅基于姿态的解决方案对实时视觉惯性导航系统（VINS）的精度和效率的影响仍不明确。 除了效率问题外，视觉地标的精确深度估计也是影响视觉惯性导航系统（VINS）精度和鲁棒性的关键因素。相比之下，激光雷达能够直接测量远距离的精确深度 [^30]，因此可利用三维激光雷达为视觉惯性导航系统提供精确的深度信息，从而提升其性能 [^31]、[^32]、[^33]。因此，经过激光雷达深度增强的视觉惯性导航系统在大多数场景下能达到与以激光雷达为中心的方法相近的精度，同时无需集成激光雷达里程计，还能降低计算成本。简而言之，轻量化的激光雷达增强型视觉惯性导航系统在许多应用中都能成为高效且精准的方案。在LE-VINS [^33] 中，研究证实通过在状态估计器中构建激光雷达深度模型，可提升视觉惯性导航系统的鲁棒性和精度。因此，借助该模型就能精确估计视觉地标的逆深度参数 [^34]，进而提高姿态估计的精度 [^33]。不过，在仅基于姿态的解决方案里，视觉地标的状态参数会被从状态向量中移除。为了实现精准且高效的状态估计，还需进一步研究仅基于姿态的激光雷达深度模型。 在本研究中，我们旨在构建一种名为PO-VINS的激光雷达增强型仅基于姿态的视觉惯性同步定位与地图构建系统，以实现高效且鲁棒的状态估计。该系统引入仅基于姿态的解决方案，通过从状态估计器中移除视觉地标状态来提升效率；同时融入轻量化的激光雷达增强机制，为视觉地标提供精确的深度测量数据以提升精度，并且也在仅基于姿态的框架内构建了激光雷达深度模型。本研究的主要贡献如下。 1) 在因子图优化框架下提出了一种激光雷达增强型紧耦合视觉惯性同步定位与地图构建系统。所提出的状态估计器是针对视觉残差模型和激光雷达深度模型的完整仅基于姿态的解决方案，能够在效率与鲁棒性之间取得平衡。 2) 视觉地标深度的分析不确定性源自仅基于姿态的视觉呈现方式。因此，我们提出了一种基于不确定性的异常值剔除方法，用于检测并剔除激光雷达深度异常值，该方法已被证明能够提升状态估计的鲁棒性。 --- 3) 我们采用经典的MSC方法来构建仅基于姿态的LDM模型，即将某个地标的所有视觉与激光雷达深度观测数据整合为单一测量值。这样一来，测量数量得以减少，估计效率也随之提高。此外，该方法还能降低激光雷达深度异常值的影响，进一步提升估计的鲁棒性。 4) 我们在公开及私有数据集上开展了全面实验，涉及低速机器人、手持设备以及车辆等场景。此外，我们还在嵌入式机载ARM计算机上验证了该方法的实时性能。 本文的后续内容安排如下：我们首先简要介绍有关VINS及激光雷达增强型VINS的相关研究；系统概述将在第三节中阐述；第四节介绍了仅基于视觉的姿态表示方法，并推导了地标的分析深度不确定性；随后，第五节给出了所提出的激光雷达增强型VISE框架，其中包含完整的仅基于视觉的姿态表示及LDM模型；第六节展示并讨论了用于定量评估的实验结果与数据；最后，我们对所提出的PO-VINS进行了总结。 ### 二、相关工作 在本研究中，我们的目标是构建一种具备激光雷达增强功能的紧耦合VINS，以实现高效且鲁棒的状态估计。因此，相关研究可分为VISE和激光雷达增强型VISE两类。此处所说的激光雷达增强，是指利用激光雷达深度数据来提升VISE的精度与鲁棒性。事实证明，紧耦合方法比松耦合方法更精确，因此我们将重点讨论前者。 ### A. 视觉惯性状态估计器 早期阶段，受计算资源限制，视觉惯性估计器通常采用卡尔曼滤波器进行建模[^9]。MonoSLAM是一种实时单目SLAM系统，在EKF框架下可在普通个人电脑上以30 Hz的频率运行[^35]。FEJ-EKF通过采用初始估计雅可比矩阵来提升估计器的一致性[^36]。IEKF也被应用于视觉惯性状态估计[^12]。有一种基于MSCKF的估计器被提出用于辅助视觉的惯性导航，且该估计器不会将视觉地标纳入状态向量[^10]。MSCKF中的延迟线性化机制避免了计算负担和信息损失，因此能实现更高的精度与效率[^10]。MSCKF 2.0在MSCKF的基础上采用了初始估计雅可比矩阵，从而实现一致的状态估计，并能在线估算相机与IMU之间的外部参数[^11]。由于具备出色的实时性能，MSCKF已成为著名的VISE框架。因此，许多近期研究都聚焦于基于MSCKF的估计器，例如R-VIO[^37]和OpenVINS[^13]。不过，基于滤波的方法中对非线性视觉测量的一次性线性化处理可能会给估计器引入较大的线性化误差，进而降低其性能[^16]。 相比之下，通过使用FGO [^15]，非线性视觉测量可以被反复线性化。因此，尽管计算复杂度更高，但FGO在视觉-惯性估计方面的精度应优于滤波器 [^16]、[^38]。通常，FGO或非线性优化是通过采用滑动窗口估计器 [^39] 来实现的，以降低状态向量的维度并提升估计效率。此外，边缘化过程利用舒尔补 [^40] 将所有被边缘化的状态转换为先验，从而避免信息丢失 [^7]、[^25]、[^39]。OKVIS [^25] 是一种基于关键帧的视觉-惯性里程计（VIO），采用非线性优化技术，其精度优于基于MSCKF的方法。VINS-Mono [^17] 进一步采用了鲁棒的视觉-惯性初始化以及基于全局位姿图优化（PGO）的在线重定位功能。Kimera [^18] 包含类似的VIO模块，该模块是基于著名的非线性优化库GTSAM [^41] 开发的。ORB-SLAM-VI [^42] 和ORB-SLAM3 [^20] 均基于ORB-SLAM [^43]、[^44]，它们也采用边缘化来构建先验。BASALT [^19] 通过非线性因子恢复技术 [^45] 从VIO中提取相关信息，用于视觉-惯性建图。直接稀疏方法 [^7] 被整合到VI-DOS [^46] 中，该方法包含动态边缘化功能。DM-VIO [^22] 是一种延迟边缘化的VIO，它通过维护第二个因子图来进行边缘化，从而提升了估计精度。 上述基于优化的方法利用边缘化技术在不损失信息的前提下提升实时性能。增量式BA，如ICE-BA [^23] 和iSAM2 [^24]、[^41]，也能通过仅更新与观测相关的有限状态来提高VISE的效率。不过，增量式BA主要是为全局建图而非里程计设计的。除了这些方法外，采用不同的特征参数化方式，如逆深度 [^34]、ParallaxBA [^47] 和PMBA [^48]，可能会导致迭代次数和运行时间的差异。然而，视觉地标的高维参数空间仍是实现高效视觉-惯性状态估计的主要障碍。纯位姿求解方案，即位姿调整，在大规模纯视觉状态估计中的效率已显示出优于BA的特点 [^28]、[^29]。不过，这种纯位姿求解方案尚未被应用于实时的紧耦合VISE中，因为在此场景下地标参数的维度非常有限。具体而言，纯位姿求解方案对紧耦合VISE效率和精度的影响尚未得到验证。 ### B. 激光雷达增强型VISE 激光雷达增强型VISE可进一步分为直接法和基于特征的方法 [^33]。直接法利用激光雷达提供的深度数据匹配视觉像素，通过光度BA来实现位姿估计；而在基于特征的方法中，视觉特征会与激光雷达深度数据相关联，再通过BA进行状态估计。 由于直接法会用到视觉像素，因此可以采用稀疏型激光雷达，比如16线旋转式激光雷达来提供深度信息。通过结合遮挡处理进行光度对齐，即可实现直接的激光-视觉里程计 [^49]。DVL-SLAM是一种类似的直接视觉SLAM。 --- 该系统采用基于窗口的优化方法，但并未考虑遮挡问题。在紧密耦合的激光雷达-视觉惯性系统（LiDAR-VIO）中，也利用了精确的激光雷达深度数据，例如R3LIVE [^50]和FAST-LIVO [^51]、[^52]。然而，为避免错误使用激光雷达深度数据而导致精度严重下降，必须考虑遮挡问题。此外，在直接法中，相机与激光雷达之间的外参误差影响更为显著，因为此时会直接使用对应像素处的激光雷达深度值。 因此，许多激光雷达增强型视觉惯性状态估计系统（VISE）采用了基于特征的方法：先检测视觉特征，再将其与激光雷达深度相关联。在DEMO [^31]中，视觉特征与激光雷达点云是在相机坐标系下的单位球面上进行关联的。这种深度关联方法也被[^53]所采用，但该方法将激光雷达深度视为常数，未考虑深度误差。LE-VINS [^33]提出了一种LDM模型，该模型在考虑深度误差的同时为视觉路标状态构建约束。相比之下，[^32]和[^54]则是在图像平面上实现深度关联，这导致将点云投影到图像平面时计算成本较高。[^55]采用了基于体素图的深度关联方法，但仅将激光雷达深度作为初始值。CamVox [^56]采用了非重复式固态激光雷达来构建深度图像，并将其与RGB图像一同输入到ORB-SLAM2 [^44]中以实现RGB-D SLAM。不过，CamVox中激光雷达与相机的视场之间不重叠的区域会被部分浪费。 综上所述，从实用性和可靠性角度来看，基于特征的方法更适合用于激光雷达增强型VISE。但无论采用何种深度关联方法，都可能出现错误或不准确的深度关联情况。主要原因是在物体边缘（如树木、车辆）处经常会发生遮挡现象。例如，若在树边缘检测到某个视觉特征，由于相机与激光雷达的外参误差，它可能会与树后墙壁上的激光雷达点相关联。因此，为了提升系统的鲁棒性，剔除激光雷达深度异常值至关重要。此外，还应进一步改进LDM模型，以发挥其仅依赖姿态框架的高效特性。如此一来，通过构建仅含姿态信息的视觉模型与LDM，我们就能实现高效且鲁棒的激光雷达增强型VISE。 ### 三、系统概述 所提出的PO-VINS是在LE-VINS [^33]的基础上进一步融入了仅依赖姿态的解决方案。引入该方案是为了提升PO-VINS中的状态估计效率。激光雷达用于为视觉路标提供精确的深度信息，从而提高系统的精度和鲁棒性；同时结合LO机制以节省计算资源。 图1展示了所提出的PO-VINS的系统流程。我们采用了以惯性导航系统（INS）为中心的[^4]处理框架，以充分利用INS的短期高精度特性。通过重力水平校准[^3]对INS进行初始化，从而粗略估计横滚角和俯仰角，而初始位置、速度和航向角则设为零。我们还可以由此推导出初始陀螺仪数据。 ![Image](images/d73dc8f712308f21d7dc504e87797d7b98524112e303322fb411b58b787578d6.jpg)
Fig. 1. System overview of the proposed PO-VINS. Here, the pose-only solution is integrated to improve the state-estimation efficiency. The LiDAR is employed to provide accurate depths for visual landmarks to improve the system's accuracy and robustness while consuming too many computational resources.
静止状态下的偏差。需要注意的是，也可以采用运动中的视觉对齐方法[^17]，但这超出了本文的讨论范围。利用初始化的状态即可进行高频INS计算，所获得的INS位姿将用于后续的相机和激光雷达数据处理。 利用从相机获取的连续视觉图像帧，采用 Shi 和 Tomasi [^5] 特征点检测方法，从分离的图像网格中并行检测特征点以提高效率。随后利用先验 INS 位姿来辅助 Lucas–Kanade 光流 [^57]，以增强跟踪连续性。通过 INS 辅助减少图像金字塔 [^4] 的使用，也可以提高跟踪效率。视觉关键帧通过判断关键帧间隔和相对运动 [^4] 来进行选择。只有关键帧中的特征点会被添加到路标管理器中进行状态估计，以提高效率。 同时，去除畸变后的高频INS位姿会被用于将累积的激光雷达点云投影到视觉关键帧时间点上。这样一来，特征对和激光雷达点便能在相机坐标系下的单位球中建立关联，进而通过平面拟合方法估算对应路标的激光雷达深度[^33]。为剔除激光雷达深度异常值，采用了多种异常值过滤方法，例如仅使用前景点云并进行平面检查[^33]。不过在非结构化环境中仍可能存在激光雷达深度异常值。因此，我们从仅含位姿的表示中推导出视觉路标的解析深度不确定性，并利用该不确定性来剔除激光雷达深度异常值。 最终，我们获得了一系列视觉特征，其中部分与激光雷达深度相关联。利用滑动窗口优化器将这些视觉特征、激光雷达深度以及IMU测量数据紧密融合，以进行最大后验估计。IMU中的陀螺仪和加速度计观测数据被用于构建IMU预积分测量模型[^58]。那些没有激光雷达深度的视觉特征则被用来构建仅含位姿的VRM。由于路标状态并未纳入状态向量，因此状态向量的维度得以降低。所提出的VRM采用紧耦合形式，因为IMU位姿状态被纳入了状态向量。 --- ![Image](images/2908fdf30c1efb6d7181840f0b8071f5adaa8d16331eda1d82cd0c6cb0b6c1fa.jpg)
Fig. 2. Illustration of the multiview geometry. The red frame sF_{s} and F_{n} represent the anchored frames of the landmark p^{w}. Note that, only keyframes are employed for state estimation.
对于那些具有激光雷达深度的视觉特征，我们提出了基于MSC的、仅含位姿形式的LDM，以减少测量数量并提高效率。通过将某个路标的所有特征观测值与激光雷达深度合并为一个测量值，系统对激光雷达深度异常值的鲁棒性也得到提升。通过整合视觉点、激光雷达深度和IMU测量数据进行非线性优化，即可实现仅含位姿的状态估计。此后，视觉路标的深度会被更新，同时利用更新后的深度来检测并剔除视觉特征异常值。与此同时，包括导航状态和IMU偏差在内的更新后的INS状态会被返回至INS计算模块，以实现实时高频导航。 ### 四、视觉路标的仅含位姿表示 本节介绍仅含位姿的视觉表示方法。我们首先从多视图几何中推导出视觉路标的仅含位姿表示。通过误差传播可得到路标的解析深度不确定性。基于推导出的不确定性，就能进一步检测并剔除激光雷达深度异常值。 ### A. 仅含位姿的视觉表示 考虑在世界坐标系（w-frame）中观测到的一个三维视觉地标$p^w$，该地标在多个图像中被捕捉到，如图2所示。相机坐标系（c-frame）的原点由$O_c$表示。$p^p = (u, v)$是图像平面即像素坐标系（p-frame）中观测到的特征。利用反投影函数$p^p$和相机内参$\pi_c^{-1}$，即[^2]，可以将$p^u = (x^u, y^u, 1)$转换为归一化相机坐标系（u-frame）。三维视觉地标$p^w$在u-frame中的投影方程可表示为： $$ \boldsymbol{p}^{\mathrm{u}}=\frac{1}{\mathcal{Z}^{\mathrm{c}}}\boldsymbol{p}^{\mathrm{c}}=\frac{1}{\mathcal{Z}^{\mathrm{c}}}\left(\mathbf{R}_{\mathrm{c}}^{\mathrm{w}}\right)^{-1}\left(\boldsymbol{p}^{\mathrm{w}}-\boldsymbol{p}_{\mathrm{wc}}^{\mathrm{w}}\right) $$ 其中，$\boldsymbol{p}^{\mathrm{c}} = (x^{\mathrm{c}}, y^{\mathrm{c}}, z^{\mathrm{c}})$为视觉地标在c-frame中的坐标，$\{\boldsymbol{p}_{\mathrm{wc}}^{\mathrm{w}}, \mathbf{R}_{\mathrm{c}}^{\mathrm{w}}\}$为相机相对于w-frame的姿态。 对于图2中的两张图像$\mathbf{F}_s$和$\mathbf{F}_\eta$，我们可以根据式(1)推导出以下方程： $$ z^{\mathrm{c}_{\eta}}p^{\mathrm{u}_{\eta}}=z^{\mathrm{c}_{s}}\mathbf{R}_{\mathrm{c}_{s}}^{\mathrm{c}_{\eta}}p^{\mathrm{u}_{s}}+p^{\mathrm{c}_{\eta}}_{\mathrm{c}_{\eta}\mathrm{c}_{s}} $$ 其中，$\{p_{c_{\eta}c_s}^{c_{\eta}},\mathbf{R}_{c_s}^{c_{\eta}}\}$是从c-frame $c_s$到c-frame $c_{\eta}$的相对变换，它们可表示为： $$ \left\{\begin{aligned}\boldsymbol{p}_{\mathrm{c}_{\eta}\mathrm{c}_{s}}^{\mathrm{c}_{\eta}}=\left(\mathbf{R}_{\mathrm{c}_{\eta}}^{\mathrm{w}}\right)^{-1}\left(\boldsymbol{p}_{\mathrm{w}\mathrm{c}_{s}}^{\mathrm{w}}-\boldsymbol{p}_{\mathrm{w}\mathrm{c}_{\eta}}^{\mathrm{w}}\right)\\ \mathbf{R}_{\mathrm{c}_{s}}^{\mathrm{c}_{\eta}}=\left(\mathbf{R}_{\mathrm{c}_{\eta}}^{\mathrm{w}}\right)^{-1}\mathbf{R}_{\mathrm{c}_{s}}^{\mathrm{w}}.\end{aligned}\right. $$ 将斜对称矩阵$[p^{u_\eta}]_\times$ [^59]从两侧左乘式(2)，即可得到： $$ z^{\mathrm{{c}}_{\varsigma}}\left[p^{\mathrm{{u}}_{\eta}}\right]_{\times}\mathbf{R}_{\mathrm{{c}}_{\varsigma}}^{\mathrm{{c}}_{\eta}}p^{\mathrm{{u}}_{\varsigma}}=-\left[p^{\mathrm{{u}}_{\eta}}\right]_{\times}p^{\mathrm{{c}}_{\eta}}_{\mathrm{{c}}_{\eta}\mathrm{{c}}_{\varsigma}}. $$ 取式(4)中的模，即可得出c-frame $z^{c_s}$中地标的深度$c_s$，其表达式为： $$ \begin{array}{r}{z^{\mathrm{c}_{s}}=\frac{\left\|\left[\boldsymbol{p}^{\mathrm{u}_{\eta}}\right]_{\times}\boldsymbol{p}_{\mathrm{c}_{\eta}\mathrm{c}_{s}}^{\mathrm{c}_{\eta}}\right\|}{\theta_{\varsigma,\eta}}\triangleq d_{\varsigma}^{(\varsigma,\eta)}}\end{array} $$ 其中$\theta_{\varsigma,\eta} = \|[p^{\mathrm{u}_\eta}]_\times \mathbf{R}_{\varsigma}^{\mathrm{c}_\eta} p^{\mathrm{u}_\varsigma}\|$。类似地，通过对式(2)的两侧左乘$z^{\mathrm{c}_\eta}$，可得到$[\mathbf{R}_{\varsigma}^{\mathrm{c}_\eta} p^{\mathrm{u}_\varsigma}]_\times$： $$ \begin{array}{r}{z^{\mathrm{c}_{\eta}}=\frac{\left\|\left[\mathbf{R}_{\mathrm{c}_{\varsigma}}^{\mathrm{c}_{\eta}}\boldsymbol{p}^{\mathrm{u}_{\varsigma}}\right]_{\times}\boldsymbol{p}_{\mathrm{c}_{\eta}\mathrm{c}_{\varsigma}}^{\mathrm{c}_{\eta}}\right\|}{\theta_{\varsigma,\eta}}\triangleq d_{\eta}^{(\varsigma,\eta)}.}\end{array} $$ 因此，式(2)所描述的两视图成像几何中的仅姿态约束可表示为： $$ \begin{array}{r}{d_{\eta}^{(\varsigma,\eta)}p^{\mathtt{u}_{\eta}}=d_{\varsigma}^{(\varsigma,\eta)}\mathbf{R}_{\mathtt{c}_{s}}^{\mathtt{c}_{\eta}}p^{\mathtt{u}_{s}}+p_{\mathtt{c}_{\eta}\mathtt{c}_{s}}^{\mathtt{c}_{\eta}}.}\end{array} $$ 式(7)中的仅姿态约束已被证明等价于两视图成像几何[^27]。假设$\mathbf{F}_{\varsigma}$和$\mathbf{F}_{\eta}$是视觉地标$\boldsymbol{p}^{\mathrm{w}}$的两个锚定坐标系，则可推导出一组约束$C(\varsigma, \eta)$如下： $$ \boldsymbol{C}\left(\boldsymbol{\varsigma},\boldsymbol{\eta}\right)=\left\{d_{j}^{\left(\varsigma,j\right)}\boldsymbol{p}^{\mathrm{u}_{j}}=d_{\varsigma}^{\left(\varsigma,\eta\right)}\mathbf{R}_{\mathrm{c}_{s}}^{\mathrm{c}_{j}}\boldsymbol{p}^{\mathrm{u}_{s}}+\boldsymbol{p}_{\mathrm{c}_{j}\mathrm{c}_{s}}^{\mathrm{c}_{j}},j\neq\varsigma\right\} $$ 其中$\mathbf{F}_j$表示另一个观测坐标系，如图2所示。式(8)是多视图几何中仅姿态的视觉表达方式，且已被证明与投影式(1)等价[^28]。仅姿态视觉表达（式8）的核心思想是：我们可以直接利用锚定坐标系的姿态和特征观测来表述地标状态，即深度。因此，可以从状态向量中移除视觉地标状态，从而提高状态估计的效率。 由此，可从式(8)推导出仅姿态的视觉测量值。需要注意的是，锚定坐标系$\mathbf{F}_{\varsigma}$和$\mathbf{F}_{\eta}$应构成地标$\mathbf{p}^{w}$的最大视差。幸运的是，参数$\theta_{\zeta,\eta}$可用于表示视差的大小[^28]。实际应用中，锚定坐标系$\mathbf{F}_{\varsigma}$被设为第一个观测到的坐标系或与激光雷达深度相关的坐标系，以便于滑动窗口边缘化处理。锚定坐标系$\mathbf{F}_{\eta}$可在其他坐标系中搜索，以匹配最大视差，从而最大化视觉约束并提高精度。 ### B. 视觉地标的解析深度不确定性 利用仅姿态深度（式5），可通过相机姿态和特征观测在锚定坐标系$d_{\zeta}^{(\zeta,\eta)}$中解析得到地标深度$F_{\zeta}$。我们还可以利用误差扰动[^3]来推导解析深度不确定性。在 --- ![Image](images/95a7e5518b8bcb285d6e34dd734aad3ba4ca8e3f4a4aebded2f05fc06ac7a57b.jpg)
Fig. 3. Relationship between the standard deviation of depth $\sigma_{d}$ and the parallax parameter $\theta_{\varsigma,\eta}$. The results are obtained from real-world datasets.
在紧密耦合的VISE系统中，相机姿态是通过IMU姿态状态和相机与IMU之间的外部参数计算得出的。由于INS姿态具有较高的短期精度，因此在长度通常为几秒的滑动窗口内，可以忽略相机的相对姿态误差。因此，深度不确定性主要由特征噪声引起。通常，特征噪声$\sigma_p$会被手动设定为一个固定值，例如在[^13]和[^17]中，而在PO-VINS中该值被设为$\sigma_p = 1.5$像素。我们可以得到特征点在u坐标系下的协方差矩阵$\Sigma_p^u$： $$ \boldsymbol{\Sigma}_{p}^{\mathrm{u}}=\left[\begin{array}{c c}{\left(\sigma_{p}\big/f_{x}\right)^{2}}&{0}\\ {0}&{\left(\sigma_{p}\big/f_{y}\right)^{2}}\\ \end{array}\right] $$ 其中$f_{x}$和$f_{y}$是内参中的焦距。 根据仅基于姿态的深度公式（5），我们可以写出关于特征误差$p^{u_s}$的雅可比矩阵： $$ \frac{\partial d_{S}^{(s,\eta)}}{\partial\boldsymbol{p}^{\mathrm{u}_{s}}}=\left(-\frac{\beta}{\theta^{3}}\left(\left(\left[\boldsymbol{p}^{\mathrm{u}_{\eta}}\right]_{\times}\mathbf{R}_{\mathrm{c}_{s}}^{\mathrm{c}_{\eta}}\right)^{\mathrm{T}}\left[\boldsymbol{p}^{\mathrm{u}_{\eta}}\right]_{\times}\mathbf{R}_{\mathrm{c}_{s}}^{\mathrm{c}_{\eta}}\boldsymbol{p}^{\mathrm{u}_{s}}\right)^{\mathrm{T}}\right)_{[0:1]} $$ 其中实数$\beta$可表示为： $$ \beta=\left\|\left[\boldsymbol{p}^{\mathrm{u}_{\eta}}\right]_{\times}\boldsymbol{p}_{\mathrm{c}_{\eta}\mathrm{c}_{\varsigma}}^{\mathrm{c}_{\eta}}\right\|. $$ 需要注意的是，由于原始特征点为二维，因此公式（10）中的第三维度并未被使用。类似地，关于特征误差$p^{\eta}$的雅可比矩阵可写为： $$ \begin{align*}\frac{\partial d_{\varsigma}^{(\varsigma,\eta)}}{\partial\boldsymbol{p}^{\mathrm{u}_{\eta}}}&=\left(-\frac{1}{\beta\theta}\left(\left(\left[\boldsymbol{p}_{\mathrm{c}_{\eta}\mathrm{c}_{\varsigma}}^{\mathrm{c}_{\eta}}\right]_{\times}\right)^{\mathrm{T}}\left[\boldsymbol{p}^{\mathrm{u}_{\eta}}\right]_{\times}\boldsymbol{p}_{\mathrm{c}_{\eta}\mathrm{c}_{\varsigma}}^{\mathrm{c}_{\eta}}\right)^{\mathrm{T}}\right.\\&\quad+\left.\frac{\beta}{\theta^{3}}\left(\left(\left[\boldsymbol{R}_{\mathrm{c}_{\varsigma}}^{\mathrm{c}_{\eta}}\boldsymbol{p}^{\mathrm{u}_{\varsigma}}\right]_{\times}\right)^{\mathrm{T}}\left[\boldsymbol{p}^{\mathrm{u}_{\eta}}\right]_{\times}\boldsymbol{R}_{\mathrm{c}_{\varsigma}}^{\mathrm{c}_{\eta}}\boldsymbol{p}^{\mathrm{u}_{\varsigma}}\right)^{\mathrm{T}}\right)_{[0:1]}.\end{align*} $$ 最终，地标深度$\Sigma_d = \sigma_d^2$的不确定性$d_\varsigma^{(\varsigma,\eta)}$可以通过解析方法计算得出： $$ \Sigma_{d}=\left(\frac{\partial d_{\varsigma}^{(\varsigma,\eta)}}{\partial\boldsymbol{p}^{\mathrm{u}_{\varsigma}}}\right)^{\mathrm{T}}\Sigma_{p}^{\mathrm{u}}\frac{\partial d_{\varsigma}^{(\varsigma,\eta)}}{\partial\boldsymbol{p}^{\mathrm{u}_{\varsigma}}}+\left(\frac{\partial d_{\varsigma}^{(\varsigma,\eta)}}{\partial\boldsymbol{p}^{\mathrm{u}_{\eta}}}\right)^{\mathrm{T}}\Sigma_{p}^{\mathrm{u}}\frac{\partial d_{\varsigma}^{(\varsigma,\eta)}}{\partial\boldsymbol{p}^{\mathrm{u}_{\eta}}}. $$ 图3展示了不同视差参数$\sigma_{d}$和深度下的深度不确定性$\theta_{s,\eta}$，即标准差。结果表明，当视差参数较大时，深度不确定性较小。 ![Image](images/94372930f8b2de386dc8ebc9b26670541724c821cdc86072d7622bd68c504dbf.jpg)
Fig. 4. Illustration of the LiDAR-depth outliers and inliers. The visual points denote the observed position of landmarks, where the LiDAR points represent the calculated position using associated LiDAR depths.
此外，深度越大，深度不确定性也越大。这些结果符合双视图几何理论[^2]。因此，这证明了公式（13）中推导出的解析深度不确定性是正确的。我们还注意到，深度不确定性$\sigma_{d}$可能超过几十厘米，这一数值远大于激光雷达的测量误差。这正是引入激光雷达增强能够提升估计精度的原因。 ### C. 基于不确定性的激光雷达深度外点剔除 在仅基于姿态的框架中，可以利用公式（13）解析计算深度不确定性。该深度不确定性可进一步用于检测并剔除激光雷达深度的外点。对于一个视觉地标，其在锚定坐标系$d_v$中的深度$\mathbf{F}_c$可利用两个锚定坐标系$\mathbf{F}_s$和$\mathbf{F}_{\eta}$下的相机姿态通过公式（5）计算得出。同时，深度不确定性$\Sigma_{d_v} = \sigma_d^2$也可利用公式（13）算出。假设该地标对应一个激光雷达深度$d_r$，且该激光雷达深度的标准差为$\sigma_{d_r}$。此处，标准差$\sigma_{d_r}$被设定为0.1米的固定值，该数值由LE-VINS中的深度关联方法确定[^33]。注意，深度$d_v$和$d_r$均位于c坐标系中。 所提出的基于不确定性的激光雷达深度外点剔除方法基于三西格玛准则。计算得到的深度$d_{v}$与激光雷达深度$d_{r}$之间的差异可表示为： $$ \varepsilon_{d}=d_{v}-d_{r}. $$ 根据随机变量的性质，差异$\varepsilon_{d}$的标准差可计算如下： $$ \sigma_{\varepsilon_{d}}=\sqrt{\sigma_{d_{r}}^{2}+\sigma_{d_{v}}^{2}}. $$ 因此，如果差值 $\varepsilon_d$ 不在 $\pm3\sigma_{\varepsilon_d}$ 范围内，则相应的激光雷达深度数据应被视为异常值。需要注意的是，激光雷达深度的异常值会被剔除，而与这些异常值相关的视觉特征对则会被保留下来，用于构建视觉测量数据。 图4展示了激光雷达深度的异常值与正常值示例。如图4所示，异常值出现在树边缘处，此时视觉点位于树上，而激光雷达点却位于墙上；对于正常值而言，视觉点与激光雷达点均位于墙上。根据实验结果可知，激光雷达深度的异常值主要出现在无结构环境中，尤其是存在大量前景和背景物体的场景。我们还在图5中展示了定性结果：采用所提出的异常值剔除方法后，与异常值相关联的视觉地标数量显著减少。 --- ![Image](images/b13671da6549666252f431e4ddbdb7fa53fdb32037687ffe0ed67f8fd7b335d1.jpg) ![Image](images/21fa5d9c6e3464ac3a1bec71005b9b8c26922c84a60ad14f0ceac0acd4708016.jpg)
(a)
(b)
Fig. 5. Impact of the outlier culling method for LiDAR depths. The blue rectangles denote the uninitialized landmarks. The cyan and green rectangles denote the initialized landmarks, and the green rectangles are associated with LiDAR depths. The big red rectangles represent the unstructured areas, i.e., the tree leaves, where outliers may frequently occur. With the proposed outlier culling algorithm, fewer landmarks are associated with LiDAR depth within the red rectangles. (a) Without outlier culling. (b) With outlier culling.
![Image](images/b318cab11a8023e70e94c97f4f976f6a91857965a7fad97ac0f4aebfd04ed5bd.jpg)
Fig. 6. FGO framework of the proposed PO-VINS. Here, the VRM and LDM represent the visual-reprojection measurement and LiDAR-depth measurement, respectively. The pose-only VRM may include two or three states. The two-state VRM is employed for all landmarks, while the three-state VRM is used for those landmarks with more than three feature observations. The pose-only MSC-based LDM compresses all observations of a single landmark and the LiDAR depth in one measurement. The IMU velocity and biases are not depicted in the figure for better visualization.
图中绿色矩形表示无结构树叶中的激光雷达深度数据（位于大红色矩形范围内）。同时，无论是否采用所提出的异常值剔除方法，地面和墙面上的结果几乎一致，如图5所示。总而言之，通过所提出的基于不确定性的异常值剔除方法，能够检测并去除激光雷达深度的异常值，从而提升系统的鲁棒性。 ### V. 仅位姿的VISE结合激光雷达增强 本节介绍了所提出的仅位姿的VISE结合激光雷达深度增强方法。首先介绍状态估计问题，以阐述PO-VINS的FGO框架，如图6所示。随后，将VRM和LDM均以仅位姿形式进行建模。我们还研究了深度估计方法，因为地标深度将被用于后续的异常值剔除和特征跟踪。最后，我们简要介绍了针对视觉特征的异常值剔除方法。 ### A. 紧耦合状态估计器 所提出的紧耦合状态估计器基于滑动窗口优化器，PO-VINS中的状态向量X可定义如下： $$ \begin{aligned}\boldsymbol{X}&=\left[\boldsymbol{x}_{0},\boldsymbol{x}_{1},\cdots,\boldsymbol{x}_{n},\boldsymbol{x}_{\mathrm{c}}^{\mathrm{b}}\right]\\\boldsymbol{x}_{k}&=\left[\boldsymbol{p}_{\mathrm{wb}_{k}}^{\mathrm{w}},\mathbf{q}_{\mathrm{b}_{k}}^{\mathrm{w}},\boldsymbol{\nu}_{\mathrm{wb}_{k}}^{\mathrm{w}},\boldsymbol{b}_{g_{k}},\boldsymbol{b}_{a_{k}},\boldsymbol{t}_{d_{k}}\right],\quad k\in[0,n]\\\boldsymbol{x}_{\mathrm{c}}^{\mathrm{b}}&=\left[\boldsymbol{p}_{\mathrm{bc}}^{\mathrm{b}},\mathbf{q}_{\mathrm{c}}^{\mathrm{b}}\right]\end{aligned} $$ 其中 $\mathbf{x}_k$ 表示每个时间节点的惯性测量单元（IMU）状态，包括位置 $\mathbf{p}_{\mathrm{wb}}^{\mathrm{w}}$、姿态四元数 $\mathbf{q}_{\mathrm{b}}^{\mathrm{w}}$ 以及w坐标系下的速度 $\mathbf{v}_{\mathrm{wb}}^{\mathrm{w}}$；$\mathbf{b}_g$ 和 $\mathbf{b}_a$ 分别为陀螺仪和加速度计的偏差；b表示IMU机体坐标系（b坐标系）；n表示滑动窗口内的IMU预积分次数；$\mathbf{x}_{\mathrm{c}}^{\mathrm{b}}$ 为相机与IMU之间的外部参数。姿态四元数 $\mathbf{q}$ 与旋转矩阵 $\mathbf{R}$ 是等价的 [^59]。需要注意的是，为了提升鲁棒性，相机与IMU之间的时延参数 $t_d$ 被建模为随机游走过程 [^3]，因为某些数据集中相机与IMU可能无法很好地同步。为简便起见，后续内容中省略了相机-IMU的时延参数。关于时延模型的实现细节，可参考GitHub上开源的LE-VINS项目 [^33] ()。 请注意，视觉地标状态参数（如逆深度或三维位置）并未包含在式(16)中。因此，所提出的状态估计器在VRM和LDM方面仅以位姿形式呈现。此外，所提出的仅位姿估计器属于紧耦合形式。PO-VINS中的状态估计是通过构建非线性优化问题来实现的，该问题可通过最小化所有测量值及先验的马氏距离[^9]之和来求解，如第17式所示（见下页底部）。其中，$e^V$是仅位姿VRM的残差；$l_i$是一个视觉地标；$F_\zeta$和$F_\eta$是其在第IV-A节中提到的两个锚定关键帧；$F_j, j \neq \zeta$是该地标$l_i$的另一个关键帧；$e^D$是基于MSC的仅位姿LDM的残差，它将某个视觉地标的所有特征观测值及其关联的激光雷达深度数据压缩为一个测量值；$e^{\text{Pre}}$是IMU预积分测量的残差；而$e^{\text{Prior}}$则是先验约束的残差，这些约束源自边缘化过程[^7]、[^25]。 PO-VINS采用了Ceres Solver [^60]——一个用于建模和解决大型优化问题的开源库。具体而言，Levenberg–Marquardt (LM) [^15]算法被用于求解式(17)中的非线性优化问题。同时，我们针对VRM和LDM采用了Huber鲁棒代价函数[^60]，以减少视觉特征和激光雷达深度异常值的影响。图6展示了所提出的PO-VINS的FGO框架。仅位姿VRM可能包含两到三个状态：当满足$\mathbf{F}_j = \mathbf{F}_{\eta}$条件时，会构建两状态VRM，并在FGO中采用双视图约束。 IMU预积分测量在两个连续的IMU状态之间构建相对位姿约束，同时纳入速度和IMU偏差约束。我们 --- 沿用了之前的工作来建模IMU预积分测量[^58]，其残差可表示为： $$ \begin{aligned}&\boldsymbol{e}^{\mathrm{Pre}}\left(\tilde{\boldsymbol{z}}_{k-1,k}^{\mathrm{Pre}},X\right)\\ &=\left[\begin{array}{c}\left(\mathbf{R}_{\mathrm{b}_{k-1}}^{\mathrm{w}}\right)^{\mathrm{T}}\left(\boldsymbol{p}_{\mathrm{wb}_{k}}^{\mathrm{w}}-\boldsymbol{p}_{\mathrm{wb}_{k-1}}^{\mathrm{w}}-\boldsymbol{\nu}_{\mathrm{wb}_{k-1}}^{\mathrm{w}}\Delta t_{k-1,k}\right)\\ -\frac{1}{2}g^{\mathrm{w}}\Delta t_{k-1,k}^{2}\end{array}\right)-\Delta\tilde{\boldsymbol{p}}_{k-1,k}^{\mathrm{Pre}}\\ &\quad\left(\begin{array}{c}\left(\mathbf{R}_{\mathrm{b}_{k-1}}^{\mathrm{w}}\right)^{\mathrm{T}}\left(\boldsymbol{\nu}_{\mathrm{wb}_{k}}^{\mathrm{w}}-\boldsymbol{\nu}_{\mathrm{wb}_{k-1}}^{\mathrm{w}}-g^{\mathrm{w}}\Delta t_{k-1,k}\right)-\Delta\tilde{\boldsymbol{\nu}}_{k-1,k}^{\mathrm{Pre}}\\ \mathrm{Log}\left(\left(\mathbf{q}_{\mathrm{b}_{k}}^{\mathrm{w}}\right)^{-1}\otimes\mathbf{q}_{\mathrm{b}_{k-1}}^{\mathrm{w}}\otimes\tilde{\mathbf{q}}_{k-1,k}^{\mathrm{Pre}}\right)\right.\\ \left.\quad\boldsymbol{b}_{g_{k}}-\boldsymbol{b}_{g_{k-1}}\right.\\ \left.\quad\boldsymbol{b}_{a_{k}}-\boldsymbol{b}_{a_{k-1}}\right.\end{array}\right].\end{aligned} $$ 其中，$\Delta\tilde{p}_{k-1,k}^{\mathrm{Pre}}$、$\Delta\tilde{v}_{k-1,k}^{\mathrm{Pre}}$和$\tilde{q}_{k-1,k}^{\mathrm{Pre}}$分别是位置、速度和姿态预积分观测值[^58]；$\mathbf{R}_{b}^{w}$表示四元数$\mathbf{q}_{b}^{w}$对应的旋转矩阵；$g^{w}$表示w坐标系下的重力矢量；$\mathrm{Log}(\cdot)$代表从四元数到旋转向量的转换[^59]；$\Delta t_{k-1,k}$是两个IMU状态之间的时间长度，即视觉关键帧的间隔。协方差矩阵$\Sigma^{\mathrm{Pre}}$是通过噪声传播得到的[^58]。 ### B. 仅位姿VRM 本部分推导仅位姿VRM的残差。根据仅位姿多视图约束(8)，锚定关键帧$\mathbf{F}_{\varsigma}$中的地标深度可表示为该锚定关键帧$\mathbf{F}_{\varsigma}$和$\mathbf{F}_{\eta}$的相机位姿的函数，即 $$ \begin{align*}d_{\varsigma}^{(\varsigma,\eta)}&\triangleq\frac{\left\|\left[\tilde{\boldsymbol{p}}^{\mathrm{u}_{\eta}}\right]_{\times}\boldsymbol{p}_{\mathrm{c}_{\eta}\mathrm{c}_{\varsigma}}^{\mathrm{c}_{\eta}}\right\|}{\theta_{\varsigma,\eta}}\\ \theta_{\varsigma,\eta}&=\left\|\left[\tilde{\boldsymbol{p}}^{\mathrm{u}_{\eta}}\right]_{\times}\mathbf{R}_{\mathrm{c}_{\varsigma}}^{\mathrm{c}_{\eta}}\tilde{\boldsymbol{p}}^{\mathrm{u}_{\varsigma}}\right\|\end{align*} $$ 其中，相对位姿$\{p_{\mathrm{c}_{\eta}\mathrm{c}_s}^{\mathrm{c}_\eta}, \mathbf{R}_{\mathrm{c}_s}^{\mathrm{c}_\eta}\}$可利用式(3)计算得出。此外，相机位姿$\{p_{\mathrm{wc}}^{\mathrm{w}}, \mathbf{R}_{\mathrm{c}}^{\mathrm{w}}\}$可表示为IMU位姿和外参的函数，即 $$ \begin{cases}\boldsymbol{p}_{\mathrm{wc}}^{\mathrm{w}}=\boldsymbol{p}_{\mathrm{wb}}^{\mathrm{w}}+\mathbf{R}_{\mathrm{b}}^{\mathrm{w}}\boldsymbol{p}_{\mathrm{bc}}^{\mathrm{b}}\\\mathbf{R}_{\mathrm{c}}^{\mathrm{w}}=\mathbf{R}_{\mathrm{b}}^{\mathrm{w}}\mathbf{R}_{\mathrm{c}}^{\mathrm{b}}\end{cases} $$ 其中，$\{p_{\mathrm{wb}}^{\mathrm{w}},\mathbf{R}_{\mathrm{b}}^{\mathrm{w}}\}$是式(16)中的IMU位姿状态，$\{p_{\mathrm{bc}}^{\mathrm{b}},\mathbf{R}_{\mathrm{c}}^{\mathrm{b}}\}$表示式(16)中的相机-IMU外参。 对于地标$\mathbf{F}_j, j \neq \varsigma$的某个观测关键帧$l_i$，仅位姿VRM的残差可根据式(8)推导得出，即 $$ \begin{aligned}\boldsymbol{e}^{V}\left(\tilde{\boldsymbol{z}}_{j,l_{i}}^{V_{s,\eta}},\boldsymbol{X}\right)&=\left[\begin{array}{l l}\boldsymbol{b}_{1}&\boldsymbol{b}_{2}\end{array}\right]^{\mathrm{T}}\left(\hat{\boldsymbol{p}}^{\mathrm{u}_{j}}-\tilde{\boldsymbol{p}}^{\mathrm{u}_{j}}\right)\\\hat{\boldsymbol{p}}^{\mathrm{u}_{j}}&=\frac{\hat{\boldsymbol{p}}^{\mathrm{c}_{j}}}{\|\hat{\boldsymbol{p}}^{\mathrm{c}_{j}}\|}\end{aligned} $$ 其中，$\hat{\boldsymbol{p}}^{u_j}$ 是在关键帧 $\mathbf{F}_j$ 的u坐标系下计算出的坐标。$\boldsymbol{b}_1 = \begin{bmatrix} 1 & 0 & 0 \end{bmatrix}^\top$ 和 $\boldsymbol{b}_2 = \begin{bmatrix} 0 & 1 & 0 \end{bmatrix}^\top$ 是两个正交基 [^4]、[^33]。需要注意的是，如果我们拥有 $\mathbf{F}_j = \mathbf{F}_{\eta}$，则残差就等同于双视图重投影残差。协方差 $\Sigma_{l_i}^{V \zeta, \eta}$ 也会像式(9)那样从像素平面传播到切平面。根据式(8)，计算得到的c坐标系坐标 $\hat{\boldsymbol{p}}^{c_j}$ 可以重新表示为 $$ \begin{array}{r}{\hat{\boldsymbol{p}}^{\mathrm{c}_{j}}=\boldsymbol{d}_{\varsigma}^{(\varsigma,\eta)}\mathbf{R}_{\mathrm{c}_{\varsigma}}^{\mathrm{c}_{j}}\tilde{\boldsymbol{p}}^{\mathrm{u}_{\varsigma}}+\boldsymbol{p}_{\mathrm{c}_{j}\mathrm{c}_{\varsigma}}^{\mathrm{c}_{j}}.}\end{array} $$ 考虑到式(19)，式(21)中的 $p^{u_j}$ 可以用更简洁的形式表示出来 $$ \begin{aligned}\hat{\boldsymbol{p}}^{\mathrm{u}_{j}}&=\frac{\theta_{S,\eta}\hat{\boldsymbol{p}}^{\mathrm{c}_{j}}}{\theta_{S,\eta}\left\|\hat{\boldsymbol{p}}^{\mathrm{c}_{j}}\right\|}=\frac{\hat{\boldsymbol{p}}^{j}}{\left\|\hat{\boldsymbol{p}}^{j}\right\|}\\\hat{\boldsymbol{p}}^{j}&=\left\|\left[\tilde{\boldsymbol{p}}^{\mathrm{u}_{\eta}}\right]_{\times}\mathbf{R}_{\mathrm{c}_{s}}^{\mathrm{c}_{\eta}}\tilde{\boldsymbol{p}}^{\mathrm{u}_{s}}\right\|\left(\mathbf{R}_{\mathrm{c}_{s}}^{\mathrm{c}_{j}}\tilde{\boldsymbol{p}}^{\mathrm{u}_{s}}+\boldsymbol{p}_{\mathrm{c}_{j}\mathrm{c}_{s}}^{\mathrm{c}_{j}}\right)\end{aligned} $$ 其中，$\hat{p}^j$ 是缩放后相机坐标系下的坐标（仅用于更清晰地表述）；相对位姿 $\{\vec{p}_{cj}^{c_j}, \mathbf{R}_{c_j}^{c_j}\}$ 也可以从式(3)和式(20)中求得。因此，计算得到的项 $\hat{p}^{u_j}$ 是IMU位姿 $\{\vec{p}_{wb_c}^{w}, \mathbf{R}_{b_c}^{w}\}, \{\vec{p}_{wb_j}^{w}, \mathbf{R}_{b_j}^{w}\}$ 和 $\{\vec{p}_{wb_n}^{w}, \mathbf{R}_{b_n}^{w}\}$ 以及相机-IMU外参 $\{\vec{p}_{bc}^{b}, \mathbf{R}_{c}^{b}\}$ 的函数。由此可见，式(21)中的VRM残差仅以位姿形式呈现，不涉及视觉路标状态。 ### C. 纯位姿LDM 如果某个视觉路标与激光雷达深度数据相关联，由于状态向量中没有深度状态，我们就需要构建纯位姿LDM。本部分介绍了两种纯位姿LDM模型：直接LDM和基于MSC的LDM，后者在PO-VINS中被采用。需要注意的是，与路标 $d_i$ 相关联的激光雷达深度 $l_i$ 始终位于锚定关键帧 $\mathbf{F}_c$ 的c坐标系中，这一关系由深度关联方法 [^33] 确定。 1) 直接LDM：由于可以利用IMU位姿状态和相机-IMU外参来计算路标深度，一种简单的LDM模型就是直接对式(19)中表达的路标深度进行约束。因此，直接LDM的残差可以表示为 $$ e^{D}\left(\tilde{\boldsymbol{z}}^{D_{i}},X\right)=\frac{\left\|\left[\tilde{\boldsymbol{p}}^{\mathrm{u}_{\eta}}\right]_{\times}\boldsymbol{p}_{\mathrm{c}_{\eta}\mathrm{c}_{s}}^{\mathrm{c}_{\eta}}\right\|}{\left\|\left[\tilde{\boldsymbol{p}}^{\mathrm{u}_{\eta}}\right]_{\times}\boldsymbol{R}_{\mathrm{c}_{s}}^{\mathrm{c}_{\eta}}\tilde{\boldsymbol{p}}^{\mathrm{u}_{s}}\right\|}-\tilde{d}_{i} $$ 其中，$d_i$ 是路标 $l_i$ 的激光雷达深度观测值，其对应的两个锚定关键帧为 $\mathbf{F}_s$ 和 $\mathbf{F}_{\eta}$；相对位姿 $\{\mathbf{p}_{c_\eta}^{c_\eta}, \mathbf{R}_{c_s}^{c_\eta}\}$ 与式(19)中的相同。式(24)中的激光雷达深度残差是IMU位姿 $\{\mathbf{p}_{wb_s}^{w}, \mathbf{R}_{b_s}^{w}\}$ 和 $\{\mathbf{p}_{wb_\eta}^{w}, \mathbf{R}_{b_\eta}^{w}\}$ 以及相机-IMU外参 $\{\mathbf{p}_{bc}^{b}, \mathbf{R}_c^{b}\}$ 的函数。因此，直接LDM属于纯位姿类型，可以直接约束位姿状态。 不过，为了避免观测信息丢失，针对单个路标应同时使用VRM和LDM。这样一来测量数量难免会增加，进而导致求解状态估计问题时的计算成本上升。此外，式(24)中的激光雷达深度约束仅作用于两个锚定关键帧的位姿。 $$ \arg\min_{\boldsymbol{X}}\frac{1}{2}\left\{\begin{array}{l}\sum_{j\neq\eta,l_{i},}\left\|\boldsymbol{e}^{V}\left(\tilde{\boldsymbol{z}}_{j,l_{i}}^{V_{\varsigma,\eta}},\boldsymbol{X}\right)\right\|_{\boldsymbol{\Sigma}_{l_{i}}^{V_{\varsigma,\eta}}}+\sum_{i\in[1,h]}\left\|\boldsymbol{e}^{D}\left(\tilde{\boldsymbol{z}}^{D_{i}},\boldsymbol{X}\right)\right\|_{\boldsymbol{\Sigma}^{D_{i}}}^{2}\\ i\in[1,m]\\ +\sum_{k\in[1,n]}\left\|\boldsymbol{e}^{\mathrm{P r e}}\left(\tilde{\boldsymbol{z}}_{k-1,k}^{\mathrm{P r e}},\boldsymbol{X}\right)\right\|_{\boldsymbol{\Sigma}_{k-1,k}^{\mathrm{P r e}}}^{2}+\left\|\boldsymbol{e}^{\mathrm{P r i o r}}\left(\tilde{\boldsymbol{z}}^{\mathrm{P r i o r}},\boldsymbol{X}\right)\right\|^{2}\end{array}\right\} $$ --- 因此，激光雷达深度异常值的影响更大，因为直接LDM并未利用与该路标相关的所有位姿状态。总而言之，直接LDM可能导致效率较低且可靠性下降。 2) 基于MSC的LDM：在著名的MSCKF算法中，可以利用左零空间投影来消除路标状态，并采用线性化测量值进行卡尔曼更新 [^10], [^11]。简而言之，MSC更新在某种程度上可被视为一种仅包含位姿的形式。受MSC更新的启发，我们提出了一种基于MSC的LDM，它将某个路标的所有特征观测值及其激光雷达深度数据压缩为单个测量值，同时保留仅含位姿的形式。因此，测量值的数量得以减少；由于第V-B节中介绍的仅含位姿的VRM不应再被使用以避免信息重复利用，故无需再次应用它。此外，通过利用所有特征观测值，还能降低激光雷达深度异常值的影响。 考虑一个路标 $l_i$，其在锚定关键帧 $\mathbf{F}_s$ 中的逆深度参数表示为 $\tau_i$。对于关键帧 $\mathbf{F}_j$ 中的特征观测，LE-VINS 中 VRM 的残差 [^33] 可写为 $$ \begin{array}{r}{e^{V}\left(\tilde{\boldsymbol{z}}_{j,l_{i}}^{V_{s}},\boldsymbol{X}\right)=\left[\boldsymbol{b}_{1}~\boldsymbol{b}_{2}\right]^{\mathrm{T}}\left(\frac{\hat{\boldsymbol{p}}^{\mathrm{c}_{j}}}{\|\hat{\boldsymbol{p}}^{\mathrm{c}_{j}}\|}-\tilde{\boldsymbol{p}}^{\mathrm{u}_{j}}\right)}\end{array} $$ 其中 $\tilde{\boldsymbol{p}}^{c_j}$ 是利用 $\tilde{\boldsymbol{p}}^{u_s}$ 计算出的坐标。$\tilde{\boldsymbol{p}}^{u_j}$ 和 $\tilde{\boldsymbol{p}}^{u_s}$ 表示u坐标系下的观测特征。类似地，LE-VINS中LDM的残差计算如下： $$ e^{D}\left(\tilde{\boldsymbol{\zeta}}^{D_{i}},\boldsymbol{X}\right)=\frac{1}{\tau_{i}}-\tilde{d}_{i}. $$ 最终，我们从多个观测关键帧中得到若干个VRM，以及针对路标 $l_{i}$ 的LDM。 对式(16)中的状态估计误差以及逆深度状态 $\tau_{i}$ 进行线性化处理，式(25)和式(26)中的残差可近似表示为 $$ \boldsymbol{e}_{i}\simeq\mathbf{J}_{Y_{i}}\delta Y_{i}+\mathbf{J}_{\tau_{i}}\delta\tau_{i}+n_{i} $$ 其中 $Y_i$ 是式(16)中与路标 $l_i$ 相关的状态部分，$n_i$ 是测量噪声矩阵，$\mathbf{J}_{Y_i}$ 和 $\mathbf{J}_{\tau_i}$ 是对应的雅可比矩阵。现在我们可以得到一个矩阵 $\mathbf{A}_i$，其列构成了 $\mathbf{J}_{\tau_i}$ 的左零空间的基 [^10], [^11]。式(27)中的残差可以通过在两边左乘 $\mathbf{A}_i^\mathrm{T}$ 来重写： $$ \mathbf{A}_{i}^{\mathrm{T}}e_{i}\simeq\mathbf{A}_{i}^{\mathrm{T}}\mathbf{J}_{Y_{i}}\delta Y_{i}+\mathbf{A}_{i}^{\mathrm{T}}n_{i}. $$ 因此，我们得到基于MSC的LDM的残差如下： $$ \boldsymbol{e}_{i}^{0}=\mathbf{A}_{i}^{\mathrm{T}}\boldsymbol{e}_{i}\simeq\mathbf{J}_{Y_{i}}^{0}\delta Y_{i}+n_{i}^{0} $$ 其中 $\mathbf{J}_{Y_i}^{0} = \mathbf{A}_i^\mathrm{T} \mathbf{J}_{Y_i}$ 和 $n_i^0 = \mathbf{A}_i^\mathrm{T} n_i$。此时的残差 $e_i^0$ 不再依赖于路标逆深度误差，因而可用于状态估计。由于单个路标的矩阵 $\mathbf{J}_{\tau_i}$ 维度很小，我们可以通过QR分解来求得 $\mathbf{A}_i^\mathrm{T}$。在特定条件下，零空间运算和舒尔补运算已被证明能保留相同的信息量 [^61]。因此，舒尔补也可作为推导式(29)中基于MSC的LDM的替代方法，但详细讨论超出了本文范围。 式(29)中基于MSC的LDM残差以仅含位姿的形式表达，既未使用路标状态，又融合了视觉特征和激光雷达深度观测值。因此，对于有激光雷达深度的路标，我们采用基于MSC的仅含位姿的LDM；否则则采用仅含位姿的VDM。需要注意的是，所提出的基于MSC的LDM能实现最优的状态估计，因为在优化过程中可通过多次迭代减小线性化误差。相比之下，传统的MSCKF仅进行一次线性化，导致估计结果并非最优 [^16]。 ### D. 视觉路标的深度估计 仅姿态的状态估计器并不包含地标点深度状态，从而提高了效率。这些深度可以通过解析方式估算，以便于剔除异常值并辅助INS特征跟踪。对于那些没有激光雷达深度的地标点，其深度由所锚定的关键帧的姿态明确表达$\mathbf{F}_s$和$\mathbf{F}_{\eta}$。因此，锚定关键帧中的地标点深度$\mathbf{F}_s$可以通过直接采用公式(19)中的仅姿态深度表示来更新，该过程需利用更新后的IMU姿态以及相机与IMU之间的外部参数。 对于那些具有激光雷达深度的地标点，我们结合视觉与激光雷达观测数据，采用仅深度的优化方法。具体而言，我们分别利用公式(25)和(26)中定义的VRM和LDM来构建新的非线性优化器。在此过程中，IMU姿态及相机-IMU外部状态被固定而不进行估计，仅对逆深度状态进行估算。估算出的逆深度状态被用于更新地标点深度及其在世界坐标系中的位置。通过这种仅深度的优化方式，激光雷达深度能够被反映到地标点深度中。如此一来，激光雷达深度异常值便能在后续异常值剔除模块中被进一步排除，相关内容将在下一部分阐述。最终，状态估计的效率应能得到小幅提升。 ### E. 异常值检测与剔除 非线性优化和深度估算完成后，我们会对视觉特征进行异常值检测与剔除。由于估计器中采用了Huber鲁棒代价函数，优化过程中异常值的影响可能并不显著。不过，借助误差统计便能轻松检测出这些异常值。利用估算出的视觉地标点深度，我们可以得到世界坐标系中的地标点位置。针对每个观测到的关键帧，均可计算出重投影误差，从而获取该地标点的一系列重投影误差值。若平均重投影误差超过了设定的特征噪声阈值$\sigma_{p}$，则该地标点、其特征观测值以及可能的激光雷达深度都会被视为异常值。否则，仅有重投影误差大于$3\sigma_{p}$的特征观测值会被移除，而视觉地标点则予以保留。这些保留下来的视觉地标点将用于INS辅助的特征跟踪[^4]。通过采用这种异常值剔除方法，那些受移动物体、重复纹理及光照变化影响的视觉特征将被去除，进而提升系统的鲁棒性。 ---
TABLE I DATASET DESCRIPTIONS
DatasetsRobotR3LIVEFusionPortableV2
CarrierWheeled robotHandheldVehicle
Camera1280*1024, 20 Hz1280*720, 15 Hz1024*768, 20 Hz
IMUADI ADIS16465, 200 HzBOSCH BMI088, 200 HzSensoror STIM300, 200 Hz
LiDARLivox Mid-70, 10 HzLivox AVIA, 10 HzOuster OS1-128, 10 Hz
Length7 sequences with 9550 s and 13.4 km6 sequences with 4462 s and 5.0 km5 sequences with 2527 s and 20.8 km
![Image](images/2db2c809b5ef477a4c8edcbf6d7ef2886bf472d88d96a2f8534d22533239fb8f.jpg)
Fig. 7. Equipment setup in the Robot dataset, and the carrier is a low-speed wheeled robot. All sensors are well-synchronized through hardware triggers, and the dataset is collected by the onboard ARM computer (Xavier).
### VI. 实验与结果 我们在不同载体的公共及私有数据集上开展了全面实验。首先介绍相关数据集与评估方法，随后展示精度与效率方面的结果，以检验所提出的PO-VINS算法。我们还进行了一系列消融实验，以便充分评估并验证所提出的方法。最后，给出了PO-VINS在机载ARM计算机上的实时性能表现。 ### A. 数据集与评估设置 1) 数据集：为全面评估所提出的方法，我们采用了公共及私有数据集。这些数据集由处于不同动态条件下的载体采集，包括低速机器人、手持设备以及高速车辆。每个数据集仅使用MEMS-IMU、摄像头和激光雷达进行定量评估。关于数据集的详细信息见表I。 在私有Robot数据集中使用了一种低速轮式机器人，其最大速度约为1.5米/秒。该数据集采用的传感器包括MEMS-IMU ADIS16465、RGB相机以及Livox公司的固态激光雷达Mid-70，如图7所示。这种固态激光雷达Mid-70具有非重复且不规则的扫描模式，有助于将视觉特征与激光雷达深度信息关联起来。所有传感器均通过GNSS接收机驱动的硬件触发器实现精准同步。高精度的真实位姿数据（位置误差不超过0.02米）是通过后处理的GNSS/INS组合导航软件获得的 [^3]。为了进行公平比较，选用了在LE-VINS中使用的七个序列 [^33]。由于Robot数据集是在校园内采集的，主要挑战性场景包括移动物体、光照变化以及重复性纹理。另外还采用了两个大规模序列来评估PO-VINS在机载ARM计算机上的实时性能，即图7中所示的NVIDIA Xavier（8核CPU，16GB内存）。 所采用的公开数据集为R3LIVE [^50]和FusionPortableV2 [^62]。R3LIVE数据集涵盖了室内外环境，难度较高。此外，由于手持式载体可能导致大幅角度运动，因此会产生运动模糊现象，尤其在光线较暗的室内环境中更为明显。由于R3LIVE数据集不包含真实位姿数据，因此可利用端到端误差来进行定量评估。故而该数据集中仅采用了六个包含端到端轨迹的序列。对于FusionPortableV2数据集，则选用了五个具有可靠真实位姿数据的车辆序列。由于车辆行驶速度极快，FusionPortableV2数据集比其他两个数据集更具挑战性，尤其是高速公路序列。在该数据集中，GNSS/INS组合导航结果被用作真实位姿参考。关于这两个数据集的更多细节可参见表I。 2) 评估设置：所提出的PO-VINS是基于LE-VINS() [^33]开发的。因此，LE-VINS被用作PO-VINS的基准系统。没有激光雷达增强功能的纯VINS系统分别表示为LE-VINS-WO和PO-VINS-WO。由于PO-VINS是基于优化的系统，因此选用了鲁棒性更强的先进（SOTA）VINS算法VINS-Mono [^17]。为了进行全面评估，还纳入了其他先进的紧耦合激光雷达视觉惯性导航系统，包括LVI-SAM [^53]、R2LIVE [^63]、R3LIVE [^50]、[^64]以及FAST-LIVO2 [^52]。由于在Robot数据集上机器人动态特性较低，我们为基于特征的系统提取了150个视觉特征；而在R3LIVE和FusionPortableV2数据集上则提取了200个特征，以适应更大的运动幅度。根据实验结果表明，为了兼顾精度与效率，LE-VINS-WO、LE-VINS、PO-VINS-WO和PO-VINS的滑动窗口大小均设为10。需要注意的是，这些系统均未采用显式的回环闭合机制。所有系统均在笔记本电脑（Intel i7-13700H，32GB内存）上的机器人操作系统（ROS）框架下运行。值得注意的是，由于Ouster OS1-128产生的点云密度过大，LVI-SAM、R2LIVE、R3LIVE和FAST-LIVO2无法在FusionPortableV2数据集上实时运行。相比之下，PO-VINS和LE-VINS能够在所有数据集上实时运行，这是因为它们的激光雷达深度增强功能轻量且高效，无需使用优化器。 由于Robot和FusionPortableV2数据集中包含了真实姿态信息，因此采用ATE进行定量评估。具体而言，我们采用EVO [^65]来计算ATE。此外，我们还利用RTE来评估PO-VINS的鲁棒性。这是因为RTE能够反映短时间内的一致性。 ---
TABLE II
COMPARISON OF THE ATES ON THE ROBOT DATASET
ATE (m)VINS-MonoFAST-LIO2LVI-SAMR2LIVEFAST-LIVO2R3LIVELE-VINS-WOLE-VINSPO-VINS-WOPO-VINS
Exp-1 (2554 m)4.674.679.165.052.324.491.861.461.741.50
Exp-2 (2533 m)2.534.961.962.622.742.651.281.161.521.20
Exp-3 (1151 m)2.510.782.051.142.650.800.840.810.860.77
Exp-4 (1657 m)1.650.830.820.670.220.750.930.841.050.90
Exp-5 (2321 m)3.645.917.372.484.534.931.532.451.710.80
Exp-6 (1539 m)5.392.735.192.022.367.902.051.611.781.86
Exp-7 (1715 m)3.733.672.243.060.973.451.340.760.750.81
Average3.453.224.112.432.263.571.401.301.341.12
LE-VINS-WO和PO-VINS-WO表示未采用激光雷达增强的配置。加粗的结果代表不同方法中的最佳表现，而带下划线的结果则为次优表现。 ![Image](images/63f951bfb27620b33f7d6d44e7210def60d8b4186c213c4d18c97004be2c6ffe.jpg)
Fig. 8. Position errors on the Robot-Exp-5 sequence with the initial point aligned. Here, only the LVINSs are employed for better visualization.
像25米这样的距离能够反映短时间内的一致性。对于R3LIVE数据集，我们采用三维端到端误差来进行评估。 ### B. 精度评估 1) 私有Robot数据集：表II展示了在私有Robot数据集上的评估结果。这七个序列最初用于LE-VINS，它们均在复杂的校园环境中采集。根据结果可知，与现有最先进方法相比，LE-VINS和PO-VINS在平均ATE方面取得了显著提升，且仅需轻量级的激光雷达增强而非复杂的激光雷达里程计。这证明了激光雷达增强型VINS的实用性与可靠性。不过，涉及激光雷达里程计的系统，即FAST-LIO2、LVI-SAM、R2LIVE、R3LIVE和FAST-LIVO2，在小规模环境下的Exp-4序列中表现更优。激光雷达子系统能够与它们构建的点云地图相匹配，从而消除漂移现象。作为基于视觉的里程计，PO-VINS即便在相同场景下也无法消除漂移。我们在图8中进一步展示了LVINS在Exp-5序列中的位置误差。与LVI-SAM、R2LIVE、R3LIVE和FAST-LIVO2相比，LE-VINS和PO-VINS在z轴上的漂移更小。PO-VINS在x轴和y轴上的表现也更为优异，而LE-VINS在y轴上的漂移最大。 此外，表II显示PO-VINS-WO的平均精度几乎与LE-VINS-WO相当。这表明仅基于姿态的视觉表征适用于紧耦合VINS，且不会损失精度。与LE-VINS-WO相比，除Exp-5外，LE-VINS在所有序列中通过引入激光雷达增强技术也提升了精度。Exp-5序列中精度下降的原因可能是激光雷达深度异常值所致。相比之下，PO-VINS在Exp-5序列中表现最佳，所提出的基于不确定性的异常值剔除机制是原因之一。由于采用了所提出的基于MSC的LDM，PO-VINS能够利用所有视觉特征与激光雷达深度来构建该模型，从而对激光雷达深度异常值更具鲁棒性。更多消融实验结果详见第VI-D节。与PO-VINS-WO相比，PO-VINS的平均绝对平移精度提升了16.4%。由于原始的VINS系统PO-VINS-WO已具备令人满意的精度，因此PO-VINS的提升并不十分显著。需要注意的是，PO-VINS在Exp-6和Exp-7序列中出现的轻微精度下降属于正常现象，因为该系统受诸多因素影响，尤其是在如此复杂的实验环境中。 由于短期相对误差能在一定程度上反映系统的鲁棒性，我们也通过评估25米以上的RTE来检验所提出的仅位姿估计方法。需要注意的是，旋转或姿态误差可以在RTE中体现出来；因此，我们仅采用RTE作为评价指标。图9展示了Exp-2和Exp-7实验中25米以上的RTE结果。从结果来看，LE-VINS-WO与PO-VINS-WO的RTE几乎相近，尽管PO-VINS-WO在各序列中的最大RTE要小得多。因此，这些结果表明所提出的仅位姿估计VINS在短时间精度方面能达到类似的鲁棒性。在引入激光雷达增强后，PO-VINS展现出的RTE与LE-VINS相当。这证明所提出的基于MSC的仅位姿估计LDM能具备与传统激光雷达增强型VINS相同的局部一致性。 2) 公开的R3LIVE-手持数据集：低速机器人数据集在动态条件方面的挑战性较低。因此，我们选用R3LIVE-手持数据集来评估所提出的方法，因为该数据集中频繁出现大幅角运动。表III展示了各系统的最终端到端误差，图10则描绘了LVINS在hkust-campus01序列上的轨迹。 ---
TABLE III
COMPARISON OF THE END-TO-END ERRORS ON THE R3LIVE DATASET
ATE (m)VINS-MonoFAST-LIO2LVI-SAMR2LIVEFAST-LIVO2R3LIVELE-VINS-WOLE-VINSPO-VINS-WOPO-VINS
main-building (1037 m)failed1.388.330.541.270.112.700.464.030.33
hkust-campus00 (1317 m)failed5.2915.690.055.965.2013.7612.5911.4310.72
hkust-campus01 (1524 m)failed2.1917.003.285.6819.575.713.035.722.43
hkust-campus02 (504 m)11.670.0815.051.630.010.085.613.066.142.54
hku-park0 (247 m)1.600.062.590.080.040.080.761.361.391.64
hku-park1 (402 m)26.610.560.550.550.540.61.110.841.270.83
AverageInvalid1.599.871.022.254.274.943.555.003.08
![Image](images/bf5b5ff51f8c10e91431f969ded2343096858ed334817193f16d671ea9fb05ee.jpg)
(a)
![Image](images/ffae88f153938223ad14ee4a38e15a1b70ca428570a89cad2738af323fbd3d3d.jpg)
(b)
Fig. 9. Comparison of the RTEs over 25 m. The results demonstrate the necessity of the proposed LiDAR-depth enhancement. (a) Robot-Exp-2 sequence. (b) Robot-Exp-7 sequence.
从结果来看，VINS-Mono在三个序列上均表现不佳，主要原因是室内环境中的大幅角运动导致了运动模糊。此外，视觉-惯性时延参数会随时间显著变化。因此，被建模为随机常数的VINS-Mono无法估算出发生变化的时延参数。相比之下，得益于以INS为中心的设计，LE-VINS-WO和PO-VINS-WO能够在所有序列上顺利运行。同时，我们将视觉-惯性时延参数建模为随机游走过程，因此能够随时间准确估算出该参数。 由于环境结构较为规整，激光雷达系统在R3LIVE-手持数据集上的表现更佳。因此，R2LIVE取得了最佳的平均结果，FAST-LIO2和FAST-LIVO2也表现出优异的性能。R3LIVE在khust-campus01序列上的结果较差，因此其平均端到端误差较大。LVI-SAM的结果也不理想，因为它主要是为旋转式激光雷达设计的，而非R3LIVE数据集中使用的Livox激光雷达。不过，根据图10(c)中的R2LIVE结果可知，基于激光雷达的系统能够匹配其先前构建的点云地图，从而得出较低的端到端误差。这正是FAST-LIO2、R2LIVE、R3LIVE和FAST-LIVO2在某些序列上能够实现厘米级端到端误差的原因。不过，若采用ATE进行评估，它们的表现可能会更差。 纯VINS系统PO-VINS-WO和LE-VINS-WO的平均误差相近。在引入激光雷达深度增强后，除hku-park0外，LE-VINS和PO-VINS在所有序列上的精度均明显优于各自的VINS系统。原因在于hku-park0是一处光线昏暗的公园，园内分布着大量无规则形态的树木和灌木。因此，受激光雷达深度异常值的影响，使用离群点剔除方法（第V-E节）后，仅有少量有效的视觉特征能被保留下来。此外，PO-VINS和LE-VINS在hkust-campus00序列上的结果较差，主要是因为较大的角运动会频繁干扰特征跟踪。不过，PO-VINS的平均误差仍低于LE-VINS。总体而言，在R3LIVE数据集上，PO-VINS展现出比基线系统LE-VINS更强的鲁棒性。在main-building、hkust-campus01和hku-park1等序列上，PO-VINS的精度也与紧耦合的LVINS相当，这也体现了轻量化激光雷达增强型VINS的实用性。 3) 公共FusionPortableV2-车辆数据集：由于先前数据集中的机器人和行人移动速度非常慢，仅为每秒几米左右，因此采用了高速的FusionPortableV2-车辆数据集。然而，由于传感器不兼容，我们无法运行LVI-SAM和R2LIVE算法。FusionPortableV2数据集上的ATE结果如表IV所示。VINS-Mono的ATE表现最差，尤其是在高速公路序列中。FAST-LIO2、R3LIVE和FAST-LIVO2的表现也较差，且无法在高速公路序列上运行。相比之下，所提出的PO-VINS能够在所有序列上顺利运行，并展现出最佳结果。因此，这种经过激光雷达增强的VINS即便不使用里程计，也表现出卓越的鲁棒性和可靠性。 PO-VINS-WO的平均ATE值低于LE-VINS-WO，这一差异源于highway00序列中的显著区别。不过，在其他序列上，LE-VINS-WO和PO-VINS-WO的精度几乎相当。 --- ![Image](images/5dcef80a15afb5dbd7caab656e378f64dbd744dd6f85ae6a5a82a9db28f7c736.jpg)
(a)
![Image](images/daa24a55c7edf638edead9dd56c2689245a52afdf7e3b2c7b1058875834734d8.jpg)
(b)
![Image](images/c89838eb90d284a6f11da7f3fd40a456d73652993d0cd86c0e267dcf3ca6d07f.jpg)
(c)
Fig. 10. Trajectories on the R3LIVE-hkust-campus01 sequence for LVINSs. The cyan rectangle in (c) denotes the abnormal height result for R2LIVE. (a) Whole trajectories. (b) Trajectories at the end point. (c) Height (z-axis) changes.
TABLE IV
COMPARISON OF THE ATES ON THE FUSIONPORTABLEV2 DATASET
ATE (m)VINS-MonoFAST-LIO2FAST-LIVO2R3LIVELE-VINS-WOLE-VINSPO-VINS-WOPO-VINS
campus00 (2.60 km)24.375.246.025.426.324.493.752.48
campus01 (2.06 km)13.875.517.75.646.355.996.226.90
downhill00 (3.74 km)71.636.540.87157.894.053.434.582.81
highway00 (9.33 km)failedfailedfailedfailed31.9825.9113.889.27
highway01 (3.05 km)141.9820.81162.17failed12.97.9910.457.93
AverageInvalidInvalidInvalidInvalid12.329.567.785.88
![Image](images/645a48145e9ec2675f4ae799e185e65b8120f130ffd295699c673f4275effb0e.jpg)
(a)
![Image](images/4295702d4fc313ee354c82be0aae92e4900be02097d400a92d52706a3bbe5f44.jpg)
(b)
![Image](images/5a0672858f4f77e2241cb4710e1110d793fc2709dda70cd60252abedb6552298.jpg)
(c)
Fig. 11. Trajectories on the FusionPortableV2-campus00 sequence for LVINSs. (a) Whole trajectories. (b) Trajectories at the end point. (c) Height (z-axis) changes.
通过引入激光雷达增强技术，PO-VINS的精度相比PO-VINS-WO有所提升，尤其在高速公路序列中表现突出。得益于所提出的仅基于位姿的激光雷达深度处理方法，PO-VINS的精度也优于LE-VINS。 第VI-B节中的精度结果表明，仅基于位姿的VINS，即PO-VINS-WO，其精度与传统的基准系统LE-VINS-WO相近。 ### C. 效率评估 图11展示了campus00序列上的轨迹。与其他系统相比，PO-VINS的轨迹与真实值对齐程度更好，且其终点距离真实值最近。综上所述，结果表明所提出的仅基于位姿的方法在高速车辆数据集上具有鲁棒性，且精度优于基准系统LE-VINS。 该结果与传统的基准系统LE-VINS-WO相近。此外，通过引入所提出的不确定性驱动的异常值剔除机制和基于MSC的LDM，仅基于位姿的激光雷达增强型VINS（即PO-VINS）在精度和鲁棒性方面均优于LE-VINS。不过，仅基于位姿的解决方案的主要优势在于更高的状态估计效率。包含FGO和边缘化过程的状态估计时间的统计结果如表V所示。 与VINS基准系统LE-VINS-WO相比，PO-VINS-WO的平均状态估计时间缩短了约33.8%。这一结果是合理的，因为根据我们的分析，仅使用基于位姿的VRM最多只能将状态向量的维度减少约50%。由于引入了额外的LDM，LE-VINS的时间开销略有增加。而PO-VINS的时间开销更短，其平均…… ---
TABLE V
COMPARISON OF THE STATE-ESTIMATION TIME ON THE ROBOT DATASET
Time (ms)LE-VINS-WOLE-VINSPO-VINS-WOPO-VINS
Exp-124.624.516.216.3
Exp-222.824.215.616.2
Exp-322.024.313.816.1
Exp-422.024.714.315.9
Exp-524.524.615.716.2
Exp-624.624.215.516.7
Exp-721.023.215.716.2
Average23.124.215.316.2
状态估计的时间开销包括因子图优化和边缘化过程。 与基准系统LE-VINS相比，状态估计时间缩短了33.1%。这一成果得益于所提出的基于MSC的LDM和仅深度优化技术。基于MSC的LDM能够将视觉特征与视觉路标的激光雷达深度观测数据整合为单一测量值，从而显著减少测量数量。同时，通过仅深度优化可以识别出激光雷达深度异常值，在保持卓越鲁棒性的前提下减少测量次数。下一节将开展消融实验，以评估这些方法所带来的影响。 效率结果表明，即便有激光雷达的辅助，仅基于姿态的解决方案在紧耦合视觉惯性导航系统中也能显著提升状态估计的效率。此外，在计算资源有限的机载ARM计算机上，效率提升会更加明显。尽管由于状态估计问题的规模较小，在这种实时配置下效率提升幅度不到100%，但结果足以改善实时性能。 ### D. 消融实验 开展消融实验是为了评估基于不确定性的异常值剔除方法、基于MSC的LDM以及仅深度优化这三种技术的影响。表VI展示了不同配置下的ATE和状态估计时间。 1) 基于不确定性的异常值剔除的影响：所提出的基于不确定性的异常值剔除方法能够检测并剔除激光雷达深度数据中的异常值，从而提高定位精度。如表VI所示，未采用该异常值剔除方法时，四个序列的ATE均有所增大，尤其是在Exp-1和Exp-6序列中。其余三个序列的ATE则几乎保持不变。与未采用异常值剔除的配置相比，所提出的PO-VINS的平均ATE降低了约21.7%。此外，无论是否采用异常值剔除，状态估计的时间开销几乎相同。由于异常值剔除后LDM数量减少，PO-VINS的状态估计效率略有提升。消融实验结果表明，针对激光雷达深度的基于不确定性的异常值剔除方法能够有效检测并剔除异常值，进而提升定位精度。
TABLE VI
ATES AND THE STATE-ESTIMATION TIME ON THE ROBOT DATASET WITH DIFFERENT CONFIGURATIONS
w/o outlier rejection $^{1}$Using direct LDM $^{2}$w/o depth-only optimization $^{3}$Proposed PO-VINS
ATE (m)Time (ms)ATE (m)Time (ms)ATE (m)Time (ms)ATE (m)Time (ms)
Exp-12.2716.72.222.11.7421.31.5016.3
Exp-21.1816.21.6921.91.3819.81.2016.2
Exp-30.7716.60.6619.90.8119.30.7716.1
Exp-40.8516.31.2520.00.8618.80.9015.9
Exp-51.0117.61.4921.00.5720.70.8016.2
Exp-62.8916.72.3720.41.7020.61.8616.7
Exp-71.0215.61.0120.90.8518.40.8116.2
Average1.4316.51.5220.91.1319.81.1216.2
未采用第IV.C节中提出的基于不确定性的异常值剔除方法。 $^{2}$采用第V.C.1节中的直接LDM。 $^{3}$未采用第V.D节中介绍的仅深度优化方法。 精度方面。需要注意的是，所提出的异常值剔除方法运行速度很快，平均时间开销约为0.2毫秒。 2) 基于MSC的LDM的影响：所提出的仅基于姿态的基于MSC的LDM将某个地标的全部视觉特征与激光雷达深度观测值整合为单一测量值。因此，如表VI所示，它能同时提升系统的鲁棒性和效率。与直接LDM配置相比，所提出的PO-VINS对激光雷达深度异常值的鲁棒性更强，因为所有特征观测值都被纳入考量；而直接LDM仅使用两个锚定关键帧中的特征观测值。同时，由于将所有相关观测值整合为一个测量值，LDM数量显著减少，进而状态估计效率也得以提升。结果表明，所提出的仅基于姿态的基于MSC的LDM既精准又高效。 3) 仅深度优化的影响：开展仅深度优化是为了更新与激光雷达深度相关的地标的深度值。根据表VI中的结果，采用所提出的PO-VINS后，在不牺牲ATE的前提下，效率较未采用仅深度优化的配置有所提升。效率提升的原因是仅深度优化能够识别出激光雷达深度异常值并将其剔除。最终，LDM数量略有减少，状态估计效率得以提升且不影响精度。仅深度优化的平均时间开销约为1.8毫秒。因此，采用所提出的仅深度优化方法具有重要意义。 ### E. 实时性能 我们在机载ARM计算机（搭载8核CPU和16GB内存的NVIDIA Xavier）上开展实验，以评估所提出的PO-VINS的实时性能。机载计算机上采用的LE-VINS和PO-VINS配置与之前在笔记本电脑上运行的配置相同。即便采用与笔记本电脑相同的配置，PO-VINS和LE-VINS都能在机载计算机上实现实时运行。 --- ![Image](images/f4ec6df9eb6063e85d2f0a905c00b804c46c6474c3e2bdbbaf2ad90a7b4890e9.jpg)
(a)
![Image](images/bcdbd458b7cdef74dcff8fdc103464b05bead33dcb3cf3aa7c333918287f04ab.jpg)
(b)
Fig. 12. Trajectories on the real-time experiments. (a) Test trajectory on the Seq-1 (2012 s and 2565 m). (b) Test trajectory on the Seq-2 (1162 s and 1462 m).
TABLE VII
PROCESSING TIME AND ATES ON THE REAL-TIME EXPERIMENTS
Front-end $^{1}$ (ms)Back-end $^{2}$ (ms)ATE (m)
LE-VINSPO-VINSLE-VINSPO-VINSLE-VINSPO-VINS
Seq-12826109482.822.05
Seq-22524104452.201.64
Average26.525.0106.546.52.511.85
$^{1}$前端处理时间包括图像预处理、特征检测与跟踪，以及深度关联。 $^{2}$后端处理时间，即状态估计时间，包括因子图优化和边缘化操作。 需要注意的是，深度关联和因子图优化仅在选取新关键帧时才进行，平均关键帧间隔约为0.3秒。因此，LE-VINS和PO-VINS都能在机载ARM计算机上实现实时运行。 图12展示了所使用的大规模序列Seq-1（时长2012秒，长度2565米）和Seq-2（时长1162秒，长度1462米）的轨迹。这两个序列也是由图7所示的轮式机器人采集的。如表VII所示，与基线系统LE-VINS相比，PO-VINS的前端时间几乎相同，而平均后端时间则缩短了56.3%。这是因为FGO仅在选取新关键帧时才进行，且平均关键帧间隔约为0.3秒。这一结果说明，所提出的仅姿态求解方案在计算资源有限的机载ARM计算机上效率更高。同时，在ATE指标方面，PO-VINS在这两条序列上的精度也明显优于LE-VINS。
TABLE VIII
TOTAL RUNNING TIME AND EQUIVALENT FPS ON THE REAL-TIME EXPERIMENTS
Sequence length (s)Total running time (s)Equivalent FPS
LE-VINSPO-VINSLE-VINSPO-VINS
Seq-12012131594430.642.6
Seq-2116283061028.038.1
等效FPS是通过将序列长度除以运行时间再乘以相机帧率得出的，其中相机帧率为20 FPS。 我们评估了LE-VINS和PO-VINS的总运行时间，并计算了等效帧率（FPS），结果如表VIII所示。此处等效FPS是通过将序列长度除以运行时间再乘以相机帧率（20帧/秒）得出的。PO-VINS在两条序列上的总运行时间分别缩短了28.2%和26.5%；此外，PO-VINS的FPS分别提升了39.2%和36.1%。 根据我们的统计，Xavier上PO-VINS和LE-VINS的平均CPU占用率分别为约14%（占用250MB内存）和18%（占用260MB内存）。简而言之，仅需两个CPU核心（CPU负载为25%）就足以让PO-VINS和LE-VINS在Xavier上实现实时运行，即便采用与笔记本电脑相同的配置也是如此。此外，PO-VINS的系统延迟主要由前端和后端处理过程导致，约为72毫秒，略大于50毫秒的相机拍摄间隔。不过，后端优化每300毫秒才进行一次。另外，低延迟惯性测量单元（IMU）可用于在不等待视觉处理的情况下提供高频的惯性导航系统（INS）先验姿态。实时实验结果证明，所提出的仅姿态求解方案和PO-VINS有效提升了机载ARM计算机上的运行效率。因此，PO-VINS可成为一种高效且稳健的里程计解决方案。 ### VII. 结论与讨论 本研究提出了一种紧密耦合的、基于激光雷达增强的视觉惯性导航系统（VINS），采用纯位姿求解方案来实现高效且鲁棒的状态估计。在所提出的纯位姿VINS中，地标点深度由锚定关键帧的位姿显式表达，因此可以将地标点状态从状态向量中直接移除。实验结果表明，纯位姿VINS的精度与传统VINS相近，即便在实时配置下，状态估计效率仍提升了33.8%。在纯位姿框架内，我们解析推导出了视觉地标的深度不确定性，借此可利用该深度不确定性来检测并剔除激光雷达深度异常值，事实证明这一方法十分有效。此外，我们提出了一种基于多尺度卷积（MSC）的激光雷达增强模块（LDM），从而将激光雷达增强功能无缝集成到纯位姿求解方案中。相较于基线系统LE-VINS，所提出的PO-VINS在多种载体采集的数据集上均展现出更高的效率和鲁棒性。具体而言，在私有机器人数据集上，其在笔记本电脑（Intel i7-13700H）上的状态估计效率提升了30%以上，在机载ARM计算机（NVIDIA Xavier）上的提升幅度更是达到50%。 --- 我们认为，纯位姿视觉惯性状态估计（VISE）可以替代传统的基于光束法平差（BA）的方法，因为它在保持同等精度的同时效率更高。通过采用轻量化的激光雷达深度增强技术，所提出的PO-VINS在大多数情况下能达到与引入激光雷达里程计数据的紧密耦合激光雷达视觉惯性导航系统（LVINS）相当的精度，实用性优势显著。不过我们注意到，由于场景视觉质量较差，PO-VINS在R3LIVE-Handheld数据集上的表现并不理想。未来的工作包括自适应地融入激光雷达里程计（LO）以构建紧密耦合的LVINS，同时维持优异的效率与鲁棒性。此外，我们还计划集成全球卫星导航系统（GNSS）来构建多传感器融合系统，从而实现无漂移的定位方案。 ## 参考文献 [^1]: J. Janai, F. Güney, A. Behl, and A. Geiger, “Computer vision for autonomous vehicles: Problems, datasets and state of the art,” Found. Trends Comput. Graph. Vis., vol. 12, no. 1–3, pp. 1–308, 2020. [^2]: R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision. Cambridge, U.K.: Cambridge Univ. Press, 2003. [^3]: P. D. Groves, Multiple View Geometry in Computer Vision. Norwood, MA, USA: Artech House, 2008. [^4]: X. Niu, H. Tang, T. Zhang, J. Fan, and J. Liu, "IC-GVINS: A robust, real-time, INS-centric GNSS-visual-inertial navigation system," IEEE Robot. Autom. Lett., vol. 8, no. 1, pp. 216–223, Jan. 2023. [^5]: J. Shi and Tomasi, “Good features to track,” in Proc. IEEE Conf. Comput. Vis. Pattern Recognit., Oct. 1994, pp. 593–600. [^6]: E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, "ORB: An efficient alternative to SIFT or SURF," in Proc. Int. Conf. Comput. Vis., Barcelona, Spain, Nov. 2011, pp. 2564–2571. [^7]: J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 40, no. 3, pp. 611–625, Mar. 2018. [^8]: C. Forster, Z. Zhang, M. Gassner, M. Werlberger, and D. Scaramuzza, "SVO: Semidirect visual odometry for monocular and multicamera systems," IEEE Trans. Robot., vol. 33, no. 2, pp. 249–265, Apr. 2017. [^9]: T. D. Barfoot, State Estimation for Robotics. Cambridge, U.K.: Cambridge Univ. Press, 2017. [^10]: A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint Kalman filter for vision-aided inertial navigation,” in Proc. IEEE Int. Conf. Robot. Autom., Apr. 2007, pp. 3565–3572. [^11]: M. Li and A. I. Mourikis, “High-precision, consistent EKF-based visual-inertial odometry,” Int. J. Robot. Res., vol. 32, no. 6, pp. 690–711, May 2013. [^12]: M. Bloesch, M. Burri, S. Omari, M. Hutter, and R. Siegwart, “Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback,” Int. J. Robot. Res., vol. 36, no. 10, pp. 1053–1072, Sep. 2017. [^13]: P. Geneva, K. Eckenhoff, W. Lee, Y. Yang, and G. Huang, “OpenVINS: A research platform for visual-inertial estimation,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2020, pp. 4666–4672. [^14]: L. Wang, T. Zhang, Y. Wang, H. Tang, and X. Niu, “Enhancing visual navigation performance by prior pose-guided active feature points distribution,” IEEE Trans. Instrum. Meas., vol. 73, pp. 1–13, 2024. [^15]: F. Dellaert and M. Kaess, “Factor graphs for robot perception,” Found. Trends Robot., vol. 6, nos. 1–2, pp. 1–139, 2017. [^16]: G. Huang, "Visual-inertial navigation: A concise review," in Proc. Int. Conf. Robot. Autom. (ICRA), May 2019, pp. 9572–9582. [^17]: T. Qin, P. Li, and S. Shen, “VINS-Mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Trans. Robot., vol. 34, no. 4, pp. 1004–1020, Aug. 2018. [^18]: A. Rosinol, M. Abate, Y. Chang, and L. Carlone, “Kimera: An open-source library for real-time metric-semantic localization and mapping,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2020, pp. 1689–1696. [^19]: V. Usenko, N. Demmel, D. Schubert, J. Stückler, and D. Cremers, “Visual-inertial mapping with non-linear factor recovery,” IEEE Robot. Autom. Lett., vol. 5, no. 2, pp. 422–429, Apr. 2020. [^20]: C. Campos, R. Elvira, J. J. G. Rodriguez, J. M. M. Montiel, and J. D. Tardos, "ORB-SLAM3: An accurate open-source library for visual, visual-inertial, and multimap SLAM," IEEE Trans. Robot., vol. 37, no. 6, pp. 1874–1890, Dec. 2021. [^21]: M. Bloesch, S. Omari, M. Hutter, and R. Siegwart, “Robust visual inertial odometry using a direct EKF-based approach,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Sep. 2015, pp. 298–304. [^22]: L. V. Stumberg and D. Cremers, “DM-VIO: Delayed marginalization visual-inertial odometry,” IEEE Robot. Autom. Lett., vol. 7, no. 2, pp. 1408–1415, Apr. 2022. [^23]: H. Liu, M. Chen, G. Zhang, H. Bao, and Y. Bao, "ICE-BA: Incremental, consistent and efficient bundle adjustment for visual-inertial SLAM," in Proc. IEEE/CVF Conf. Comput. Vis. Pattern Recognit., Jun. 2018, pp. 1974–1982. [^24]: M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, “ISAM2: Incremental smoothing and mapping using the Bayes tree,” Int. J. Robot. Res., vol. 31, no. 2, pp. 216–235, Feb. 2012. [^25]: S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, “Keyframe-based visual-inertial odometry using nonlinear optimization,” Int. J. Robot. Res., vol. 34, no. 3, pp. 314–334, Mar. 2014. [^26]: E. D. Nerurkar, K. J. Wu, and S. I. Roumeliotis, "C-KLAM: Constrained keyframe-based localization and mapping," in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2014, pp. 3638–3643. [^27]: Q. Cai, Y. Wu, L. Zhang, and P. Zhang, “Equivalent constraints for two-view geometry: Pose solution/pure rotation identification and 3D reconstruction,” Int. J. Comput. Vis., vol. 127, no. 2, pp. 163–180, Feb. 2019. [^28]: Q. Cai, L. Zhang, Y. Wu, W. Yu, and D. Hu, “A pose-only solution to visual reconstruction and navigation,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 45, no. 1, pp. 73–86, Jan. 2023. [^29]: Y. Ge, L. Zhang, Y. Wu, and D. Hu, "PIPO-SLAM: Lightweight visual-inertial SLAM with preintegration merging theory and pose-only descriptions of multiple view geometry," IEEE Trans. Robot., vol. 40, pp. 2046–2059, 2024. [^30]: T. Zhang, L. Wei, H. Tang, M. Yuan, L. Wang, and X. Niu, “SE-LIO: Semantic-enhanced solid-state-LiDAR-inertial odometry for tree-rich environments,” IEEE Trans. Instrum. Meas., vol. 74, pp. 1–13, 2025. [^31]: J. Zhang, M. Kaess, and S. Singh, “A real-time method for depth enhanced visual odometry,” Auto. Robots, vol. 41, no. 1, pp. 31–43, Jan. 2017. [^32]: S.-S. Huang, Z.-Y. Ma, T.-J. Mu, H. Fu, and S.-M. Hu, "LiDAR-monocular visual odometry using point and line features," in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2020, pp. 1091–1097. [^33]: H. Tang, X. Niu, T. Zhang, L. Wang, and J. Liu, "LE-VINS: A robust solid-state-LiDAR-enhanced visual-inertial navigation system for low-speed robots," IEEE Trans. Instrum. Meas., vol. 72, pp. 1–13, 2023. [^34]: J. Civera, A. J. Davison, and J. Montiel, “Inverse depth parametrization for monocular SLAM,” IEEE Trans. Robot., vol. 24, no. 5, pp. 932–945, Oct. 2008. [^35]: A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse, “MonoSLAM: Real-time single camera SLAM,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 29, no. 6, pp. 1052–1067, Jun. 2007. [^36]: G. P. Huang, A. I. Mourikis, and S. I. Roumeliotis, “Analysis and improvement of the consistency of extended Kalman filter based SLAM,” in Proc. IEEE Int. Conf. Robot. Autom., May 2008, pp. 473–479. [^37]: Z. Huai and G. Huang, “Robocentric visual-inertial odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Oct. 2018, pp. 6319–6326. [^38]: C. Cadena et al., “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age,” IEEE Trans. Robot., vol. 32, no. 6, pp. 1309–1332, Dec. 2016. [^39]: G. Sibley, L. Matthies, and G. Sukhatme, “Sliding window filter with application to planetary landing,” J. Field Robot., vol. 27, no. 5, pp. 587–608, Sep. 2010. [^40]: F. Zhang, The Schur Complement and Its Applications, vol. 4. Cham, Switzerland: Springer, 2006. [^41]: F. Dellaert, “Factor graphs and GTSAM: A hands-on introduction,” Georgia Inst. Technol., Atlanta, GA, USA, Tech. Rep. GT-RIM-CP&R2012-002, 2012. [^42]: R. Mur-Artal and J. D. Tardos, “Visual-inertial monocular SLAM with map reuse,” IEEE Robot. Autom. Lett., vol. 2, no. 2, pp. 796–803, Apr. 2017. ## [^43]: R. Mur-Artal, J. M. M. Montiel, and J. D. Tardós, “ORB-SLAM: A versatile and accurate monocular SLAM system,” IEEE Trans. Robot., vol. 31, no. 5, pp. 1147–1163, Oct. 2015 --- [^44]: R. Mur-Artal and J. D. Tardos, “ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras,” IEEE Trans. Robot., vol. 33, no. 5, pp. 1255–1262, Oct. 2017. [^45]: M. Mazuran, W. Burgard, and G. D. Tipaldi, “Nonlinear factor recovery for long-term SLAM,” Int. J. Robot. Res., vol. 35, nos. 1–3, pp. 50–72, Jan. 2016. [^46]: L. Von Stumberg, V. Usenko, and D. Cremers, “Direct sparse visual-inertial odometry using dynamic marginalization,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2018, pp. 2510–2517. [^47]: L. Zhao, S. Huang, Y. Sun, L. Yan, and G. Dissanayake, “ParallaxBA: Bundle adjustment using parallax angle feature parametrization,” Int. J. Robot. Res., vol. 34, nos. 4–5, pp. 493–516, Apr. 2015. [^48]: L. Liu, T. Zhang, B. Leighton, L. Zhao, S. Huang, and G. Dissanayake, "Robust global structure from motion pipeline with parallax on manifold bundle adjustment and initialization," IEEE Robot. Autom. Lett., vol. 4, no. 2, pp. 2164–2171, Apr. 2019. [^49]: K. Huang, J. Xiao, and C. Stachniss, “Accurate direct visual-laser odometry with explicit occlusion handling and plane detection,” in Proc. Int. Conf. Robot. Autom. (ICRA), Montreal, QC, Canada, May 2019, pp. 1295–1301. [^50]: J. Lin and F. Zhang, “R $^{3}$LIVE: A robust, real-time, RGB-colored, LiDAR-inertial-visual tightly-coupled state estimation and mapping package,” in Proc. Int. Conf. Robot. Autom. (ICRA), May 2022, pp. 10672–10678. [^51]: C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, "FAST-LIVO: Fast and tightly-coupled sparse-direct LiDAR-inertial-visual odometry," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Oct. 2022, pp. 4003–4009. [^52]: C. Zheng et al., “FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry,” IEEE Trans. Robot., vol. 41, pp. 326–346, 2024. [^53]: T. Shan, B. Englot, C. Ratti, and D. Rus, "LVI-SAM: Tightly-coupled LiDAR-visual-inertial odometry via smoothing and mapping," in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2021, pp. 5692–5698. [^54]: J. Graeter, A. Wilczynski, and M. Lauer, “LIMO: LiDAR-monocular visual odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Oct. 2018, pp. 7872–7879. [^55]: P. Wang, Z. Fang, S. Zhao, Y. Chen, M. Zhou, and S. An, “Vanishing point aided LiDAR-visual-inertial estimator,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2021, pp. 13120–13126. [^56]: Y. Zhu, C. Zheng, C. Yuan, X. Huang, and X. Hong, “CamVox: A low-cost and accurate LiDAR-assisted visual SLAM system,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), May 2021, pp. 5049–5055. [^57]: B. D. Lucas and T. Kanade, “An iterative image registration technique with an application to stereo vision,” in Proc. 7th Int. Joint Conf. Artif. Intell., 1981, pp. 674–679. [^58]: H. Tang, T. Zhang, X. Niu, J. Fan, and J. Liu, “Impact of the Earth rotation compensation on MEMS-IMU preintegration of factor graph optimization,” IEEE Sensors J., vol. 22, no. 17, pp. 17194–17204, Sep. 2022. انواع 17 [^60]: S. Agarwal and K. Mierle. (Mar. 2022). Ceres Solver. The Ceres Solver Team. [Online]. Available: [^61]: Y. Yang, J. Maley, and G. Huang, "Null-space-based marginalization: Analysis and algorithm," in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst. (IROS), Sep. 2017, pp. 6749–6755. [^62]: H. Wei et al., “FusionPortableV2: A unified multi-sensor dataset for generalized SLAM across diverse platforms and scalable environments,” 2024, arXiv:2404.08563. [^63]: J. Lin, C. Zheng, W. Xu, and F. Zhang, "R²LIVE: A robust, real-time, LiDAR-inertial-visual tightly-coupled state estimator and mapping," IEEE Robot. Autom. Lett., vol. 6, no. 4, pp. 7469–7476, Oct. 2021. [^64]: J. Lin and F. Zhang, "R'LIVE++: A robust, real-time, radiance reconstruction package with a tightly-coupled LiDAR-inertial-visual state estimator," IEEE Trans. Pattern Anal. Mach. Intell., vol. 46, no. 12, pp. 11168–11185, Dec. 2024. [^65] Michael Grupp. （2023年3月）。EVO。[在线]。可获取地址：