SA-LIVO：激光雷达和相机「按方向选择性融合」，无GPU稳跑10Hz，12.3ms/帧且内存暴降6.3倍！

点击下方卡片，关注「3D视觉工坊」公众号
选择星标，干货第一时间送达

来源：3D视觉工坊

3D视觉硬件汇总，包括结构光3D相机(支持室内外，焊接，机械臂抓取等场景)，相位偏折术、散斑、机械臂抓取和焊接软件，手持扫描仪(支持定位、重定位、稠密建图)、四足狗和小车定位和导航，底盘。点击->查看详情：

紧耦合激光-视觉-惯性里程计通过融合精确的几何深度与互补的视觉测量实现鲁棒定位，但其外部传感器面临独立的失效模式。激光雷达退化发生在扫描几何约束不足时；视觉测量在不良光照或纹理缺失条件下退化。现有应对措施，包括二值退化检测、协方差膨胀和场景级质量门控，均在模态层面操作，未解决联合信息矩阵的方向依赖结构。因此，视觉残差进入了激光雷达已经良好约束的位姿方向，而在不足的方向上，视觉补偿分散到整个状态空间而非集中到需要的地方。本文提出 SA-LIVO，一个完整的激光-惯性-视觉里程计系统，通过方向选择性融合和信息高效传感器处理来解决这些局限性。子空间感知信息融合框架计算联合激光-视觉信息矩阵的特征分解，并对每个特征方向应用单阈值线性钳位软门控，在衰减退化方向的同时以全强度保留可观测方向。激光和视觉残差随后在单个 InEKF 循环中在共享线性化点处联合优化，消除了顺序更新的线性化点不匹配问题。效率直接源于融合策略：由于视觉信息仅在激光雷达不足的方向上贡献，光度雅可比只需在 InEKF 循环之前组装一次，并在所有迭代中重用，避免了传统迭代滤波器的每迭代视觉处理成本。在三个公开基准（HILTI'22、New College、Oxford Spires）的 29 个序列以及额外的并发退化场景上的实验表明，精度与最强基线相当，并且在竞争性激光-视觉-惯性系统发散的顺序中保持有界漂移。SA-LIVO 在笔记本电脑 CPU 上平均每帧 ，在无 GPU 加速的嵌入式 ARM 板上为 ，峰值内存相比竞争方法降低  倍。

I. 引言

图1：SA-LIVO在多样化环境中的代表性建图结果——(a1)-(c3) Oxford Spires 室外和室内场景；(d)-(f) 自采集室内和地下序列；(g)-(h) FAST-LIVO2 数据集

在未知环境中运行的自主机器人和车辆，同时定位与建图是一项基础能力。在最近的 SLAM 范式中，激光-惯性-视觉里程计通过紧耦合三种互补模态受到越来越多的关注：激光雷达提供精确的深度和密集几何约束；相机提供丰富的视觉测量；IMU 提供高频运动学先验。在几何结构化的环境中，这种紧耦合实现了远超单模态方法的精度。然而在实际中，一个或两个外部传感器都可能退化。激光雷达退化发生在观测几何无法在一个或多个位姿方向上提供足够约束时，例如点面匹配变得近似共面，使某些平移或旋转自由度未被观测到。相机退化发生在成像条件抑制可用图像梯度时：黑暗、运动模糊、过曝光或无纹理表面都可能使视觉测量不可靠。由于这些失效模式由独立的环境因素（几何 vs. 光照）驱动，并发退化不仅可能而且常见。在这种情况下，现有的紧耦合系统往往会发散，而不是优雅地退化到最可靠的可用约束。

自然的应对是通过紧融合实现相互补偿，但现有传感器退化处理方法在传感器模态层面而非联合信息矩阵的方向层面起作用。二值退化检测通过拒绝或约束激光雷达更新来响应病态扫描匹配 Hessian，放弃了近乎退化的扫描仍然提供的部分约束。协方差膨胀和标量降权各向同性地衰减传感器的整体贡献，在良好约束和不良约束的位姿方向上同等降低其影响。场景级质量门控决定是否在给定帧中应用视觉更新，但不控制其残差在状态空间中的作用位置。实际的失效结构本质上是逐方向的：单次激光雷达扫描可能同时过约束某些旋转方向，同时使某个平移自由度未被观测到；视觉梯度可能强约束某些位姿轴，而在其他方向贡献可忽略的信息。模态级干预无法解决这种方向不匹配：光度残差进入激光雷达已经可靠的区域，而视觉补偿无差别地扩散，而非集中在激光雷达约束真正不足的方向上。

在这种方向盲融合下，计算也被错误分配。视觉-惯性里程计流水线包含跟踪、雅可比组装和滤波器更新，无论场景几何如何，每帧都产生固定成本。然而其在良好约束方向上的边际精度增益有限，在退化方向上则稀释到所有方向。因此计算按帧数而非按产生的精度增益比例消耗。

本文提出 SA-LIVO（子空间感知激光-惯性-视觉里程计），一个可部署的实时激光-惯性-视觉里程计系统，其核心贡献是方向选择性传感器融合，并辅以匹配此融合策略的激光和视觉流水线，使系统足够高效以在笔记本电脑和嵌入式硬件上实时部署。SA-LIVO 的贡献包括：

1）子空间感知信息融合（SAIF）。我们通过单一标量阈值对联合激光-视觉信息矩阵的每个特征方向按其信息幅度进行门控。这将四个需手工调参的每传感器参数替换为一个，并产生一个可证明半正定的融合矩阵，能连续退化为单传感器更新而无需模式切换。

2）统一单循环联合 InEKF 更新。激光和视觉残差在公共线性化点处共享一个 InEKF 循环，消除了顺序每传感器更新的线性化点不匹配，并将迭代次数限制在固定预算内，无论融合的模态数量如何。

3）高效预计算视觉信息形式。光度雅可比在传播状态处计算一次，并在所有 InEKF 迭代中重用，消除了迭代光度滤波器中占主导的每迭代 VIO 成本。该形式在滑动窗口上累积以跨越比任何单一视点更丰富的子空间，并带有每观测去相关因子以防止重用膨胀视觉信息矩阵。

4）高效多尺度平面建图。每体素充分统计量实现常数时间多尺度 PCA 和无量纲平面性准则，将平面拟合成本与点数解耦，并消除了每序列重调固定特征值测试的需要。平面对应关系在第一次 InEKF 迭代后缓存，使后续迭代仅重新计算点到平面距离，而饱和优先策略有利于长期冻结体素，防止漂移污染的新近表面主导激光信息矩阵。

我们在 HILTI'22、NCD 和 Oxford Spires 基准的 29 个序列以及自采集数据集和 FAST-LIVO2 数据集上评估 SA-LIVO。SA-LIVO 在评估基准上达到有竞争力的精度，并在 R3LIVE 和 SR-LIVO 发散的顺序上保持有界漂移。我们的代码和数据集将公开。

II. 相关工作

A. 激光里程计与退化

基于迭代最近点或点面匹配的直接激光里程计在结构化环境中达到高精度。然而，当扫描几何沿一个或多个方向提供不足约束时（如长走廊、大开放楼层或隧道），扫描匹配 Hessian 变为奇异或近奇异，导致无约束状态更新和快速漂移。几项工作通过匹配 Hessian 的特征谱检测此条件，并通过切换到降自由度更新或拒绝扫描来响应。更近期的可定位性感知配准在 ICP 层面应用方向约束滤波，但门控仍停留在激光流水线内部，未传播到融合的多传感器信息形式中。虽然能有效防止发散，但二值切换是全有或全无的，忽略了近乎退化方向仍提供的部分约束。轻量级现代激光里程计如 KISS-ICP 表明，简单的点点匹配配合自适应运动模型在良好约束场景中具有竞争力，但未提供部分退化的显式处理机制。除退化检测外，匹配中使用的平面表示的可靠性构成单独挑战。VoxelMap 引入自适应体素分辨率和平面不确定性，但其平面接受准则使用绝对特征值阈值，需针对不同体素大小、点密度和场景尺度重调。相比之下，我们的平面性比是无量纲且尺度不变的，单一阈值可跨体素大小、点密度和场景尺度迁移而无需重调。

B. 视觉-惯性里程计

基于滤波的 VIO 可追溯到 MSCKF，现代开源流水线如 OpenVINS 和 ROVIO；后者在无激光耦合的迭代 EKF 内部应用直接光度更新。基于关键帧的 VIO 系统如 VINS-Mono、ORB-SLAM3 和 OKVIS 从多帧三角化或立体基线恢复地图点深度，深度不确定性随距离增长。流形上的 IMU 预积分是关键帧 VIO 的标准；在我们的系统中，同一角色由 InEKF 流形上的中点积分填补。

当激光深度可用时，特征像素很少与稀疏激光点重合，因此基于特征的跟踪必须从附近点插值深度，并带有几何相关的误差。直接方法通过在直接从激光扫描获取的预锚定 3D 点处采样光度残差来规避此问题，使其成为紧耦合激光-视觉的自然选择，但代价是需要精确的位姿先验。

即使激光点作为深度锚点，深度本身并非严格可靠：扫描线间隙、镜面表面如玻璃和水都可能产生缺失、虚假或有偏差的回波。单像素光度残差也对图像噪声和光照变化敏感。基于面片的光度公式缓解了这两个问题：对小邻域内的残差求平均可吸收孤立的锚点误差和每像素噪声，同时增加每个激光锚定点贡献的信息。DSO 引入首次估计雅可比技术和每帧仿射亮度校正，但在纯单目设置中运行，无激光深度锚定。我们的直接、基于面片的 VIO 将每个地图点锚定到激光测量，并在 InEKF 循环前计算一次雅可比。每观测信息去相关和每帧仿射亮度补偿保持视觉信息形式无偏且对光照变化鲁棒。

C. 基于不变滤波器的估计

不变扩展卡尔曼滤波器将导航状态置于矩阵李群上，并利用群仿射系统对称性获得独立于系统轨迹的传播雅可比。Brossard 等将其表征为可观测性保持性质：全局偏航和绝对位置的非可观子空间在所有滤波器迭代中保持不变。相比之下，标准左不变 EKF 在快速运动下可能将信息投影到不可观方向，引入线性化偏差。Hartley 等在全腿式机器人状态估计系统上验证了该公式，表明 IMU、运动学和接触残差可在单个右不变循环内一致融合。InvLIO 随后将 InEKF 扩展到激光-惯性里程计，在李代数中表达点面残差以保持右不变结构。SuIn-LIO 进一步将 InEKF 与 surfel 建图结合，在公开基准上展示了一致的状态估计和实时扫描配准。SA-LIVO 将混合右不变 InEKF 公式应用于联合激光-视觉设置：光度残差在同一右不变误差约定下公式化，并在单个统一迭代循环中与激光残差融合，据我们所知是首批这样做的 LIVO 系统之一。

D. 激光-视觉-惯性融合

松耦合 LIVO 系统使用一个传感器的输出初始化另一个；它们轻量且易于实现，尽管紧耦合通常在挑战性场景中带来更好精度。紧耦合系统联合优化所有传感器的残差，但融合架构差异显著。LIC-Fusion 开创了 MSCKF 风格的特征级激光-相机-IMU 融合，而后续基于滤波的流水线转向使用稀疏激光锚定点的直接光度更新。FAST-LIVO2 通过顺序迭代 EKF 循环处理激光和视觉残差实现强精度，但两种模态在不同状态估计处线性化，顺序结构增加了相对于联合循环的总迭代次数。R3LIVE 通过像素级光度模型构建稠密彩色地图，以更高每帧计算为代价提供丰富场景表示。LVI-SAM 在因子图中组合激光和视觉因子以实现灵活的多会话建图，性能取决于两种模态中可靠的特征提取。基于平滑的系统在因子图后端中为状态增加语义或运动学模态，展示了紧耦合范式的通用性。然而，在这些设计中，激光和视觉信息之间的权重统一应用于所有位姿方向：没有现有工作分析联合信息矩阵的方向依赖谱结构，或在传感器不可靠的特定方向上衰减其贡献。现有退化感知方法在模态层面做二值接受/拒绝决策。协方差加权方法缩放整体传感器置信度但仍是各向同性注入。场景级门控决定是否使用视觉更新，但不控制其在状态空间中的作用位置。SA-LIVO 通过在单个统一信息形式中结合联合特征基线性钳位软门控与场景级 VIO 质量因子和每观测去相关来解决此差距。

图2：SA-LIVO系统概述——LiDAR、相机和IMU流从顶部进入；IMU测量驱动连续状态传播。蓝色路径为激光-惯性里程计；橙色路径为VIO；红色路径为SAIF+联合更新；绿色路径为体素地图

III. 系统概述

SA-LIVO 包含两个并行信息流水线——激光-惯性和视觉——它们馈入联合 InEKF 更新，如图2所示。系统处理激光（10 Hz；支持其他频率）、相机（10 Hz）和 IMU（200 Hz）数据流。异步采样的激光点通过 IMU 后向传播以相机速率重组成扫描，确保激光和图像数据之间的时间对齐。激光和视觉测量随后通过联合右不变迭代扩展卡尔曼滤波器紧耦合。

在激光分支（第V节）中，下采样激光点匹配到自适应体素地图中的平面。每个匹配产生一个点面残差  及其完全传播的方差 。经  门控后，存活的匹配累积为激光信息矩阵  和向量 。饱和体素（平面已良好收敛的）优先于新构建的体素，以抑制漂移污染的约束。

在视觉分支（第VI节）中，激光锚定的 3D 地图点投影到当前图像。每帧仿射亮度模型补偿曝光变化，之后光度残差与滑动窗口中的参考观测进行比较。雅可比在传播状态处计算一次并在所有迭代中重用。经每观测信息去相关和多门控离群点剔除后，跟踪点累积为  和 。场景级 VIO 质量因子  在融合前门控视觉信息。

SAIF（第VII节）对联合信息矩阵  进行特征分解以识别总可观测性结构。每个特征方向随后通过  进行软门控，产生融合形式 。InEKF 求解所得线性系统以得到状态修正。收敛后，体素地图用精修位姿更新。

表I：符号汇总表——包含状态、协方差、旋转、位置、速度、偏置、流形算子、信息矩阵、质量因子、特征值、门控权重、点坐标、法线、残差、雅可比等符号定义

IV. 状态表示与 IMU 传播

我们基于不变扩展卡尔曼滤波器构建，将导航状态置于矩阵李群上，并利用其对称性减少标准左不变 EKF 产生的线性化偏差。我们采用混合右不变参数化，以适应本文核心的联合信息形式融合。姿态误差遵循右不变约定，给出单位姿态自转移，消除了左不变 EKF 每步的姿态误差旋转；而位置和速度误差保持欧氏。这种欧氏选择是刻意的：它使激光和视觉信息形式以及基于它们的 SAIF 特征分解保持在单个线性误差空间中，而完整的  参数化会非线性地重参数化速度误差并破坏该兼容性。完整的群仿射一致性保证是完整  InEKF 的性质；我们采用混合形式以便整个误差状态保持在单个线性空间中，这是 SAIF 特征分解（第VII节）在相干误差表示上操作的先决条件。一个后果是轨迹相关的速度-旋转耦合块，将在本节后面精确给出。我们的关键修改是统一的联合更新：激光和视觉信息形式在单个线性化点处融合，并在一个迭代循环中求解，消除了顺序每模态更新的线性化点不匹配。

A. 状态与流形

我们取 IMU 坐标系为机体坐标系，世界坐标系为全局参考系，与文献[8]一致。状态流形为 ，，状态向量为：

其中  为 IMU 到世界坐标系旋转， 为世界坐标系位置和速度， 为陀螺和加速度计偏置， 为在线估计的重力向量。误差状态为 ，其中  算子采用右不变约定：，状态更新 。我们采用混合右不变参数化，其中旋转误差为右不变的，而速度和位置误差保持欧氏 。除了上述融合兼容性理由外，此选择还简化了传播：旋转自转移变为单位矩阵 ，消除了 FAST-LIO2 的左不变约定中每步姿态误差的旋转，并简化了协方差传播。一个结构后果是速度-旋转耦合块变为 （世界中心的比力斜对称）而非左不变公式的机体框架 。两者等价于耦合方向的一个旋转，不引入额外近似。完整的  Barrau-Bonnabel 构造将进一步给出轨迹无关的 ，但它需要重参数化速度误差本身 ；我们保留欧氏  以与下游信息形式融合兼容。

B. IMU 传播

在连续传感器帧之间，高频 IMU 测量提供运动学连续性的唯一来源；前向传播将状态估计推进到激光扫描终点，并通过后向积分去畸变点云。第  个 IMU 测量的离散状态转移模型为：

其中  为原始 IMU 测量， 为过程噪声，运动学函数  按文献[8]定义。状态协方差传播为：

其中  和  为式(2)的状态和噪声雅可比，在当前估计处求值。扫描间隔内获取的激光点通过 IMU 后向传播进行运动补偿，然后进入测量更新步骤。

V. 尺度不变漂移鲁棒 LIO

激光信息形式  通过将下采样的机体框架点匹配到体素地图中的平面表面，并累积加权点面残差来组装。本节推导残差和右不变雅可比，描述自适应地图结构和平面拟合流水线，呈现信息一致的噪声模型，并详述多尺度对应搜索策略。

A. 点面残差

每个下采样的机体框架点  通过激光-IMU 外参  和当前状态  转换到世界坐标系：

给定体素地图中匹配的平面 ，有符号点面距离为：

其中 。在右不变误差约定  下，关于  的雅可比为：

推导如下。将  扰动到一阶，得到 ，因此 ，其中  由 SO(3) 伴随恒等式给出。旋转块因此依赖于世界框架位置  而非机体框架点；直接使用  会将旋转不确定性错误地投影到平面法线上。

B. 自适应体素地图结构

地图采用由每个条目包含八叉树的哈希表组织的自适应体素结构，其边长根据点的机体框架距离  自适应：

其中  为无量纲比率， 为最大体素大小。对数量化缩放匹配旋转激光雷达的波束足迹增长：在距离  处相邻波束间隔约 ，其中  为角分辨率，因此单位面积点密度按  下降。固定单元大小因此会将远场回波隔离到单个体素中，点数太少无法可靠拟合平面。通过每次  翻倍时使单元大小翻倍，每体素期望点数保持大致恒定，使主成分分析在传感器全范围内保持良好条件。平面性准则、增量 PCA 和不确定性模型共享相同的每体素充分统计量。

C. 尺度不变平面性准则

当点分布足够各向异性时，体素被接受为平面表面。传统测试  将最小 PCA 特征值与固定  阈值  比较。因为  随体素体积（大致 ）和局部点密度缩放，同一  不能跨传感器配置或场景尺度迁移。

我们改用无量纲比率：

其中  为体素中  点样本协方差。 为完美平面； 为各向同性。作为特征值之比， 是尺度不变的：同一阈值  适用于任何体素大小或距离。体素仅在两个条件都满足时才被接受为平面：

其中  为可配置的绝对保护值。 测试是主要的尺度不变门控； 防止接受绝对平坦度仍受传感器噪声主导的拟合，独立于点计数或体素尺度。一旦两个条件都满足，体素统计被冻结，不再执行进一步的点更新。

图3：平面性感知体素冻结——随着激光点在体素内累积，局部平面结构逐渐变得可观测，估计法线稳定。一旦平面性比满足 ，体素统计被冻结，不再进行进一步更新

D.  多尺度 PCA 通过充分统计量

单个体素可能包含太少的点而无法可靠估计平面，尤其是在回波稀疏的远距离。自然的补救是聚合邻近体素进行更粗尺度的拟合。朴素地，这需要从邻域收集所有点并重新运行 PCA：一个  操作，当邻域包含数千个累积点时是 prohibitive 的。

我们观察到 PCA 仅需要三个聚合量，即充分统计量，因为它们捕获了协方差公式所需的一切，并省去了保留原始点本身。样本协方差可从以下精确恢复：

使得 。每个体素在每次新点插入时增量维护 ，无论当前是否满足平面性测试。关键是，即使未能通过  的体素也维护有效的充分统计量，可贡献于邻域拟合。聚合  邻域然后仅需要  次哈希表查找和向量加法，每体素一次，与每个体素包含多少点无关。 对称特征值求解器在  内完成 PCA。

E. 噪声与不确定性模型

为了形成信息一致的 ，每个残差必须按其真实期望噪声方差缩放。遵循文献[12]中引入的双分量波束对齐分解，每个机体框架激光点  被视为受两个独立噪声源影响：沿波束方向  的测距噪声 ，由飞行时间量化引起；以及垂直于波束的角度噪声 ，将编码器噪声和波束发散扩展吸收到单个各向同性横向项中。这些给出机体框架点协方差：

其中 。横向项按  缩放，因此远场点具有显著大于近场点的垂直不确定性。传播到世界框架并合并当前状态不确定性：

其中 ，， 为右不变雅可比  所需的世界中心 IMU 框架向量（在剧烈运动下此处使用  会将旋转不确定性错误投影到平面法线上）。此耦合在状态不确定性大时自动衰减信息贡献，防止过度自信的更新。

从小型、有噪声或分布不良的点簇拟合的平面不如从数百个分布良好的回波构建的可靠；将所有平面视为同等可信会导致过度自信的信息贡献和有偏的状态更新。对每个接受的平面 （单位法线和质心），我们通过一阶特征向量扰动理论将每点不确定性  传播通过 PCA，获得平面参数协方差 。敏感性由 （垂直于平面）与面内特征值之间的特征间隙控制：厚实、分布良好的簇牢固地固定法线，而薄或细长的簇让小的扰动显著旋转它。具体地，最小特征向量  对点  的敏感性为：

对  为零。每行  记录  每单位扰动向  倾斜多少，幅度为 ，符合特征间隙直觉。堆叠法线和质心敏感性：

其中  收集特征向量。 平面协方差为：。

F. 激光信息矩阵组装

总残差方差，传播平面不确定性和点噪声：

其中 ，。经  新息门控  拒绝离群点) 后，激光信息形式为：

其中  为通过门控的点索引集。式(17)中的负号遵循梯度下降约定 ，与视觉信息向量(32)一致。此流水线将波束级噪声通过机体框架、世界框架和平面参数传播到每点残差方差。所得  是信息一致的：每个条目反映真实期望平方信息梯度，而非固定的临时标量。

体素搜索、法线查找和式(6)中的雅可比  仅依赖于匹配平面 ，在 InEKF 迭代中不变。这些对应关系因此在第一次迭代  后缓存；后续迭代仅重新计算标量点面距离 ，将每迭代激光成本从  降低到 。对于  个下采样点和  次迭代，这消除了每帧  次体素哈希查找。

G. 饱和优先多候选平面选择

每个下采样激光点必须匹配到地图上的一个平面。匹配策略必须回答两个问题：在地图上的何处寻找平面，以及多个看似合理的匹配中保留哪一个。两个选择都决定了地图中积累的漂移有多少能泄漏到激光信息形式  中。我们的策略是将匹配偏向于过去被观测多次的平面，因为这些平面有效地平均了短期位姿漂移，而仅由少数最近扫描构建的平面继承了插入时存在的任何位姿误差。

对每个查询点，在两个空间尺度上搜索候选平面，使密集近场和稀疏远场几何由同一程序处理。

图4：用于几何约束关联的分层体素搜索——查询点首先探测中心体素（Scale-0），然后扩展到同心Chebyshev壳层（Scale-1）

在 Scale-0 我们仅探测包含查询点的单个体素，若该体素为空则偏移一个体素。这在局部表面已经良好采样时足够，这是短距离的常见情况。

在 Scale-1 我们围绕查询体素以同心壳层向外扩展，并在每添加新壳层时在累积点集上重新拟合平面。壳层  是距中心 Chebyshev 距离为  的体素集。扩展在双重平面性准则(9)满足或达到最大壳层索引  时停止 。稀疏远场簇因此只需付出所需数量的壳层；密集近场簇在  停止。当 Scale-0 已返回饱和平面且  和  时，Scale-1 完全跳过，避免在稳定区域中的冗余工作。

当两个尺度的候选都存活时，仅保留法线在最近平面候选法线  内的候选。这移除了恰好在查询点几何附近但属于不同物理表面的匹配。

剩余候选按作为无漂移锚点的可信度排序，而非按距离远近。当体素的累积点数达到每体素上限时称为饱和，此时其平面参数被冻结不再更新。饱和平面因此总结了来自不同视点的许多过去观测，对任何单帧的瞬时位姿误差 largely insensitive。新近插入的平面则相反，由仅少数近邻位姿贡献的少量点构建，因此这些位姿中的任何漂移直接传播到平面。

最终选择按四个有序轮次进行；仅当较早轮次返回空时才咨询后一轮次：

• 第1轮：仅接受饱和平面（长期锚点）。
• 第2轮：添加仍满足平面性准则  的非饱和平面。
• 第3轮：接纳任何几何有效平面。
• 第4轮：当无平面候选合格时，回退到归一化残差  最小的候选。

此排序将  偏向于已被许多过去观测交叉验证的平面，防止地图中最新的、漂移污染的表面主导滤波器更新。

VI. 激光锚定的直接光度 VIO

直接光度 VIO 避免了特征提取和深度三角化不确定性。然而，在迭代滤波器内部朴素应用它，会在每次迭代重新计算昂贵的雅可比，并在滑动窗口帧间累积时间相关性。本节展示如何通过设计消除这两种成本。该模块围绕三个信息效率原则构建。首先，视觉雅可比在 InEKF 迭代循环前计算一次并在所有迭代中重用，利用光度梯度对收敛期间亚度状态变化的不敏感性。其次，每观测使用计数去相关因子防止被重复跟踪的点在连续帧间膨胀 。第三，多门控离群点滤波（视差、绝对残差、 和视角门控）确保只有几何和光度上可靠的观测贡献。与场景级质量因子 （第 VI-F 节）一起，这些形成跨空间、时间和计算域的多级信息过滤机制。

图5：信息高效直接光度VIO流水线——(a)传感器输入；(b)视觉地图准备；(c)循环前光度模型；(d)多级滤波；(e)VIO信息

A. 激光锚定的视觉地图点

在纯视觉系统中，3D 地图点从图像对应关系三角化，产生随距离增长的深度不确定性，在远处降低  的可靠性。我们通过直接重用激光扫描点作为视觉地图点来消除此不确定性：每个视觉点  是转换到世界坐标系的激光扫描点，3D 位置精确到激光噪声，协方差继承自(11)-(12)。激光体素地图在每个视觉点处提供表面法线 ，用于深度连续性和视角准入准则。

B. 点选择

激光锚定的地图点在  体素网格中管理；每个单元最多一个点进入滑动窗口，按梯度能量分数选择：

其中  为投影位置的中心差分图像梯度。这种确定性、富纹理的选择最大化每观测贡献的 Fisher 信息。

点在准入前经过两个几何滤波器。深度连续性：若其  邻域（半径 ）中任何像素携带的有效深度与点自身深度相差超过 ，则拒绝该点，防止遮挡边界附近的观测进入窗口。视角：仅当表面法线  与视线方向  之间的夹角低于  时点才被准入：

C. 每帧仿射亮度模型

时间亮度变化引入系统性光度偏差，若未补偿则会破坏直接残差并偏移 。我们将参考观测  与当前图像  之间的亮度传递建模为仿射函数：

其中  对每帧  独立估计。参数通过对每个观测的所有面片像素进行梯度加权最小二乘获得：

其中  为属于帧  的滑动窗口观测集合， 为稀疏面片模式中的像素数， 表示观测  面片的第  个像素强度， 为观测  在参考帧（创建时冻结）的平均梯度幅度。像素级累积考虑面片内强度方差，因此高纹理观测对增益估计  的影响比例上大于均匀强度观测。所得的  正规方程以闭式求解。为防止退化估计， 和  钳位到允许范围  和 。来自亮度变化极端的相机的观测  接受指数惩罚以减少其对状态更新的影响。仿射参数在传播状态处估计一次，作为 InEKF 迭代循环之前的视觉预计算的一部分。

D. 光度残差与循环前雅可比

在每个 InEKF 迭代重新评估完整光度雅可比是主要的 VIO 成本，但产生可忽略的精度收益，这促使本节开发的循环前雅可比冻结。每个观测使用以投影位置  为中心的稀疏 9 像素交叉模式：

半径为 （默认 ，），每个像素通过双线性插值采样，每像素残差平均形成标量 。对滑动窗口中 3D 点  的每个观测 ，仿射补偿后的光度残差为：

其中  为相机坐标系中的点， 为观测创建时冻结的参考帧位置。关于  的  雅可比遵循链式法则 。下面推导的目标是将扰动  用相机坐标系量表达，使图像梯度（原生在相机坐标系中）耦合到状态扰动时无需在每个迭代重新评估的残余机体框架旋转。关键工具是 SO(3) 伴随恒等式 ，它将斜对称算子移过旋转，并将机体框架斜对称  重写为相机框架向量 。在右不变扰动  下应用，得到：

其中第二行应用伴随恒等式将斜对称从机体框架推到相机框架。与图像到 3D 梯度 （ 为相机模型投影雅可比）组合，得到：

整个视觉信息形式  在 InEKF 循环前在传播状态  处组装一次，并在所有  次迭代中冻结。在本文使用的收敛阈值  下，500 px 焦距相机每迭代最多将投影像素位移 ，远低于双线性插值采样间隔。冻结  因此每观测消除  次冗余评估，精度损失可忽略。与跨帧固定雅可比以保持估计器一致性的首次估计雅可比技术不同，此冻结限于单个更新循环，纯由效率驱动，利用光度残差及其雅可比对 InEKF 收敛的亚度、亚毫米增量不敏感的特性。相比之下，激光残差在每个迭代重新评估，因此联合更新仍跟踪最新状态。

每观测光度噪声方差是深度条件的：

其中  为固定每像素光度噪声基底（所有实验设为 ）， 为图像到 3D 梯度链， 为相机坐标系中的点不确定性，通过相机外参从(12)传播。深度条件项确保位置不确定性较大的远场点对  的贡献比例较小。

E. 带信息去相关的滑动窗口

视觉模块维护最近  帧的滑动窗口， 作为典型值。每帧存储从激光锚定地图点选择的至多  个观测，通过均匀步长下采样保持空间覆盖。

滑动窗口是信息形式融合的结构要求而非启发式改进。单帧光度信息矩阵  严重病态：因为所有图像雅可比  共享单一视点，信息集中在图像平面方向，而沿光轴方向和弱投影的旋转方向接收接近零的信息。在 SAIF 线性钳位门控中， 的方向被  衰减，因此  的每个特征方向必须携带足够的联合信息才能以全强度贡献。如果  在  较弱的特征方向上贡献可忽略（正是视觉模态应稳定化的方向），门控将比例地衰减这些方向，激光与视觉之间的互补性仅在联合幅度升至  以上时才实现。在  帧不同相机基线上累积雅可比跨越更丰富的子空间，提高  在单帧观测无法达到的方向上的特征值。滑动窗口因此直接决定了 SAIF 操作的  的秩和方向覆盖。

重复帧抑制防止近乎相同的帧进入窗口：若新帧与前一帧的旋转变化低于  且平移变化低于 ，则拒绝新帧。当窗口超过  帧时，最旧的帧被驱逐，其观测被丢弃。与被驱逐帧 ID 关联的仿射参数被修剪以防止无界内存增长。

同一 3D 点在多个滑动窗口帧中的观测引入时间相关性，违反 InEKF 的测量独立性假设。为缓解此问题，每个观测维护一个使用计数器 ，每次贡献给  时递增。去相关因子：

确保点的第一次贡献以全尺度进入，而后续重用被逐步衰减。这由第 VII-E 节描述的收敛后协方差膨胀  补充，共同补偿去相关单独无法消除的残余相关性。

F. VIO 质量因子

在信息形式进入融合阶段前，场景级质量因子  门控视觉信息形式，防止退化或损坏的 VIO 贡献给状态更新。三个乘法分量捕获互补失效模式：

其中

惩罚大平均光度残差（灰度级单位）；阈值  和  分别设为 12 和 8 灰度级，值来自训练序列的经验光度残差分布； 随有效观测数达到最小支撑阈值  线性升至 1；以及