利用彩色点云图，通过双重稀疏性和分层优化实现鲁棒且高效的视觉测距技术

《Satellite Navigation》：Robust and efficient visual odometry using colored point cloud maps via dual-sparsity and hierarchical optimization

　　摘要

在GNSS信号受到挑战的环境中，可靠的定位是导航的基础，然而在没有全局约束的情况下，单目视觉里程计(VO)不可避免地会出现漂移。点云地图可以提供全局校正来减轻这种漂移。然而，现有的异构特征注册方法未能充分利用视觉和点云之间的共享信息。这一限制导致定位精度不佳、鲁棒性降低以及计算开销增加。为了解决这些问题，我们提出了一种基于彩色点云地图的视觉里程计系统，该系统利用全局点云地图作为约束，并利用彩色点云地图与相机之间的共享特征（颜色）来确保定位的准确性和鲁棒性。该系统主要包括两个部分：稀疏彩色点云构建和视觉定位。在稀疏彩色点云构建模块中，我们引入了一种地图稀疏化策略，将视觉梯度与点云关联起来，确保保留的稀疏点云保留了显著的视觉梯度信息，从而降低了计算成本。这一策略进一步融入到视觉到地图的匹配阶段，形成了“双重稀疏匹配”方案。在定位阶段，我们提出了一种基于层次优化的迭代卡尔曼滤波算法，该算法对多分辨率图像进行多层次迭代优化，以防止定位陷入局部最优解，同时提高精度。在公共和自收集的数据序列上的实验表明，我们的方法在精度和效率方面都有显著提升。与一种代表性的基于地图的方法(DSL)相比，我们的方法在多个具有挑战性的序列中将平均绝对误差(RMSE)降低了52%至95%，在R3live_5数据集上，定位精度从1.883米提高到了0.152米（降低了91.92%）。与基于学习的I2D-Loc++方法相比，我们的方法在精度上提高了多达76.6%，并且在极端情况下（例如R3live_3/4，其中I2D-Loc++和ORB-SLAM3因视觉退化而失败）仍能保持鲁棒跟踪。此外，该系统保持了接近实时的效率，总处理时间最多降低了47.7%（例如在Whu_1数据集上从121.11秒减少到63.34秒）。即使在严重的几何退化情况下，我们的跟踪器仍然保持鲁棒，平均绝对误差为0.076米，而仅使用几何信息的LiDAR–IMU定位的误差则增加到了9.23米。

引言

定位是自动驾驶和机器人系统的核心任务（Li等人，2021a）。它涉及估计系统的状态（例如位置和方向），这是下游任务（如导航、避障和决策制定）的基础（El-Sheimy & Li，2021）。一种常见的方法是基于视觉传感器的相对定位，该方法通过估计相邻帧之间的运动来推断系统在环境中的轨迹。然而，这种基于图像特征的相对定位容易受到光照变化、纹理缺失、运动模糊和遮挡的影响，导致误差随时间累积并导致轨迹偏差（Wu等人，2025；Li等人，2021b）。为了解决这个问题，传统的视觉定位方法通常将视觉数据与其他传感器（例如IMU、LiDAR、GNSS）融合起来，以抑制误差累积并保持稳定的定位精度（Hua等人，2023；Zhang & Jiang，2025）。然而，这需要额外的传感器支持，这增加了硬件和集成的复杂性，对于紧凑或资源受限的平台来说可能并不总是理想的（Mei等人，2025）。

图1

本文的替代文本可能是使用人工智能生成的。

提出的仅基于相机的地图定位框架概述

随着多传感器融合技术的发展，现在许多系统可以自动构建和更新地图（Gu等人，2024）。基于地图的视觉定位方法利用预先构建的全局地图来进行定位（Zhang等人，2023），从而避免了纯相对定位相关的分歧问题（Chalvatzaras等人，2022）。通过将当前观测结果与全局地图对齐，这些方法提供了更高的定位精度，并在长期运行中保持稳定的性能，通常不需要依赖外部定位信号（例如GNSS）。这使得基于地图的定位在复杂的室内、地下或GNSS信号受到限制的环境中具有吸引力。

然而，在实践中，基于地图的定位仍然面临几个挑战，特别是在针对具有有限感知或计算资源的平台时：

1. 由于地图和定位传感器数据中存在大量冗余信息，有效地避免冗余计算同时确保高效的数据融合仍然是提高定位精度和计算效率的关键挑战（Ge等人，2024；Usenko等人，2019）。冗余信息增加了计算负担，引发了不必要的数据处理，从而降低了实时性能和系统的响应能力。

2. 传统的基于地图的定位方法通常依赖于来自不同传感器的异构特征进行定位。虽然这在某些情况下可以提高鲁棒性，但这些方法通常没有充分利用观察相同环境的传感器之间的共享信息（Zhang等人，2023a；Sauerbeck等人，2023）。因此，融合可能无法捕捉到所有可用的约束，在复杂的或视觉上有歧义的条件下导致定位精度下降。

3. 局部最优问题长期以来一直是影响基于地图的方法定位精度的主要因素。在大型或结构重复的环境中，传统方法容易积累误差，并可能收敛到次优的姿态估计（Chalvatzaras等人，2022；Wu等人，2023），尤其是在优化窗口较大或地图密集且噪声较大的情况下。

从系统的角度来看，用于地图构建和实时定位的传感器配置也可以不同。在许多实际情况中，一个或几个映射平台可能配备了更丰富的传感器套件（例如LiDAR–IMU–相机）来构建先前的地图，而在同一环境中运行的其他平台可能只携带相机。典型的例子包括共享工厂楼层预构建地图的室内物流和仓库机器人车队、在地下停车场、隧道或地铁站中工作的检查机器人，以及必须保持低成本同时在预映射环境中可靠定位的校园或医院配送车。在这项工作中，我们明确关注这种设置：我们假设已经通过任何合适的映射流程（可能使用多传感器融合）预构建了彩色点云地图，并设计了一种仅基于相机的、基于地图的定位算法，该算法旨在在轻量级平台上在线运行。在这种情况下，映射工作可以离线完成并在不同部署中重复使用，而每个在线代理只需要一个单目相机，这有助于简化客户端侧的传感器配置。

为了解决上述挑战，我们提出了一种基于彩色点云地图的纯视觉跟踪方法，专为这种“地图预构建、仅使用相机的客户端”设置量身定制。与需要所有模式的在线多传感器融合系统不同，我们的方法不假设定位客户端上带有LiDAR或IMU，仅依靠一个单目相机和预构建的彩色点云地图来实现准确的定位。在算法上，我们在全局点云和相机观测数据上引入了一种双向稀疏策略，保留了高梯度点，从而在映射和定位过程中都保留了高梯度信息。这抑制了信息量低的区域，减轻了传统方法中的局部最优问题，并提高了全局一致性和鲁棒性。此外，我们设计了一种迭代层次优化方案，结合了基于重投影误差的几何细化和光度更新步骤；多次更新显著提升了定位精度，同时保持在资源受限平台上的计算负载在可管理范围内。图1展示了整个框架及其各个组件的作用。

具体来说，我们的贡献总结如下：

我们提出了一种仅基于相机的、基于地图的视觉跟踪方法，该方法使用预构建的彩色点云地图进行实时定位。该方法直接利用客户端侧的地图信息，避免了在线的相机–LiDAR同步和多传感器融合，从而简化了定位平台的传感器配置。全局地图约束为高精度定位提供了强有力的几何先验，而颜色信息增强了稳定性和鲁棒性。我们引入了一种双向稀疏策略，在地图构建和视觉定位中都提取了高梯度点。生成的地图突出了视觉上显著的structure，同时丢弃了弱或冗余的区域，观测级别的稀疏化将计算重点放在了信息丰富的图像区域。这种稀疏但信息丰富的表示减少了局部最优解的风险，提高了定位精度和鲁棒性。我们提出了一种新颖的迭代卡尔曼风格的更新方法，该方法结合了层次优化，替代了计算成本高昂的全局优化，例如完整的束调整或大窗口平滑。优化遵循一个预测–校正方案，分为两个层次：一个使用重投影误差获得稳定姿态估计的几何层次，以及一个使用图像强度残差细化这一估计的光度层次。这种分层迭代策略避免了在单一步骤中解决大型非线性问题，减少了资源受限平台上的计算成本，并在提高定位精度的同时减轻了与局部最优解相关的问题。我们在开源和自收集的数据集上进行了广泛的实验来验证所提出的方法。我们进一步将我们的方法与强大的多传感器基线进行了比较，以量化完整的LiDAR–IMU–相机融合与我们的仅基于相机的定位方法之间的精度差异。结果表明，我们的方法在传感器退化和复杂环境中实现了具有竞争力的定位精度和鲁棒性，同时在传感器要求和定位性能之间取得了合理的平衡。

相关工作

视觉里程计(VO)提供了一种低成本和高效率的相对姿态估计解决方案，但它在没有全局约束的情况下不可避免地会出现累积漂移。为了提高鲁棒性和长期导航性能，先前的研究主要集中在两个方向上：多传感器融合（例如IMU/LiDAR/GNSS）和基于地图的定位（例如视觉特征地图或LiDAR点云地图）。尽管取得了稳步进展，实际导航仍然面临持续的挑战，包括在长期外观变化下的鲁棒性、在几何退化下的可靠性，以及精度和实时计算成本之间的权衡。这些差距激发了开发轻量级基于地图的视觉定位的动机，这种定位可以利用跨异构数据的共享信息，同时保持部署效率。

**多传感器融合下的视觉跟踪**

VO通常与IMU、LiDAR或GNSS融合使用，以利用互补的感知：IMU稳定短期运动，LiDAR提供几何结构，GNSS提供绝对参考。这样的系统在自动驾驶、机器人技术和无人机导航中被广泛采用；然而，它们通常会增加硬件复杂性，并且仍然需要强大的全局约束以实现长期一致性。

**使用IMU的视觉跟踪**

基于视觉-惯性的系统大致分为基于滤波器和基于优化的设计。基于滤波器的MSCKF（Mourikis，2007）通过地标边缘化实现了高效率，但随着时间的推移可能会累积线性化误差。ROVIO（Bloesch等人，2015）结合了光度线索来提高鲁棒性，但其计算量仍然很大，并且在特征贫乏的场景中性能会下降。R-VIO（Huai & Huang，2022）通过以机器人为中心的公式提高了局部精度，但在长期一致性方面仍然存在问题。OpenVINS（Geneva等人，2020年）提供了一个模块化框架，同时继承了基于滤波器方法的共同局限性：在没有全局优化或闭环处理的情况下，累积的误差无法被事后纠正。基于优化的VINS（Qin等人，2018年）和OKV-IS（Leutenegger，2015年）使用滑动窗口优化来保持准确性，但长期运行仍会面临漂移和计算负担增加的问题。VI-ORB-SLAM（Mur-Artal和Tardós，2017年）和ORB-SLAM3（Campos等人，2021年）进一步整合了全局优化和闭环处理，提高了在视觉丰富场景中的准确性，但它们在长期外观变化下往往变得脆弱，并且运行时间成本增加。

学习增强的视觉惯性（VIO）最近的研究将深度网络整合到VIO中以学习特征或融合策略：Learning to Fuse（Rambach等人，2016年）和VINET（Clark等人，2017年）从融合特征中回归姿态；Buchanan等人（2022年）学习IMU偏置的演变；Pajula等人（2023年）使用变换器交叉注意力来放宽校准/同步约束；Gu等人（2024年）预测速度和不确定性以用于下游融合；Wang（2024年）使用特征金字塔和注意力来抑制噪声。虽然这些方法提高了适应性，但它们通常需要多传感器设置、大量训练和更高的计算能力，这对于轻量级导航平台来说可能不理想。

使用LiDAR的视觉跟踪LiDAR通过提供精确的深度和密集的3D结构来补充视觉里程计（VO），有利于大规模和特征贫乏的场景。Loosely coupled（Zhang，2015年）使用VO来初始化LiDAR配准，而不进行联合优化。VIL-SLAM（Shao等人，2019年）分别处理LiDAR、相机和IMU数据。DIMO（Zhang & Kaess，2014年）、LIMO（Graeter & Wilczynski，2018年）和CamVox（Zhu等人，2021年）将LiDAR深度注入视觉模块，但由于LiDAR点与2D图像特征之间的分辨率不匹配，深度关联仍然容易出错。Tightly coupled LIC-Fusion（Zuo等人，2019a）、VIL-ENS（Wisth等人，2022年）、R3live（Lin，2022年）和SR-LIVO（Yuan等人，2024年）共同优化多传感器因素以提高准确性和鲁棒性，但长期运行仍可能受到漂移积累、退化和系统复杂性增加的影响。

使用GNSS的视觉跟踪GNSS提供全局参考，以重置漂移并改善户外导航的可扩展性。早期工作（Mascaro等人，2018年；Qin等人，2019年）使用松耦合融合，而紧密耦合的形式在GNSS短暂中断时进一步提高了准确性和鲁棒性（Cioffi，2020年；Boche等人，2022年）。然而，在密集的城市环境中，GNSS由于多径效应和遮挡仍然不可靠。为了应对这个问题，Cao等人（2022年）对原始伪距进行了建模，Yin等人（2024年）使用向上视角的相机对卫星数据进行过滤，Lee等人（2020年）在恢复后逐步重新引入GNSS，但是严重的或长期的干扰仍然具有挑战性。

使用地图的视觉定位基于地图的定位使用由同质（仅视觉）或异质（例如，LiDAR）传感器构建的地图引入全局约束。当前的趋势强调提高跨模态关联的鲁棒性，同时减少在线计算量。

同质传感器（视觉地图和神经场）经典流程依赖于稳健的关键点和描述符，如SIFT和BRIEF（Calonder等人，2010年），以及基于图像的定位（Zhang，2006年）和FAB-Map（Cummins & Newman，2008年）来解决检索和感知混淆问题。不确定性感知的对应处理改进了姿态估计（Kim等人，2015年），通过滑动窗口方案可以保持循环检测和地图一致性（Qin等人，2018年），7自由度（7-DoF）对齐支持地图融合（Yamaguchi等人，2018年）。最近，NeRF（Mildenhall等人，2021年）和3DGS（Kerbl等人，2023年）实现了基于渲染的姿态精细化：iNeRF/piNeRF（Yen-Chen等人，2021年），Lin等人（2023年）最小化光度损失，混合方法（Chen & Cong，2024年）结合特征匹配和渲染深度进行PnP plus精细化，以及iComMa（Sun等人，2023年）和其他基于3DGS的定位工作（Botashev等人，2024年；Lu等人，2025年）遵循类似的光度驱动优化。尽管这些方法功能强大，但它们通常需要精确的初始化和迭代渲染，导致高计算成本，并限制了实时部署（Lu等人，2025年；Sun等人，2023年）。

异质传感器（LiDAR点云地图）跨模态对齐是必不可少的。Wolcott（2014年）通过归一化互信息将LiDAR强度与图像对齐；Caselitz等人（2016年）将ORB-SLAM的稀疏点与LiDAR地图结合。Lin等人（2021年）将立体视差地图与基于分割的LiDAR地图匹配，Yabuuchi等人（2021年）使用点平面ICP和OpenVSLAM（Sumikura & Shibuya，2019年）提高稳定性。其他系统利用几何原语和约束：Zuo等人（2019b）将半密集立体点云与LiDAR地图对齐；Wang等人（2021年）和Yu等人（2020年）在VO/VINS流程中结合点和线约束；Lu等人（2020年）引入共面约束用于异质地图融合；Lynen等人（2015年）将SfM与3D点云结合，但对于大规模场景敏感；Huang等人（2020年）采用混合地图表示来提高跟踪能力。基于学习的跨模态定位也有进步：CMRNet（Cattaneo等人，2019年）使用图像-LiDAR投影细化姿态；HyperMap（Chang等人，2021年）通过学习到的3D特征压缩存储；I2D-Loc（Chen等人，2022年）通过加权深度完成提高性能；I2D-Loc++（Yu等人，2024a）通过学习图像到深度的流动并使用光流强制执行多视图一致性来进一步加强跨模态姿态跟踪，随后进行PnP精细化；Zhang等人（2023a）在点平面ICP中强制执行语义一致性；AdaFusion（Lai等人，2022）使用2D/3D注意力更好地关联图像和点云。尽管准确性有所提高，但许多方法仍对外观变化敏感，需要大量计算（通常基于GPU），或在复杂场景中依赖于脆弱的跨模态对应关系。

总体而言，多传感器融合提高了短期鲁棒性，但增加了传感器和计算需求，而基于地图的定位提供了全局约束，但通常受到不可靠的跨模态关联和高在线成本的影响。我们的工作旨在通过实现在彩色点云地图上仅使用相机的轻量级定位来填补这一空白，明确利用共享的光度信息来加强地图-图像关联。特别是，我们通过梯度感知的地图稀疏化、双稀疏匹配和基于分层优化的迭代卡尔曼风格更新，在计算受限的情况下实现稳健的长期导航。

图2
此图像的替代文本可能是使用AI生成的。

完整尺寸图像
所提出方法的系统流程
方法概述
图2总结了我们的系统，包括两个组件：稀疏彩色点云地图生成和仅使用相机的视觉跟踪。对于地图生成，我们基于SR_LIVO（Yuan等人，2024年）：LiDAR-惯性子系统估计连续且准确的姿态并重建密集的几何点云地图，而视觉子系统细化相机内参、LiDAR-相机外参和渲染参数，将LiDAR点投影到图像中，并从相应的像素分配每个点的RGB值。因此，几何信息由LiDAR/IMU提供，颜色由相机提供，生成可以实时渲染的密集彩色地图。我们进一步使用图像金字塔提取稀疏高梯度特征，并将它们与LiDAR-惯性优化的姿态融合到密集彩色地图中，形成稀疏彩色点云地图。

对于在线跟踪，我们使用（i）地图-视觉关联和（ii）VO模块。在关联过程中，通过Lucas-Kanade（LK）光流检测和跟踪稀疏高梯度2D特征，然后与稀疏彩色地图关联，实现在图像和地图两方面的双稀疏匹配；这抑制了冗余的低信息点，避免了不良的局部最小值，并注入全局地图约束以减少长期漂移。我们还应用隐藏点去除（HPR）（Katz & Tal，2007年）来仅保留从当前视点可见的地图点，提高2D-3D对应的可靠性。

VO模块使用具有恒定速度预测和两次测量更新的迭代误差状态卡尔曼滤波器（IESKF）。利用LK 2D轨迹和相关3D地图点，第一次更新通过最小化重投影残差来进行PnP风格的几何校正，在中等光度干扰和跟踪噪声下获得稳定的粗略姿态。第二次更新通过最小化当前图像与重新投影（通常是高梯度/显著的）地图点之间的强度一致性残差来进行直接光度精细化。因此，LK仅提供2D对应关系，而两次IESKF更新分别以粗略到精细的方式优化不同的几何和光度姿态目标：PnP更新在大运动下稳定帧间估计，光度精细化实现亚像素对齐，并将姿态紧密锚定到全局彩色地图上，提高鲁棒性和长期定位准确性。

地图构建和地图-图像匹配为了提高基于彩色点云地图的VO系统的鲁棒性和效率，我们通过提取具有显著图像梯度的视觉特征点来优化密集彩色点云地图，从而得到一个稀疏彩色点云地图。与密集地图相比，稀疏地图包含更少的点云，在将彩色点投影到图像上时节省了大量时间，从而提高了实时性能。此外，图像颜色空间的非线性可能导致不匹配，增加局部最优解的风险。受DSO（Engel等人，2017年）的特征提取方法的启发，我们开发了一种多分辨率自适应梯度特征提取算法，该算法动态调整梯度阈值以提高特征点提取的准确性和可靠性。该算法在不同分辨率级别应用不同的提取策略：在原始图像分辨率下，我们计算每个像素的梯度并选择满足预设阈值的特征点；在更高分辨率的图像层上，我们进一步细化提取。

因此，稀疏彩色点云地图保留了对VO系统有益的关键特征点，有效提高了定位准确性和实时性能。这种优化不仅提高了系统在复杂环境中的鲁棒性，还显著提高了在不同应用场景下的适应性。此外，在视觉定位模块中，该过程应用于相机图像以提取稀疏视觉特征点。通过匹配这两种类型的稀疏特征，我们避免了视觉跟踪中后续后估计陷入局部最优解的风险，从而进一步提高了视觉跟踪系统的鲁棒性。这种稀疏到稀疏的匹配还降低了数据关联的复杂性，使系统能够在实时环境中高效运行。此外，通过用与地图一致的特征对应关系约束帧级姿态更新，跟踪器即使在低纹理或部分退化的视图中也能保持长期轨迹稳定性。

视觉跟踪
我们定义系统的状态向量为：
$$\begin{aligned} \begin{aligned} \varvec{x}&= \begin{bmatrix} {\phantom {a}}^{M}\varvec{p}_{c},\, {\phantom {a}}^{M}\varvec{v}_{c},\, {\phantom {a}}^{M}\varvec{R}_{c},\, \varvec{\omega } \end{bmatrix}, \quad \varvec{w} = \begin{bmatrix}\varvec{n}_{v},\, \varvec{n}_{\omega }\end{bmatrix}, \end{aligned} \end{aligned}$$
其中 \(^{M}\varvec{p}_{c}\) 和 \({\phantom {a}}^{M}\varvec{R}_{c}\) 分别表示相机在地图坐标系中的位置和姿态。相机在地图坐标系中的线速度及其在机体坐标系中的角速度分别用 \({\phantom {a}}^{M}\varvec{v}_{c}\) 和 \(\varvec{\omega }\) 表示。此外，这两种速度都被建模为随机游走。线速度由高斯噪声 \(\varvec{n}_{v}\) 驱动，角速度由高斯噪声 \(\varvec{n}_{\omega }\) 驱动。

为了模拟相机的运动，我们基于纯LiDAR里程计（Yu等人，2024b）提出两种适用于不同传感器类型的恒定速度运动模型。第一种模型适用于手持设备或某些空中车辆，其中旋转和平移之间的耦合较弱。在这个模型中，我们假设在第\(k^{\textrm{th}}\)帧的持续时间\(t_{k}\)内，地图坐标系中的线速度和机体坐标系中的角速度保持不变。这些速度假设与前一个帧的速度相等。基于这个假设，状态转换函数定义为：
$$\begin{aligned} \begin{aligned} \varvec{f}(\varvec{x},\varvec{w}) = \begin{bmatrix} {\phantom {a}}^{M}\varvec{v}_{c}\Delta t\\ \varvec{n}_{v}\Delta t\\ \varvec{\omega }\Delta t\\ \varvec{n}_{\omega }\Delta t \end{bmatrix}. \end{aligned} \end{aligned}$$
第二种模型适用于基于地面的移动机器人，在这种情况下，旋转和平移之间的耦合较强。在这种情况下，假设在第\(k^{\textrm{th}}\)帧的持续时间\(t_{k}\)内，机体坐标系中的线速度和角速度保持不变，并且与前一个帧的速度相等。相应的状态转换函数定义为
$$\begin{aligned}
\begin{aligned}
\varvec{f}(\varvec{x},\varvec{w}) = \begin{bmatrix}
\bigl ({\phantom {a}}^{M}\varvec{R}_{c}\boxplus \varvec{\omega }\Delta t\bigr )\bigl ({\phantom {a}}^{M}\varvec{R}_{c}\bigr )^{-1}{\phantom {a}}^{M}\varvec{v}_{c}\Delta t\\
\Bigl (\bigl ({\phantom {a}}^{M}\varvec{R}_{c}\boxplus \varvec{\omega }\Delta t\bigr )\bigl ({\phantom {a}}^{M}\varvec{R}_{c}\bigr )^{-1}-\varvec{I}\Bigr ){\phantom {a}}^{M}\varvec{v}_{c}+\varvec{n}_{v}\Delta t\\
\varvec{\omega }\Delta t\\
\varvec{n}_{\omega }\Delta t
\end{bmatrix}.
\end{aligned}
\end{aligned}
$$
(3)
因此，基于恒定速度运动模型的离散转换方程可以如下推导：
$$\begin{aligned}
\begin{aligned}
\varvec{x}_{k+1} = \varvec{x}_{k}\boxplus \varvec{f}(\varvec{x}_{k},\varvec{w}_{k},
\end{aligned}
$$
(4)
其中 \(\boxplus\) 是状态流形上的“加号”操作。

相机观测模型
在本节中，我们介绍了两种常用的相机观测模型：PnP投影模型和光度投影模型。为了实现PnP投影模型，我们利用之前构建的点云映射中的m个映射点 \(\varvec{P}=\{\varvec{P}_{1},\ldots ,\varvec{P}_{m}\}\)，这些点在之前的图像帧 \(I_{k-1}\) 中的投影位置分别表示为 \(\{\varvec{\rho }_{1,k-1},\ldots ,\varvec{\rho }_{m,k-1}\}\)。通过应用Lucas–Kanade光流方法，我们计算这些映射点在当前图像 \(I_{k}\) 中的对应位置，表示为 \(\{\varvec{\rho }_{1,k},\ldots ,\varvec{\rho }_{m,k}\}\)。利用这些对应关系，我们构建PnP投影模型并通过最小化重投影误差来优化相机状态估计。具体来说，考虑第 \(s^{\textrm{th}}\) 个映射点 \(\varvec{P}_{s}=(\varvec{M}_{s}^{\textrm{T}},\varvec{\gamma }_{s}^{\textrm{T}})^{\textrm{T}}\)，其中 \(\varvec{P}_{s}=\varvec{M}_{s}^{\textrm{T}}\) 表示该点的映射位置，\(\varvec{\gamma }_{s}^{\textrm{T}}\) 表示颜色。设相机状态估计为 \(\check{\varvec{x}}_{k}\)。点 \(\varvec{P}_{s}\) 的重投影误差定义为：
$$\begin{aligned}
\begin{aligned}
\varvec{r}\bigl (\check{\varvec{x}}_{k},\varvec{\rho }_{s,k},{{\phantom {a}}}^{G}\varvec{M}_{s}\bigr ) = \left\| \varvec{\rho }_{s,k}-\varvec{\pi }\bigl (\varvec{M}_{s},\check{\varvec{x}}_{k}\bigr )\right\| .
\end{aligned}
$$
(5)
其中 \(\varvec{\pi }\bigl (\varvec{M}_{s},\check{\varvec{x}}_{k}\bigr )\) 是基于当前状态估计 \(\check{\varvec{x}}_{k}\) 的点 \(\varvec{P}_{s}\) 的标准相机针孔模型预测投影，\(\varvec{\rho }_{s,k}\) 是当前图像中点的实际观测位置。

在讨论了PnP投影模型之后，我们现在讨论光度投影模型。以第 \(s^{\textrm{th}}\) 个跟踪点 \(P_{s}\) 为例，其光度误差可以表示为：
$$\begin{aligned}
\varvec{o}\bigl (\check{\varvec{x}}_{k},\, {\phantom {a}}^{M}\varvec{p}_{s},\, \varvec{c}_{s}\bigr ) = \varvec{c}_{s}-\varvec{\gamma }_{s},
\end{aligned}
$$
(6)
其中 \(^{M}\varvec{p}_{s}\) 表示映射框架中的映射点3D位置，\(\varvec{c}_{s}\) 表示映射点的颜色，\(\varvec{\gamma }_{s}\) 是从图像中观测到的颜色。这种PnP投影模型和光度投影模型的结合利用了每种模型的优点：PnP投影模型使用已知映射点的精确深度信息进行快速帧间估计，并为状态估计中的光度投影提供准确的先验；而光度投影模型利用映射和图像之间的光度信息进行帧到映射的匹配，从而进一步提高系统的精度和鲁棒性。

IESKF前向传播
从(4)开始，前向传播关系如下：
$$\begin{aligned}
\begin{aligned}
\widehat{\varvec{x}}_{k+1}&= \check{\varvec{x}}_{k}\boxplus \varvec{f}(\check{\varvec{x}}_{k},\varvec{0}),\\
\widehat{\varvec{P}}_{k+1}&= \varvec{F}_{\widetilde{\varvec{x}}_{k}}\check{\varvec{P}}_{k}\varvec{F}_{\widetilde{\varvec{x}}_{k}}^{\textrm{T}} + \varvec{F}_{\varvec{w}_{k}}\varvec{Q}_{k}\varvec{F}_{\varvec{w}_{k}}^{\textrm{T}},
\end{aligned}
$$
(7)
其中 \(\check{\varvec{x}}_{k}\) 和 \(\check{\varvec{P}}_{k}\) 分别表示第 \(k\) 帧的后验状态和协方差。 \(\widehat{\varvec{x}}_{k+1}\) 和 \(\widehat{\varvec{P}}_{k+1}\) 表示第 \(k+1\) 帧的预测状态和协方差。矩阵 \(\varvec{Q}_{k}\) 是噪声 \(\varvec{w}_{k}\) 的协方差，而矩阵 \(\varvec{F}_{\widetilde{\varvec{x}}_{k}}\) 和 \(\varvec{F}_{\varvec{w}_{k}}\) 定义如下：
$$\begin{aligned}
\begin{aligned}
\varvec{F}_{\widetilde{\varvec{x}}_{k}}&= \left. \frac{\partial (\varvec{x}_{k+1}\boxplus \widehat{\varvec{x}}_{k+1})}{\partial \widetilde{\varvec{x}}_{k}}\right| _{\widetilde{\varvec{x}}_{k}=\varvec{0},\,\varvec{w}_{k}=\varvec{0}},\\
\varvec{F}_{\varvec{w}_{k}}&= \left. \frac{\partial (\varvec{x}_{k+1}\boxplus \widehat{\varvec{x}}_{k+1})}{\partial \varvec{w}_{k}}\right| _{\widetilde{\varvec{x}}_{k}=\varvec{0},\,\varvec{w}_{k}=\varvec{0}}, \\ \widetilde{\varvec{x}}_{k}&= \varvec{x}_{k}\boxminus \widehat{\varvec{x}}_{k},
\end{aligned}
$$
(8)
其中 \(\widetilde{\varvec{x}}_{k}\) 是误差状态向量。 \(\boxminus\) 是状态流形上的“减号”操作。

基于分层优化的IESKF更新
从(5)开始，考虑像素跟踪误差和映射点位置误差，真实零残差 \(\varvec{r}(\varvec{x}_{k},\varvec{\rho }_{s,k}^{\textrm{gt}},{\phantom {a}}^{M}\varvec{p}_{s}^{\textrm{gt}})\) 的一阶泰勒展开得到：
$$\begin{aligned}
\varvec{r}\bigl (\varvec{x}_{k},\varvec{\rho }_{s,k}^{\textrm{gt}},{\phantom {a}}^{M}\varvec{p}_{s}^{\textrm{gt}}\bigr ) \approx \varvec{r}\bigl (\check{\varvec{x}}_{k},\varvec{\rho }_{s,k},{\phantom {a}}^{M}\varvec{p}_{s}\bigr ) +\varvec{H}_{s}^{\varvec{r}}\widetilde{\varvec{x}}_{k} +\varvec{\alpha }_{s},
\end{aligned}
$$
(9)
其中 \(\varvec{\rho }_{s,k}^{\textrm{gt}}\) 和 \({\phantom {a}}^{M}\varvec{p}_{s}^{\textrm{gt}}\) 是不含误差的 \(\varvec{\rho }_{s,k}\) 和 \({\phantom {a}}^{M}\varvec{p}_{s}\) 的真实值。 \(\varvec{H}_{s}^{\varvec{r}}\) 表示在 \(\widetilde{\varvec{x}}_{k}=\varvec{0}\) 时计算的 \(\varvec{r}\bigl (\check{\varvec{x}}_{k}\boxplus \widetilde{\varvec{x}}_{k},\varvec{\rho }_{s,k},{\phantom {a}}^{M}\varvec{p}_{s}\bigr )\) 的雅可比矩阵，\({\alpha }_{s}\) 来自像素跟踪误差和映射点位置误差。同时，\(\varvec{\alpha }_{s}\sim \mathcal {N}(\varvec{0},\varvec{\Sigma }_{\varvec{\alpha }_{s}})\) 并且
$$\begin{aligned}
\varvec{H}_{s}^{\varvec{r}} = \left. \frac{\partial \varvec{r}\bigl (\check{\varvec{x}}_{k}\boxplus \widetilde{\varvec{x}}_{k},\varvec{\rho }_{s,k},{\phantom {a}}^{M}\varvec{p}_{s}\bigr )}{\partial \widetilde{\varvec{x}}_{k}} \right| _{\widetilde{\varvec{x}}_{k}=\varvec{0}},
\end{aligned}
(10)
$$
$$\begin{aligned}
\varvec{\Sigma }_{\alpha _{s}} = \varvec{\Sigma }_{\varvec{n}_{\varvec{\rho }}} + \varvec{F}_{\varvec{p}_{s}}^{\varvec{r}} \varvec{\Sigma }_{\varvec{p}_{s}} \bigl (\varvec{F}_{\varvec{p}_{s}}^{\varvec{r}}\bigr )^{\textrm{T}},
\end{aligned}
(11)
$$
$$\begin{aligned}
\varvec{F}_{\varvec{p}_{s}}^{\varvec{r}} = \frac{\partial \varvec{r}\bigl (\check{\varvec{x}}_{k},\varvec{\rho }_{s,k},{\phantom {a}}^{M}\varvec{p}_{s}\bigr )}{\partial \,{\phantom {a}}^{M}\varvec{p}_{s}},
\end{aligned}
(12)
$$
其中 \(\varvec{\Sigma }_{\varvec{n}_{\varvec{\rho }}}\) 是 \(\varvec{\rho }_{s,k}\) 的协方差矩阵，\(\varvec{\Sigma }_{\varvec{p}_{s}}\) 是 \({\phantom {a}}^{M}\varvec{p}_{s}\) 的信息矩阵。然后我们可以通过积分均匀运动先验得到以下最大后验估计（MAP）问题：
$$\begin{aligned}
\begin{aligned}
\min _{\widetilde{\varvec{x}}_{k}} \Bigl (&\left\| \check{\varvec{x}}_{k}\boxminus \widehat{\varvec{x}}_{k}+\varvec{J}\widetilde{\varvec{x}}_{k}\right\| _{\widehat{\varvec{P}}_{k}^{-1}}^{2} \\
&+\sum _{s=1}^{m}\left\| \varvec{r}\bigl (\check{\varvec{x}}_{k},\varvec{\rho }_{s,k},{\phantom {a}}^{M}\varvec{p}_{s}\bigr )+\varvec{H}_{s}^{\varvec{r}}\widetilde{\varvec{x}}_{k}\right\| _{\varvec{\Sigma }_{\alpha _{s}}}^{2} \Bigr ),
\end{aligned}
(13)
$$
其中 \(\varvec{J}\) 是将状态误差从预测切线空间投影到估计切线空间的雅可比矩阵。另外，设
$$\begin{aligned}
\varvec{H} = \bigl [\varvec{H}_{1}^{\varvec{r}\,\textrm{T}},\ldots ,\varvec{H}_{m}^{\varvec{r}\,\textrm{T}}\bigr ]^{\textrm{T}}, \quad \varvec{R} = \operatorname {diag}\bigl (\varvec{\Sigma }_{\alpha _{1}},\ldots ,\varvec{\Sigma }_{\alpha _{m}}\bigr ),
\end{aligned}
(14)
$$
$$\begin{aligned}
\check{\varvec{z}}_{k} = \begin{bmatrix} \varvec{r}\bigl (\check{\varvec{x}}_{1},\varvec{\rho }_{1,k},{\phantom {a}}^{M}\varvec{p}_{1}\bigr ),\ldots , \varvec{r}\bigl (\check{\varvec{x}}_{m},\varvec{\rho }_{m,k},{\phantom {a}}^{M}\varvec{p}_{m}\bigr ) \end{bmatrix}^{\textrm{T}},
\end{aligned}
(15)
$$
$$\begin{aligned}
\varvec{P} = \varvec{J}^{-1}\widehat{\varvec{P}}_{k}^{-1}\varvec{J}^{-\textrm{T}},
\end{aligned}
(16)
$$
那么状态更新的公式如下：
$$\begin{aligned}
\check{\varvec{x}}_{k} = \check{\varvec{x}}_{k}\boxplus \left( -\varvec{K}\check{\varvec{z}}_{k} -\left( \varvec{I}-\varvec{K}\varvec{H}\right) \varvec{J}^{-1}\left( \check{\varvec{x}}_{k}\boxminus \widehat{\varvec{x}}_{k}\right) ,
\end{aligned}
(17)
$$
其中
$$\begin{aligned}
\varvec{K} = \left( \varvec{H}^{\textrm{T}}\varvec{R}^{-1}\varvec{H}+\varvec{P}^{-1}\right) ^{-1}\varvec{H}^{\textrm{T}}\varvec{R}^{-1}.
\end{aligned}
(18)
$$
接下来，我们介绍基于分层优化的IESKF光度更新。我们考虑映射框架中的点颜色噪声和当前图像中的对应颜色，光度残差的一阶泰勒展开得到：
$$\begin{aligned}
\varvec{o}\bigl (\varvec{x}_{k},{\phantom {a}}^{M}\varvec{p}_{s},\varvec{c}_{s}\bigr ) \approx \varvec{o}\bigl (\check{\varvec{x}}_{k},{\phantom {a}}^{M}\varvec{p}_{s},\varvec{c}_{s}\bigr ) +\varvec{H}_{s}^{\varvec{o}}\widetilde{\varvec{x}}_{k} +\varvec{\beta }_{s},
\end{aligned}
(19)
$$
其中 \(\varvec{\beta }_{s}\) 来自映射框架中的颜色误差和当前图像中的对应颜色误差。同时，\(\varvec{\beta }_{s}\sim \mathcal {N}(\varvec{0},\varvec{\Sigma }_{\varvec{\beta }_{s})\) 并且
$$\begin{aligned}
\varvec{H}_{s}^{\varvec{o}} = \left. \frac{\partial \varvec{o}\bigl (\check{\varvec{x}}_{k}\boxplus \widetilde{\varvec{x}}_{k},{{\phantom {a}}}^{G}\varvec{p}_{s},\varvec{c}_{s}\bigr )}{\partial \widetilde{\varvec{x}}_{k}} \right| _{\widetilde{\varvec{x}}_{k}=\varvec{0}},
\end{aligned}
(20)
$$
$$\begin{aligned}
\varvec{\Sigma }_{\beta _{s}} = \varvec{\Sigma }_{n_{c_{s}}} + \varvec{\Sigma }_{n_{\gamma _{s}}} + \varvec{F}_{\varvec{p}_{s}}^{\varvec{o}} \varvec{\Sigma }_{\varvec{p}_{s}} \bigl (\varvec{F}_{\varvec{p}_{s}}^{\varvec{o}}\bigr )^{\textrm{T}},
\end{aligned}
(21)
$$
$$\begin{aligned}
\varvec{F}_{\varvec{p}_{s}}^{\varvec{o}} = \frac{\partial \varvec{o}\bigl (\如果多个像素超过了阈值，那么梯度幅度最大的像素将被选为该块的代表像素。如果没有像素符合标准，就会增加块的大小并降低阈值，然后重复提取过程，直到获得合适的特征点。图4中展示了这种方法的示意图。

这种自适应提取方法会动态调整像素块的大小，以确保特征点分布均匀。如图4a所示，提取结果表明特征点分布均匀且具有显著的视觉梯度。SR-LIVO可以实时估计姿态，然后将这些姿态与提取的梯度点以及相机的RGB信息结合起来为点云着色，从而生成稀疏的彩色点云地图。图4b展示了这种稀疏映射的效果，稀疏特征提取确保了地图中的所有点都具有显著的视觉梯度，减少了内存使用，并防止了视觉定位陷入局部最优解。

在将点云地图与2D图像进行匹配的过程中，仅依赖深度滤波通常无法有效去除噪声，导致大量无关点的出现。为了解决这个问题，我们引入了HPR算法。与深度滤波不同，该算法不仅利用深度信息，还结合了遮挡感知滤波机制，能够识别并移除被其他物体遮挡或位于视野之外的点。这种方法使HPR算法能够保留更多有效且可见的点，显著提高了点云地图匹配的准确性。HPR算法的投影效果在图4c中得到了清晰展示，映射到图像上的点及其对应的像素显示出一致的色彩。

图5展示了我们的方法与VO/VIO方法的性能对比。a–j展示了我们的系统与VO/VIO方法在十个序列上的轨迹和残差随时间的比较。为了确保公平比较，所有方法都根据最早的共同有效时间进行了时间对齐，同时考虑了不同VO/VIO方法所需的不同初始化时间。

图6展示了ORB-SLAM3跟踪的示意图。以R3live_03为例，a显示了在正常结构化环境中提取的ORB描述符。b显示了当设备面向白色墙壁时，无法提取有效的ORB描述符。

在这个实验中，我们将我们的方法与ORB-SLAM3和VINS-Mono进行了比较，以评估我们系统的性能。VINS-Mono采用了一种紧密耦合的方法来整合视觉和IMU数据，从而提高了在低纹理环境中的姿态估计稳定性。相比之下，ORB-SLAM3是一个纯视觉里程计系统，它依靠ORB特征进行姿态估计，并在映射和闭环检测方面表现出色。通过将我们的方法与视觉惯性系统（VINS-Mono）和纯视觉系统（ORB-SLAM3）进行比较，我们得到了更全面的评估。图5表明，我们方法估计的轨迹与真实轨迹高度一致，展示了随时间的高准确性和稳定性。这种稳定性主要归功于我们的系统能够直接从先前的彩色点云地图中提取出对应于视觉特征的3D点。通过提供准确的深度信息，我们的方法确保了正确的尺度估计，这对于保持姿态准确性至关重要。此外，精确的深度数据还通过PnP算法改进了帧与帧之间的姿态估计，减轻了与三角测量相关的误差，并为后续的光度细化提供了可靠的初始估计。相比之下，VINS-Mono和ORB-SLAM3的性能并不总是令人满意。VINS-Mono虽然成功完成了所有测试序列，显示出一定的鲁棒性，但由于尺度漂移，其轨迹存在不同程度的偏差。尽管IMU测量有助于补偿视觉跟踪的失败（例如，纹理较弱或运动模糊），并改善了短期姿态估计，但其尺度仍然主要依赖于单目视觉观测，这仅提供了相对深度。因此，估计的尺度会随时间逐渐漂移，尤其是在大规模地下停车序列Whu_5中，由于轨迹较长且结构重复，VINS-Mono的准确性最差。

在六个数据序列中，我们的方法与DSL进行了比较。DSL（Ye & Huang，2020）是一种单目视觉里程计方法，它通过利用先前的点云surfel地图来提高姿态估计的准确性。这种方法的核心创新在于引入了共面约束来纠正实时姿态估计中的误差，从而能够准确估计具有绝对尺度的全局6自由度相机姿态。表2展示了通过ATE（RMSE）测量的定位准确性比较（单位：米）。

图7显示了我们系统与DSL的定位结果。在R3live_3和R3live_4上，DSL的轨迹出现了偏差，因此没有展示。从图7可以看出，在理想情况下（如R3live_1和R3live_2），DSL估计的轨迹与真实轨迹吻合良好。在R3live_5的停车序列中，DSL大致 following the global trajectory，但由于动态植被、光照变化和局部结构复杂性，导致轨迹出现了明显的偏差，这一点在表2中的ATE值上有所体现。在Whu_1、Whu_2、Whu_3和Whu_4序列中，轨迹在遇到未建模的退化场景之前保持了较好的一致性；一旦遇到这些退化场景，轨迹开始出现不同程度的偏差，无法完全回到正确的路径。在具有长轨迹和重复结构的大规模地下停车序列Whu_5中，DSL仍然能够完成任务，但轨迹显示出明显的偏差和尺度不一致性，导致定位准确性下降。在R3live_3和R3live_4中，由于场景严重退化（例如设备长时间面对墙壁），DSL的准确性最差。

与基于地图的视觉里程计（DSL）相比，在本节中，我们将我们的方法与DSL进行了比较。DSL利用先前的点云surfel地图来提高姿态估计的准确性。该方法的核心创新在于引入了共面约束来纠正实时姿态估计中的误差，从而能够准确估计具有绝对尺度的全球6自由度相机姿态。表2展示了通过ATE（RMSE）测量的定位准确性比较（单位：米）。

图7显示了我们系统与DSL的定位结果对比。在R3live_3和R3live_4上，DSL的轨迹出现了偏差，因此没有展示。在理想情况下（如R3live_1和R3live_2），DSL估计的轨迹与真实轨迹吻合良好。在R3live_5的停车序列中，DSL能够大致跟随全局轨迹，但由于动态植被、光照变化和局部结构复杂性，导致轨迹出现明显偏差，这在表2中的ATE值上有体现。在Whu_1、Whu_2、Whu_3和Whu_4序列中，轨迹在遇到未建模的退化场景之前保持了较好的一致性；一旦遇到这些场景，轨迹开始出现不同程度的偏差，不再完全回到正确的路径。在具有长轨迹和重复结构的大规模地下停车序列Whu_5中，DSL仍然能够完成任务，但轨迹显示出明显的偏差和尺度不一致性。然而，在其余序列中，运动模糊和低纹理区域导致无法可靠地提取ORB描述符（如图5所示），从而导致尺度偏差和定位准确性下降。

我们的方法与基于地图的视觉里程计（DSL）在六个数据序列中的比较。在R3live_3和R3live_4上，DSL的轨迹出现了偏差，因此没有展示。从图7可以看出，在理想情况下（如R3live_1和R3live_2），DSL估计的轨迹与真实轨迹吻合良好。在R3live_5的停车序列中，DSL能够大致跟随全局轨迹，但由于动态植被、光照变化和局部结构复杂性，导致轨迹出现明显偏差，这在表2中的ATE值上有体现。在Whu_1、Whu_2、Whu_3和Whu_4序列中，尽管在遇到未建模的退化场景之前轨迹保持了一致性，但一旦遇到这些场景，轨迹开始出现不同程度的偏差，无法完全回到正确的路径。在具有长轨迹和重复结构的大规模地下停车序列Whu_5中，DSL仍然能够完成任务，但轨迹显示出明显的偏差和尺度不一致性，导致定位准确性下降。

与基于学习的相机定位方法（如I2D-Loc++）在LiDAR地图中的比较。在本节中，我们将我们的方法与I2D-Loc++（Yu等人，2024a）进行了比较。I2D-Loc++是一种代表性的基于学习的跨模态相机姿态跟踪框架，用于LiDAR地图。I2D-Loc++将3D LiDAR地图投影到合成深度图像中，并使用学习到的图像到深度流来建立2D-3D对应关系，然后通过基于PnP的求解器和优化进行姿态跟踪，从而在一般条件下实现跨模态姿态估计。为了进行比较，我们使用相同的LiDAR地图和相同的校准相机内参/外参对这两种方法进行了评估。定位准确性以ATE（RMSE，单位：米）进行了报告。

我们的方法在所有序列上都实现了更高的准确性，特别是在具有挑战性的停车（R3live_5）和地下停车（Whu_5）场景中表现尤为突出。我们还进行了计算效率的比较。DSL采用了一种与直接方法类似的优化策略，但没有报告每帧的处理时间。为了确保公平评估，我们测量了每个数据集的总运行时间。如表3所示，我们的方法在大多数序列上实现了相当或更低的计算成本，并且在长轨迹（如R3live_5和Whu_5）上展现了显著的效率提升。计算效率的提高主要归因于稀疏特征的提取和迭代卡尔曼滤波器更新中采用的层次化优化策略。稀疏特征提取减轻了计算和存储负担，显著提高了效率。层次化优化方法构建了多分辨率图像金字塔，并实施了收敛终止策略，确保了收敛的准确性，同时限制了迭代次数，从而提高了效率。实际上，这种设计在每帧计算负载和整体定位准确性之间取得了良好的平衡，使得系统适用于资源受限的平台。在R3live_5公园序列中，I2D-Loc++表现出相对较大的平均绝对误差（ATE）（0.650米），这可能是由于非刚性植被和临时物体的存在导致对应关系稳定性下降所致。相比之下，我们的方法实现了0.152米的ATE，相当于提高了76.6%。同样，在Whu_5地下停车序列中，由于重复结构和低光照条件使得特征关联变得困难，I2D-Loc++积累了显著的漂移（0.285米），而我们的方法通过利用密集的颜色信息和全局地图约束保持了更高的准确性（0.139米）。总体而言，性能差异源于跨模态信息利用方式的本质区别。I2D-Loc++严重依赖于2D-2D光流的质量和学习的图像到深度流来生成精确的对应关系；在纹理低、运动模糊或突然移动的退化场景中，这些流场容易出现异常值或失败，从而影响整个跟踪流程。此外，基于PnP的下游求解器对对应关系的质量非常敏感，当预测的光流变得不可靠时，姿态估计也会受到影响。

相比之下，我们的系统通过利用彩色点云图的视觉梯度来构建约束。结合优先考虑视觉上显著地图点的稀疏策略和分层优化方案，即使局部视觉线索稀少或模糊，我们的方法也能保持稳健性。如表4所示，我们的方法不仅在R3live_3/4和Whu_3等极端情况下避免了发散，而且在所有评估的序列中都优于I2D-Loc++，为复杂真实世界环境中的定位提供了更可靠的解决方案。

正如（Yu等人，2024a）所报告的，I2D-Loc++的推理吞吐量大约为1帧/秒，这主要是由于其两个图像到深度分支和额外的光流网络的计算开销，以及下游的PnP和优化步骤。相比之下，如表3所示，我们的处理时间接近于数据持续时间，表明在实际应用中接近实时性能。

#### 多传感器与仅使用摄像机的定位对比研究
我们进行了多传感器基于地图的方法与仅使用摄像机的方法的对比实验，以量化在高规格传感器（LiDAR + IMU + Camera）仅用于地图绘制而实时定位完全依赖单目相机时的性能上限和准确性损失。在地图绘制阶段，Livox LiDAR、IMU和相机一起离线运行SR-LIVO，生成高精度的先验彩色点云图和参考轨迹；使用相同的数据，这种配置可以作为重型多传感器融合的近似上限。在这个共同的地图绘制基础上，我们比较了三种实时定位方案：R3live（实时紧密耦合的LiDAR–IMU–Camera）、FAST_LIO_LOCALIZATION（Fast-LIO2的定位模式）以及我们的方法，后者使用相同的彩色点云图，但仅使用单目图像和光度约束进行姿态估计。具体来说，FAST_LIO_LOCALIZATION通过将Livox AVIA点云与相同的先验SR-LIVO地图对齐来融合LiDAR–IMU状态估计和扫描到地图的对齐（它仅使用几何信息，不利用地图颜色），提供了一个强有力的参考，并揭示了与我们纯视觉跟踪方法之间的差距。

#### 消融研究
#### 多传感器与仅使用摄像机的定位对比
我们进行了多传感器基于地图的方法与仅使用摄像机的方法的对比实验，以量化在高规格传感器（LiDAR + IMU + Camera）仅用于地图绘制而实时定位完全依赖单目相机时的性能上限和准确性损失。在地图绘制阶段，Livox LiDAR、IMU和相机一起离线运行SR-LIVO，生成高精度的先验彩色点云图和参考轨迹；使用相同的数据，这种配置可以作为重型多传感器融合的近似上限。在这个共同的地图绘制基础上，我们比较了三种实时定位方案：R3live（实时紧密耦合的LiDAR–IMU–Camera）、FAST_LIO_LOCALIZATION（Fast-LIO2的定位模式）以及我们的方法，后者使用相同的彩色点云图，但仅使用单目图像和光度约束进行姿态估计。具体来说，FAST_LIO_LOCALIZATION通过将Livox AVIA点云与相同的先验SR-LIVO地图对齐来融合LiDAR–IMU状态估计和扫描到地图的对齐（它仅使用几何信息，不利用地图颜色），提供了一个强有力的参考，并揭示了与我们纯视觉跟踪方法之间的差距。

#### 测试结果总结
如表5所示，在结构上约束良好的环境（R3live_1、R3live_2、R3live_5、Whu_5）中，对于LiDAR，R3live在所有方法中始终实现了最低的ATE。在这些序列中，FAST_LIO_LOCALIZATION的表现略优于或与我们的方法相当，而仅使用摄像机的系统则略差或相当，这反映了在扫描到地图的约束稳定且信息丰富时利用LiDAR–IMU几何信息所带来的残余优势。相比之下，在包含强烈运动和传感器退化（快速旋转、运动模糊和临时失去稳定视点）的Whu_1–Whu_4序列中，FAST_LIO_LOCALIZATION主要依赖于稀疏的几何约束进行对齐，并且对退化或模糊的扫描到地图对齐更加敏感，导致ATE略高于我们的方法。我们的方法通过强制彩色地图和图像之间的光度一致性，仍然能够从残余的纹理和颜色中提取有用的约束，因此在这些具有挑战性的序列中实现了略微更好的准确性。

在最为退化的R3live_3和R3live_4序列中，传感器长时间面对墙壁或地面，导致LiDAR观测和先验地图在局部退化为大片几乎无特征的平面。在这种长期的几何退化情况下，扫描到地图的约束变得薄弱且条件不良，迫使估计器更加依赖IMU的传播；结果，累积的漂移和偏差无法得到充分校正，导致仅使用几何信息的FAST_LIO_LOCALIZATION基线的ATE大幅下降（见表5）。为了保证可复现性，我们报告了FAST_LIO_LOCALIZATION基线中使用的代表性调整参数：lidar_blind = 0.1米，filter_size_map = 0.2米（体素-地图分辨率），filter_size_surface = 0.2米（平面/表面分辨率）。相比之下，我们的方法通过增加彩色点云和图像之间的颜色和光度线索来增强约束，即使几何信息较弱，也能利用细微的纹理和颜色差异，从而在R3live_3和R3live_4上保持稳定的跟踪并生成误差较小的完整轨迹。总体而言，SR-LIVO离线地图绘制为多传感器融合性能提供了近似上限，而在在线方法中，R3live是最强的LiDAR–IMU–Camera基线；我们的仅使用摄像机的定位在许多结构良好的场景中实现了可比的准确性，并在几何条件退化的场景中表现出更强的稳健性，同时大幅降低了传感器要求和系统复杂性。

#### 提出系统的关键组件消融研究
除了与多传感器基于地图的基线进行对比外，我们还对系统的主组件进行了消融研究，以量化每个设计选择的贡献。具体来说，我们研究了三个方面：（i）使用彩色点云图进行视觉跟踪，（ii）在地图构建和定位中的双向稀疏策略，以及（iii）分层几何-光度卡尔曼风格更新。

#### 彩色点云图和光度约束的影响
为了评估颜色信息及其相关光度约束的作用，我们考虑了三种变体，除了完整系统之外。第一种是仅使用几何信息的变体（PnP-only, Ours w/o Color），其中先验地图转换为无色点云，仅在优化中使用几何约束：光度更新被禁用，深度仅用于支持基于LK的PnP姿态估计。这种变体相当于在状态层面上仅使用PnP进行几何更新。第二种是仅使用光度信息的变体（Photo-only），我们保留了彩色点云图，但禁用了PnP/几何更新，直接应用从先验传播而来的光度IESKF更新，即仅通过最小化光度残差来细化姿态，而不使用2D-3D PnP约束。完整方法（Full (Ours)）在彩色点云图上结合了几何（PnP）和光度更新。

#### 双向稀疏性的影响
我们通过以诊断为导向的消融研究来评估所提出的双向稀疏策略的贡献。在“Ours w/o Bidirectional Sparsity”变体中，我们禁用了地图构建和在线跟踪中的高梯度点选择。这增加了参与优化的点数，但也引入了许多信息量低或不可靠的约束（例如，纹理薄弱区域、局部重复结构和噪声观测）。表7报告了每种变体以及在实时（RT）执行下的序列平均绝对误差（RMSE）以及处理帧率（Proc. Ratio），同时使用了固定率的输入以及使用相同点预算但不同点选择策略的固定预算控制。结果表明，密集的、未经过滤的设置通过两种耦合效应降低了性能：首先，它增加了每帧的计算量和延迟；在RT执行中，这可能导致积压累积和随后的帧跳跃，当系统无法跟上时（表现为Proc. Ratio降低），这会破坏时间连续性，并可能在长序列或具有挑战性的序列中触发跟踪丢失。例如，在R3live_2和Whu_5中，Proc. Ratio降至50%以下，这与密集变体中严重的积压导致的跟踪失败密切相关。其次，即使系统仍然处理了大量帧，密集变体在多个序列中也产生了更高的ATE，表明退化不仅由计算拥堵引起，还与约束质量下降和异常点压力增加有关。

为了将“点数”与“点质量”分开，我们进一步评估了固定预算控制（Budget-N），其中用于优化的点数保持与完整系统相同（每帧500个点），但仅改变点选择策略（随机采样或体素网格降采样）。表7显示，在相同的点预算下，这些简单的策略仍然不如双向稀疏性稳定和准确。这表明，好处不仅仅是使用更少的点，而是选择了更多具有信息量和可靠性的约束，从而提高了可观测性，减少了异常点的影响，并稳定了收敛。总体而言，双向稀疏性提高了实时可行性，并通过将地图和在线观测集中在结构上显著的区域来增强稳健性。

#### 分层几何-光度卡尔曼风格更新的影响
最后，我们研究了在迭代卡尔曼风格更新中引入分层优化方案的影响。我们构建了一个基线（Ours w/o Hierarchy），该基线采用单层几何-光度优化：姿态仍然通过解决PnP问题并结合光度残差来更新，但光度细化仅在单一尺度上进行，没有使用粗到细的图像金字塔或任何早期停止策略。如表8所示，这种变体不仅产生了明显更大的轨迹误差，还导致更频繁的跟踪失败。在R3live_2、R3live_3、R3live_5、Whu_3、Whu_3和Whu_5等具有挑战性的序列中，Ours w/o Hierarchy无法完成整个轨迹。即使在仍能产生有效结果的序列（例如R3live_1、R3live_4、Whu_1、Whu_2）中，其ATE也始终比完整系统差。在运行时间方面，Ours w/o Hierarchy仅将总处理时间减少了约0-4%，表明简单的单层几何-光度优化并没有带来显著的效率提升。相比之下，完整的分层优化首先在几何层面上获得稳定的初始姿态估计，然后使用粗到细的金字塔和早期终止机制对光度残差进行精细化优化。这项技术显著提高了Accuracy（精度）并显著增强了收敛的稳定性，同时仅引入了非常小的额外计算开销。总体而言，这些实验结果表明，所提出的三个模块——即彩色点云地图、双向稀疏策略以及分层几何-光度卡尔曼式更新——对于实现精度、鲁棒性和效率之间的平衡都是不可或缺的。移除其中任何一个模块都会导致定位性能明显下降，这证实了相对于视觉里程计/视觉惯性测量（VO/VIO）和多传感器基线的改进不仅仅是由于使用了先验地图，而是源于我们地图表示和优化框架的独特设计。

**结论**
本文介绍了一种基于稀疏彩色点云地图的视觉定位系统，旨在克服传统基于地图的视觉定位中遇到的轨迹漂移和高计算成本问题。首先，我们提出了一种地图稀疏化策略，该策略将视觉梯度与点云关联起来，仅保留最重要的梯度特征以降低计算需求。这一策略被集成到视觉到地图的匹配过程中，形成了“双重稀疏匹配”方法，使得具有高梯度的视觉特征和地图点对齐，从而有效减少了收敛到局部最优解的风险。接下来，我们引入了一种基于分层优化的迭代卡尔曼滤波算法，该算法采用恒定速度假设进行运动预测，并将帧间重投影误差和帧到地图的光度误差作为观测值。该算法在多种图像分辨率上执行优化，从而防止定位陷入局部最优解并提高其精度。实验结果表明，所提出的方法超越了当前的视觉-惯性系统和基于地图的方法，在长期稳定性、计算效率和鲁棒性方面表现更优。整个框架如图8所示。未来，我们将探索基于环境变化的自适应地图稀疏化方法，并结合惯性测量单元（IMU）的测量数据，以进一步提高系统在不同场景下的性能。

**局限性与未来工作**
我们的系统将几何先验的姿态更新与针对彩色点云地图的直接光度细化相结合，这提高了精度，但在极端光照不一致的情况下（例如曝光/伽马值变化、强阴影、镜面反射以及昼夜变化）暴露出了关键的限制。在这种情况下，光度残差会变得有偏，从而缩小收敛范围并降低细化的效果，尤其是在低纹理/饱和区域或地图颜色随时间漂移的情况下。虽然分层粗略到精细的调度稳定了初始化过程并抑制了高频噪声，但几何先验阶段在光度项不可靠时也提供了一种鲁棒的备用方案，但它无法消除外观偏差。因此，在未来的工作中，我们将结合光照耐受性细化技术（例如联合增益/偏差估计和标准化/梯度恒定性），以及考虑不确定性的鲁棒性门控机制，并在受控的光照压力测试中评估其收敛性和精度退化情况。
直接的像素匹配也使得光度阶段容易受到动态场景的影响：非地图像素和移动物体的瞬态遮挡会引入异常残差，这些残差可能在高度动态的场景中主导更新结果，使估计值偏离几何一致的姿态。双重稀疏性和可见性感知的关联机制可以减轻不匹配问题，但并未明确拒绝动态/非地图像素；目前，我们依靠可见性感知的关联和光度目标中的鲁棒加权来减少瞬态遮挡的影响，但我们认识到这并非一个专门的动态物体拒绝模块。因此，在未来的工作中，我们将集成一个专门的非地图像素抑制模块（例如基于不确定性的残差掩蔽、鲁棒M估计和时间不一致性检查，并可选地加入轻量级的语义先验），并通过现场动态实验和定量实验对其进行验证，同时保持核心的双重稀疏性和分层优化流程。