# SA-LIVO：一种具备子空间感知退化处理能力的高效激光雷达-惯性-视觉里程计
曹 Yinong$^{1}$、何 Xin$^{2,*}$、朱 Shouzheng$^{2}$、王 Senyuan$^{2}$、江 Changhui$^{2}$、张 Chenyang$^{2}$、何 Pingfeng$^{2}$、王 Tingwei$^{2}$、陈 Yuwei$^{2}$、刘 Shijie$^{2}$、李 Chunlai$^{1}$、黄 Genghua$^{1}$、王 Jianyu$^{1,*}$
摘要—紧耦合的激光雷达-惯性-视觉里程计（LIVO）将几何深度信息与视觉测量数据融合，但其外置传感器会独立失效：当扫描几何结构约束不足时激光雷达会失效，光照条件差或缺乏纹理时视觉传感器会失效。现有的应对方案（二元退化检测、协方差膨胀、场景级质量筛选）仅在模态层面发挥作用，因此单一的各向同性增益会将视觉残差引入激光雷达已有较强约束的方向，无法将其集中在约束薄弱的区域。我们提出了具备子空间感知能力的激光雷达-惯性-视觉里程计（SALIVO），其采用的子空间感知信息融合（SAIF）技术会对激光雷达与视觉信息的联合矩阵进行特征分解，通过单阈值线性限幅来对每个特征方向进行筛选：减弱振幅较低的方向的权重，同时让观测效果良好的方向以全强度传递；该系统还具备鲁棒的残差级筛选机制以及场景级质量因子，可剔除受损的测量数据。激光雷达与视觉残差共享同一个不变的扩展卡尔曼滤波（InEKF）循环及线性化点，因此光度雅可比矩阵只需计算一次即可在多次迭代中重复使用。在29组公开基准测试序列（HILTI'22、Newer College数据集、Oxford Spires）以及其他同时发生退化的场景中，SALIVO在精度上与最先进的基线方法相当，且在竞争系统发散的情况下仍能保持稳定性。在全部基线方法均能运行的HILTI'22子集上，该算法在笔记本电脑CPU上的每帧处理时间平均为12.3毫秒，在无需GPU的嵌入式ARM板上为26.8毫秒，峰值内存占用仅为前者的1/3.6至1/6.3。论文录用后我们将开源相关代码。
关键词—激光雷达-惯性-视觉里程计、子空间感知融合、退化现象、信息高效估计、直接光度VIO、InEKF。
## 一、引言
同时定位与地图构建（SLAM）是自主机器人与车辆在未知环境中作业的核心能力。在近年的SLAM技术中，激光雷达-惯性-视觉里程计（LIVO）因紧密融合了三种互补的传感模态而受到广泛关注：激光雷达可提供精确的深度信息与密集的几何约束；摄像头能输出丰富的视觉测量数据；惯性测量单元（IMU）则能提供高频率的运动先验[^1] [^2] [^3]。在几何结构复杂的环境中，这种紧密融合所能达到的精度远超单一模态的方案。但在实际应用中，这两种外置传感器中的其中一种或两种可能会出现性能退化。当观测到的几何结构无法在某个或多个位姿方向上提供足够约束时，就会出现激光雷达退化现象，例如点面匹配几乎共面时，某些平移或旋转自由度就无法被观测到。当成像条件破坏了可用的图像梯度时，就会发生摄像头退化：黑暗、运动模糊、曝光过度或无纹理表面都会导致视觉测量数据不可靠。由于这些失效模式由相互独立的环境因素（几何结构与光照条件）驱动，同时发生退化不仅有可能，而且十分常见。在此类情况下，现有的紧耦合系统往往会直接发散，而非平滑地降级为依赖最可靠的可用约束。
The natural response is mutual compensation through tight fusion, but existing approaches to sensor degradation act at the sensor-modality level rather than at the direction level of the joint information matrix. Binary degeneracy detection responds to an ill-conditioned scan-matching Hessian by rejecting or constraining the LiDAR update [^9], [^11], forfeiting the partial constraints a nearly degenerate scan still provides. Covariance inflation and scalar down-weighting attenuate a sensor's overall contribution isotropically, reducing its influence equally across well-constrained and poorly constrained pose directions. Scene-level quality gating decides whether a visual update is applied in a given frame [^1] but does not control where in the state space its residuals act. The actual failure structure is inherently per-direction: a single LiDAR scan may simultaneously over-constrain certain rotation directions while leaving a translational degree of freedom unobserved; visual gradients may constrain some pose axes strongly while contributing negligible information in others.
在这种无视方向的融合方式下，计算资源也被错配了。视觉-惯性里程计（VIO）流程包括跟踪、雅可比矩阵构建以及滤波器更新，无论场景几何形状如何，每帧都会产生固定的计算开销。然而，在约束良好的方向上其精度提升有限，而在退化场景下其精度增益还会被分散到六个位姿方向中。
我们提出了SA-LIVO（子空间感知型LIVO），这是一种可部署的实时激光雷达-惯性-视觉里程计系统。其核心贡献在于方向选择性的传感器融合，这一策略得到了专为匹配该融合策略而设计的激光雷达与视觉处理流程的支持。SA-LIVO的贡献如下：
1) 子空间感知信息融合（SAIF）。我们根据联合激光雷达-视觉信息矩阵中各个位姿方向的信息幅度与一个标量阈值进行比较，以此来决定是否采纳该方向的信息。在融合阶段，这种方法用单一的频谱可观测性阈值取代了针对特定传感器的退化阈值，最终得到一个可证明为正定的半定融合矩阵。
---
Fig. 1. Representative mapping results of SA-LIVO across diverse environments. (a1)-(c3): Oxford Spires [^4] outdoor and indoor scenes. (d)-(f): self-collected indoor and underground sequences. (g)-(h): FAST-LIVO2 dataset [^1].
该矩阵会连续退化成单传感器更新结果，且无需进行模式切换。
2) 统一的单循环联合InEKF更新。激光雷达与视觉残差在共同的线性化点处共享一个InEKF循环，从而消除了顺序处理各传感器数据时出现的线性化点不匹配问题，并且无论融合的模态数量多少，迭代次数都被限定在一个固定范围内。
3) 高效的预计算视觉信息形式。光度雅可比矩阵在状态传播后仅计算一次，便可反复用于所有InEKF迭代过程中，从而消除了迭代式光度滤波器中占主导地位的每迭代VIO计算开销。这种形式是在滑动窗口上累积而成的，能够覆盖比单一视角更丰富的子空间；同时，每个观测值的解相关因子可防止这种重复使用导致视觉信息矩阵膨胀。
4) 高效的多尺度平面映射。基于体素级别的充分统计数据，可实现恒定时间内的多尺度主成分分析（PCA）以及无量纲的平面度判定标准。这不仅将平面拟合成本与点数解耦，还免去了逐序列进行的固定特征值测试。首次InEKF迭代后会缓存平面对应关系，后续迭代仅需重新计算点到平面的距离；平面支撑范围会自适应地扩展直至满足平面度标准，如此一来，激光雷达信息矩阵便能与当前可观测的几何结构相匹配，而非依赖于积累的地图历史数据。
我们在HILTI'22、Newer College数据集（NCD）以及Oxford Spires基准测试中的29个序列上评估SA-LIVO [^4] [^5] [^6]，同时也在我们自行采集的数据集以及FAST-LIVO2数据集中专门设计的并发退化序列上进行了评估 [^1]；这些环境下的代表性建图结果如图1所示。SA-LIVO在各类基准测试中均展现出具有竞争力的精度，且在R3LIVE和SR-LIVO出现偏差的序列中仍能保持有限的漂移量。我们的代码及自行采集的数据集将在论文被接收后发布。
### II. 相关工作
### A. 激光雷达里程计与退化现象
基于迭代最近点（ICP）或点面匹配的直接激光雷达里程计 [^7]、[^8] 在结构化的环境中能够实现高精度。然而，当扫描几何形状在某个或多个方向上提供的约束不足时（例如在长走廊、大面积开阔地面或隧道中），扫描匹配的Hessian矩阵就会变为奇异或近似奇异，进而导致状态更新不受约束并产生快速漂移。已有多项研究通过匹配Hessian矩阵的特征谱来检测这种状况，并采用切换至
---
低自由度更新模式或拒绝该次扫描的方式加以应对 [^9]、[^11]。更先进的具备可定位性感知功能的配准算法 [^12] 会在ICP层面应用方向性约束过滤，但这种门控机制仍局限于激光雷达处理流程内部，并未传递到多传感器信息融合的形式中。Switch-SLAM [^13] 将这种切换机制提升到多传感器层面，一旦检测到退化现象便将估计任务从激光雷达切换到视觉里程计。FAST-LIVGO [^14] 同样会从LIVO的Hessian特征值中读取退化信息，但仅用它来选择异常值剔除模式并触发全球导航卫星系统（GNSS）辅助恢复，而非用于门控融合信息。虽然这些方法能有效防止发散，但二元切换属于非此即彼的模式，忽略了几乎退化的方向仍能提供的部分约束。像KISS-ICP [^16] 这类轻量级的现代激光雷达里程计表明，当搭配自适应运动模型时，简单的点对点匹配在约束良好的场景中也能具备竞争力，但它并未提供处理部分退化现象的明确机制。除了退化检测外，匹配过程中所用平面表示的可靠性也是另一大挑战。VoxelMap [^15] 引入了自适应体素分辨率和平面不确定性概念，但其平面接受准则采用的是绝对特征值阈值，该阈值会因体素大小、点密度和场景尺度的不同而有所差异，而我们的平面度比则是无量纲的，无需调整即可直接适用。
### B. 视觉惯性里程计
基于滤波的视觉惯性里程计可追溯至MSCKF [^17]，现代开源框架如OpenVINS [^18] 和ROVIO [^19] 均属于此类；后者在迭代扩展卡尔曼滤波内部应用了直接光度更新机制，且未与激光雷达耦合。基于关键帧的视觉惯性里程计系统如VINS-Mono [^20]、ORB-SLAM3 [^21] 和OKVIS [^22] 通过多帧三角测量或立体基线来恢复地图点深度，且深度不确定性会随距离增加而增大。基于流形上的IMU预积分技术 [^23] 是关键帧视觉惯性里程计的标准做法；在我们的系统中，这一角色由InEKF流形上的中点积分来承担。
当有激光雷达深度数据时，特征像素很少会同时—
由于激光雷达回波稀疏，因此需基于特征进行跟踪。
必须利用附近的几何点来插值深度信息。
这种依赖几何的不确定性使得直接方法[^24] [^25] [^26]能够规避这一问题。
具体做法是在预先锚定的三维点上采样光度残差。
这些点直接取自激光雷达扫描结果，因此天然适合用于紧密的激光雷达-视觉耦合。
不过这种方式需要精确的位姿先验作为支撑。
accurate pose priors.
即便某个激光雷达点被用作深度锚点，其深度值也并非绝对可靠：扫描线之间的间隙、玻璃等镜面反射表面以及水面都可能导致回波缺失、异常或存在偏差。单像素的光度残差也容易受到图像噪声和光照变化的影响。基于图像块的光度建模方法可缓解这些问题：通过对小邻域内的残差取平均值，既能消除孤立的锚点误差和像素级噪声，又能提升每个激光雷达锚定点所提供的信息量。DSO算法[^24]引入了首次估计雅可比矩阵（FEJ）技术[^27] [^28]以及每帧仿射亮度校正机制，但该算法仅在纯单目环境下运行，并未利用激光雷达深度锚点。我们的基于图像块的直接视觉惯性观测系统会将每个地图点与激光雷达测量值相关联，并在InEKF循环之前预先计算雅可比矩阵。
### C. 基于不变滤波的估计方法
不变扩展卡尔曼滤波（InEKF）[^29]将导航状态定义在矩阵李群上，并利用群仿射系统的对称性来获取与系统轨迹无关的传播雅可比矩阵。Brossard等人[^30]将其称为可观测性保持特性：全局偏航角和绝对位置的不可观测子空间在所有滤波迭代中均保持不变。相比之下，传统的左不变EKF在快速运动时会将信息投影到不可观测方向上，从而引发线性化偏差。Hartley等人[^31]在完整的足式机器人状态估计系统中验证了该公式的有效性，证明惯性测量单元、运动学及接触残差能够在单一右不变循环中实现一致融合。随后，InvLIO算法[^32]将InEKF扩展到激光雷达-惯性里程计领域，通过在李代数中表达点到平面残差来维持右不变结构。SuIn-LIO算法[^33]进一步将InEKF与基于曲面片的建图相结合，在公开基准测试中展现了稳定的状态估计能力和实时扫描配准性能。SA-LIVO则针对激光雷达-视觉融合场景采用了混合右不变公式：光度残差按照相同的右不变误差约定进行建模，并与激光雷达残差在单一迭代循环中融合，据我们所知，这是首批采用此类设计的LIVO系统。
### D. 激光雷达-视觉-惯性融合
松耦合的LIVO系统[^10]、[^34]利用一个传感器的输出初始化另一个传感器；这类系统体积小且易于实现，不过在复杂场景中，紧耦合方式通常能带来更高的精度。紧耦合系统会联合优化所有传感器的残差，但在融合架构上存在显著差异。LIC-Fusion[^35]率先在特征层面实现了MSCKF风格的激光雷达-相机-惯性测量单元融合，而后续的基于滤波的流程（$R^{2}$LIVE [^36]、FAST-LIVO [^37]）则转向了基于稀疏激光雷达锚定点的直接光度更新。FAST-LIVO2[^1]通过在迭代EKF循环中依次处理激光雷达和视觉残差来实现高精度，不过两种模态是在不同的状态估计下线性化的，且这种顺序结构会导致总迭代次数比联合循环更多。R3LIVE[^2]通过像素级光度模型构建密集彩色地图，虽然能提供丰富的场景表征，但每帧计算量更大。LVI-SAM[^3]结合了激光雷达和视觉因子图以实现灵活的多会话建图，其性能取决于两种模态中可靠的特征提取能力。基于平滑的系统（Kimera [^38]、VILENS [^39]）在图后端加入了语义或运动学模态来丰富状态信息，体现了紧耦合范式的通用性。不过在这些设计中，激光雷达和视觉信息的权重在所有姿态方向上都是均匀分配的：现有研究尚未分析联合信息矩阵的方向依赖性谱结构，也未针对各传感器不可靠的特定方向调整其贡献权重。SA-LIVO填补了这一空白，它将联合特征基线性限幅软门控、场景级的VIO质量因子以及单次观测内的去相关操作整合到统一的信息形式中。
---
Fig. 2. System overview of SA-LIVO. IMU measurements drive state propagation (Section IV-B). The blue LiDAR-inertial odometry (LIO) path matches downsampled LiDAR points to planes in the voxel map and the orange VIO path tracks sparse photometric patches anchored on those points (Section V, VI); each yields an information form, $(\mathbf{A}_L, \mathbf{b}_L)$ and $(\mathbf{A}_V, \mathbf{b}_V$. The red path eigendecomposes the joint information $\mathbf{\Lambda}_L + q\mathbf{\Lambda}_V$, soft-gates it at $\sigma_{min}$, and drives the iterated InEKF update together with the prior $\mathbf{P}^{-1}$ (Section VII, VII-E); the green path feeds the corrected pose back into the voxel map. Dashed arrows are cross-module data flow, solid arrows intra-module steps.
现有研究并未分析联合信息矩阵的方向依赖性谱结构，也未针对各传感器不可靠的特定方向调整其贡献权重。SA-LIVO填补了这一空白，它将联合特征基线性限幅软门控、场景级的VIO质量因子以及单次观测内的去相关操作整合到统一的信息形式中。
### III. 系统概述
TABLE I Notation Summary
| Symbol | Definition |
| $x, \tilde{x}, \tilde{x}^{t}$ | State; propagated; t-th iterate |
| $\mathbf{P}, \delta x$ | Covariance; error state |
| $\mathbf{R}, \mathbf{p}, \mathbf{v}$ | Body-to-world rotation, position, velocity |
| g | Online-estimated gravity vector |
| $\mathbf{b}_{g}, \mathbf{b}_{a}$ | Gyro / accel bias |
| $\oplus, \square$ | Manifold $\oplus / \square$ |
| $\Delta_{L}, \mathbf{b}_{L}$ | LiDAR info. matrix / vector |
| $\Delta_{V}, \mathbf{b}_{V}$ | Visual info. matrix / vector |
| q | VIO quality factor $\in [0,1]$ |
| $\Delta_{f}, \mathbf{b}_{f}$ | Fused info. matrix / vector |
| $\mathbf{U}, \lambda_{k}$ | Eigenvectors / eigenvalues of $\mathbf{\Lambda}_{L} + q\mathbf{\Lambda}_{V}$ |
| $\mathbf{K}$ | Total posterior information $\mathbf{\Lambda}_{f} + \mathbf{P}^{-1}$ |
| $\gamma(k)$ | Gate weight min( $\sqrt{\lambda_{k}}/\sigma_{\min}, 1$) \in [^0] [^1] |
| $\sigma_{\min}$ | Information-amplitude threshold for $\gamma(k)=1$ |
| $\mathbf{p}_{b}^{b}, \mathbf{p}_{w}^{w}$ | LiDAR point in body / world frame |
| $\Sigma_{b}, \Sigma_{w}$ | Point cov. in body / world frame |
| n | Plane normal |
| $r_{i}, \sigma_{i}^{2}, h_{i}$ | LiDAR residual; noise var.; Jacobian |
| $r_{j}, r_{\text{rms},j}$ | Mean and RMS patch photometric residual |
| $J_{j}$ | Photometric Jacobian (frozen pre-loop) |
如图2所示，SA-LIVO包含激光雷达-惯性测量单元和视觉两条并行信息流水线，它们共同输入到联合InEKF更新模块中；表I汇总了全文使用的符号说明。该系统可处理激光雷达（10 Hz；也支持其他频率）、相机（10 Hz）和惯性测量单元（200 Hz）的数据流。通过惯性测量单元反向传播，异步采样的激光雷达点会被重新组合成与相机频率同步的扫描数据，从而确保激光雷达数据与图像数据的时间对齐。
在激光雷达分支（第V节）中，降采样后的激光雷达点会与体素图上拟合的平面进行匹配，这些平面的支撑域会自适应扩展。每次匹配都会产生带完全传播方差的点对平面残差$r_i$ $\sigma_i^2$。经过$\chi^2$门控处理后，剩余的匹配结果会累积成激光雷达信息矩阵$\boldsymbol{\Lambda}_L \in \mathbb{R}^{6 \times 6}$和向量$\mathbf{b}_L \in \mathbb{R}^6$。
在视觉分支（第VI节）中，激光雷达锚定的3D地图点会被投影到当前图像中。先通过逐帧仿射亮度模型补偿曝光差异，再针对滑动窗口中的参考观测值计算光度残差。雅可比矩阵在传播状态下单次计算后便可在所有迭代中复用（第VI-D节对此进行了论证）。经过单次观测内的信息去相关和多门控异常值剔除后，跟踪到的点会累积为$\mathbf{\Lambda}_V \in \mathbb{R}^{6 \times 6}$和$\mathbf{b}_V \in \mathbb{R}^6$。在融合前，场景级的VIO质量因子q会对视觉信息进行门控处理。
SAIF（第七节）通过对联合信息矩阵$\boldsymbol{\Lambda}_L + q \boldsymbol{\Lambda}_V$进行特征分解，来确定整体的可观测性结构。随后通过$\gamma(k) = \min(\sqrt{\lambda_k} / \sigma_{\min}, 1)$对每个特征方向进行软门控处理，从而得到融合后的形式$(\boldsymbol{\Lambda}_f, \mathbf{b}_f)$。InEKF会求解由此产生的线性系统，以得到状态修正量。收敛之后，体素地图便会用优化后的位姿进行更新。
### 四、状态表示与IMU传播
我们从不变扩展卡尔曼滤波器（InEKF）[^29]、[^30]中借鉴了一个要素：其右不变姿态误差。这种误差形式能实现姿态的自同构变换，从而消除左不变EKF中姿态误差所产生的一步旋转效应。位置和速度误差仍保持欧几里得特性，因此状态流形为$\text{SO}(3) \times \mathbb{R}^{15}$，而非矩阵群。
---
SE₂(3)并非如此，我们也会明确说明这种混合形式并未继承的特性。群仿射一致性保证——即不可观测的全局偏航角和绝对位置子空间在迭代过程中保持不变[^29]、[^30]——这一性质仅属于完全由李群构成且具有群仿射动力学特性的状态，而SO(3)×R¹⁵并不具备该特性。这种取舍是为了实现融合而非传播：欧几里得的δp和δv保留了激光雷达和视觉信息的原有形式，基于它们构建的SAIF特征分解（第七节）也处于同一个线性误差空间中；而SE₂(3)会对速度误差进行非线性重新参数化（$\eta_v = \mathbf{v} - \eta_R \hat{\mathbf{v}}$）。因此我们声称，相较于左不变EKF，本方法能降低姿态线性化偏差，但并非达到完全不变滤波的一致性。我们的关键改进在于统一的联合更新机制：激光雷达和视觉信息形式在单个线性化点处融合，并在一个迭代循环中求解，从而消除了逐模态更新时出现的线性化点不匹配问题。
### A. 状态与流形
我们将IMU坐标系设为机体坐标系，世界坐标系设为全局参考系，这与[^8]中的设定一致。状态流形为$\mathcal{M} \triangleq \mathrm{SO}(3) \times \mathbb{R}^{15}$，包含$\dim(\mathcal{M}) = 18$以及状态向量：
$$ \mathbf{x}\triangleq\left[\mathbf{R}^{\top},\mathbf{p}^{\top},\mathbf{v}^{\top},\mathbf{b}_{g}^{\top},\mathbf{b}_{a}^{\top},\mathbf{g}^{\top}\right]^{\top}\in\mathcal{M}, $$
其中$\mathbf{R} \in \text{SO}(3)$表示IMU到世界的旋转，$\mathbf{p}, \mathbf{v} \in \mathbb{R}^{3}$为世界坐标系下的位置和速度，$\mathbf{b}_g, \mathbf{b}_a$为陀螺仪和加速度计偏差，$\mathbf{g}$为在线估计的重力矢量。误差状态为$\delta x = x \boxminus \hat{x} \in \mathbb{R}^{18}$，其中的$\boxplus/\boxminus$运算符采用右不变（InEKF）约定[^29]：$\delta \phi = \text{Log}(\mathbf{R} \hat{\mathbf{x}}_R^\top)$，状态更新为$\mathbf{R} \leftarrow \text{Exp}(\delta \phi)$。如上文所述，速度和位置误差仍保持欧几里得特性（$\delta \mathbf{v} = \mathbf{v} - \hat{\mathbf{v}}$、$\delta \mathbf{p} = \mathbf{p} - \hat{\mathbf{p}}$）。这确定了区分该混合形式的两个传播模块。旋转自同构变换变为恒等变换（$\mathbf{F}_x[\delta \phi, \delta \phi] = \mathbf{I}$），从而消除了FAST-LIO2[^8]中左不变约定所导致的姿态误差一步旋转效应，同时也简化了协方差传播（3）。速度-旋转耦合模块变为$\mathbf{F}_x[\delta \mathbf{v}, \delta \phi] = -\lfloor \mathbf{R} \mathbf{a} \rfloor_x \Delta t$（以世界为基准的比力斜量），而非左不变公式中的机体坐标系下的$-\mathbf{R} \lfloor \bar{\mathbf{a}} \rfloor_x \Delta t$；两者在耦合方向上仅存在旋转差异，不会引入额外近似误差。这两种形式都不等同于完整的$\mathbf{F}_x[\delta \mathbf{v}, \delta \phi] = \lfloor \mathbf{g} \rfloor_x \Delta t$ Barrau-Bonnabel构造中所得到的与轨迹无关的$\text{SE}_2(3)$，原因如上所述：该模块源于我们刻意舍弃的非线性速度误差重新参数化过程。
### B. IMU传播
在连续的传感器帧之间，高频率的IMU测量数据是提供运动连续性的唯一来源；将它们向前传播可使状态估计推进到激光雷达扫描终点，并通过反向积分来消除点云畸变。第i个IMU测量时刻的离散状态转移模型为：
$$ \mathbf{x}_{i+1}=\mathbf{x}_{i}\boxplus\Delta t\mathbf{f}(\mathbf{x}_{i},\mathbf{u}_{i},\mathbf{w}_{i}), $$
其中$\mathbf{u}_i = [\boldsymbol{\omega}_m^\top, \mathbf{a}_m^\top]^\top$为原始IMU测量值，$\mathbf{w}_i \sim \mathcal{N}(\mathbf{0}, \mathbf{Q})$为过程噪声，运动函数f的定义参见[^8]。状态协方差的传播公式为：
$$ \hat{\mathbf{P}}_{k}=\mathbf{F}_{x}\hat{\mathbf{P}}_{k-1}\mathbf{F}_{x}^{\top}+\mathbf{F}_{w}\mathbf{Q}\mathbf{F}_{w}^{\top}, $$
其中$\mathbf{F}_x \in \mathbb{R}^{18 \times 18}$和$\mathbf{F}_w \in \mathbb{R}^{18 \times 12}$分别为公式(2)在当前估计值下的状态和噪声雅可比矩阵。
### V. 尺度不变的抗漂移LIO系统
激光雷达信息形式($\mathbf{\Lambda}_L, \mathbf{b}_L$)是通过将下采样后的机体坐标系点与点云图中的平面进行匹配，并累加加权点到平面残差来构建的。本节将推导残差和右不变雅可比矩阵，描述自适应地图结构与平面拟合流程，介绍与信息一致的噪声模型，并详细说明多尺度对应搜索策略。
### A. 点到平面残差
每个下采样后的机体坐标系点$\mathbf{p}^{b}$通过激光雷达-IMU外参$(\mathbf{R}_{LI}, \mathbf{p}_{LI})$和当前状态$(\mathbf{R}, \mathbf{p})$变换到世界坐标系下：
$$ \mathbf{p}^{I}=\mathbf{R}_{LI}\mathbf{p}^{b}+\mathbf{p}_{LI},\quad\mathbf{p}^{w}=\mathbf{R}\mathbf{p}^{I}+\mathbf{p}. $$
给定点云图中匹配到的平面，其单位法向量为$\mathbf{n}_{i}$，质心为$\mathbf{c}_{i}$，则带符号的点到平面距离为：
$$ \boldsymbol{r}_{i}=\mathbf{n}_{i}^{\top}\mathbf{p}_{i}^{w}+d_{i}, $$
其中$d_i = -\mathbf{n}_i^\top \mathbf{c}_i$。按照右不变误差约定$[\delta\phi; \delta\mathbf{p}]$，关于$\mathbf{R} = \exp(\delta\phi) \hat{\mathbf{x}}_R$的雅可比矩阵为：
$$ \mathbf{h}_{i}=\left[\begin{matrix}{\lfloor\mathbf{R p}_{i}^{I}\rfloor\times\mathbf{n}_{i}}\\ {\mathbf{n}_{i}}\\ \end{matrix}\right]\in\mathbb{R}^{6\times1}, $$
其推导过程如下：将$\mathbf{R} \leftarrow \text{Exp}(\delta \phi)\mathbf{R}$扰动至一阶项可得$\mathbf{p}^w \approx \mathbf{R}\mathbf{p}^I + \lfloor \mathbf{R}\mathbf{p}^I \rfloor_{\times}^\top \delta \phi + \mathbf{p}$，因此$\partial r_i / \partial \delta \phi = \mathbf{n}_i^\top \lfloor \mathbf{R}\mathbf{p}_i^I \rfloor_{\times}^\top = (\lfloor \mathbf{R}\mathbf{p}_i^I \rfloor_{\times} \mathbf{n}_i)^\top$，其中$\lfloor \mathbf{R}\mathbf{p}_i^I \rfloor_{\times} = \mathbf{R} \lfloor \mathbf{p}_i^I \rfloor_{\times} \mathbf{R}^\top$由SO(3)伴随恒等式给出。因此旋转部分取决于世界坐标系下的位置$\mathbf{R}\mathbf{p}_i^I$而非机体坐标系下的点（详见第V-E节）。
### B. 体素地图结构
该地图是由固定边长$v_{s}$的立方体体素构成的扁平哈希网格，每个被占据的哈希条目对应一个单元格，无需八叉树细分。所有点都以相同分辨率进入同一网格，因此插入操作仅需一次哈希查找，地图也不保存每个单元格的细分状态。
固定的单元格尺寸本身无法适配完整的传感器量程。在距离r处，旋转式激光雷达的相邻光束间距约为$r\Delta\theta$，其中$\Delta\theta$为角度分辨率，因此单位面积内的点密度会随$r^{-2}$而降低，远距离回波所在单元格中的点数过少，无法可靠拟合平面。我们并未通过随距离增大单元格尺寸来解决这一问题（这种方式会在几何信息未知时提前确定支撑范围），而是保持网格均匀，在查询时动态调整支撑范围：平面拟合会向外扩展邻域，直到累积的回波数据足以确认平面存在（详见第V-G节）。这样一来，密度自适应是由实际存在的回波数据驱动的，而非依赖距离启发式规则，且主成分分析（PCA）在单一分辨率参数下就能在全量程内保持良好的条件数。平面性判定准则、增量PCA以及不确定性模型都共享相同的每个体素的充分统计量（详见第V-D至V-E节）。
---
adaptation is thereby driven by the returns actually present rather than by a range heuristic, and principal component analysis (PCA) stays well-conditioned across the full range with a single resolution parameter. The planarity criterion, incremental PCA, and uncertainty model all share the same per-voxel sufficient statistics (Sections V-D-V-E).
Fig. 3. Scale-invariant planarity acceptance. As returns accumulate (left to right) the normal n stabilizes and the planarity ratio $\kappa$ falls; the fit is accepted once $\kappa < \kappa_{th}$.
### C. 尺度不变的平面性判定准则
当体素内的点分布具有足够的各向异性时，该体素会被视为平面。常规的测试$\lambda_{\mathrm{min}} < \epsilon$会将最小的主成分分析特征值与一个固定的$m^{2}$阈值$\epsilon$进行比较。由于$\lambda_{\mathrm{min}}$会随体素体积（大致为$\propto v_{s}^{2}$）以及局部点密度而变化，因此相同的$\epsilon$阈值无法在不同传感器配置或场景尺度下通用。
我们转而采用无量纲比值：
$$ \kappa=\frac{\lambda_{\mathrm{m i n}}}{\mathrm{t r}\left(\mathbf{\Sigma}_{\mathrm{p t s}}\right)}\in\left[0,\frac{1}{3}\right], $$
where $\Sigma_{\mathrm{pts}}$ is the N-point sample covariance in the voxel. $\kappa=0$ is a perfect plane; $\kappa=1/3$ is isotropic. Being a ratio of eigenvalues, $\kappa$ is scale-invariant: the same threshold $\kappa_{\mathrm{th}}=0.05$ applies regardless of voxel size or range. A fit is accepted as planar when
$$ \kappa<\kappa_{\mathrm{t h}}, $$
这一单一测试即可在所有生成残差的位置做出接受判定（见图3）；它并不与绝对的特征值阈值绑定，而这正是消除固定$\epsilon$阈值所要求的序列依赖性之关键。
接受判定与冻结机制是相互独立的。一旦体素达到每个体素的点数上限$N_{cap}$，便停止继续积累点数据，该上限既能限制内存占用，也能确定公式(12)中的$N_{eff}$。冻结机制并非由平面性触发：该上限仅用于限定一个单元格所汇总观测数据的数量，而$\kappa$则用于判定当前几何形状是否支持平面，二者不应混为一谈。
### D. 基于充分统计量的O(1)多尺度主成分分析
单个体素内的点数可能不足以进行可靠的平面估计，尤其是在回波稀疏的远距离场景下。自然的解决办法是聚合相邻体素以得到更粗尺度的拟合结果。但传统做法需要收集邻域内的所有点并运行主成分分析：这是一种计算量为$O(N_{pts})$的操作，当邻域内累积的点数达到数千个时，这种计算量便难以承受。
我们发现主成分分析仅需三个聚合量，它们被称为充分统计量，因为协方差公式所需的所有信息都包含在其中，无需保留原始点数据。样本协方差可通过以下公式精确还原：
$$ \mathbf{s}_{1}=\textstyle\sum_{i=1}^{N}\mathbf{x}_{i},\quad\mathbf{S}_{2}=\textstyle\sum_{i=1}^{N}\mathbf{x}_{i}\mathbf{x}_{i}^{\top},\quad n=N, $$
其中$\{\mathbf{x}_i\}_{i=1}^N$是体素内累积的点，因此有$\mathbf{\Sigma}_{\mathrm{pts}} = \mathbf{S}_2/n - (\mathbf{s}_1/n)(\mathbf{s}_1/n)^\top$。每个体素在每次插入新点时都会逐步更新$(\mathbf{s}_1, \mathbf{S}_2, n)$，无论其当前是否满足平面性测试。关键在于，即使未能通过$\kappa < \kappa_{\mathrm{th}}$测试的体素，也仍保留着可用于邻域拟合的有效充分统计量。聚合半宽为$(2\ell+1)^3$的$\ell$邻域时，仅需进行$(2\ell+1)^3$次可哈希查找和向量加法，且每次操作仅针对单个体素，与每个体素包含的点数无关。最后通过$3\times3$对称特征求解器即可在$O(1)$时间内完成主成分分析。
### E. 噪声与不确定性模型
To form an information-consistent $\Lambda_L$, each residual must be scaled by its true expected noise variance. Following the two-component beam-aligned decomposition introduced in [^15], each LiDAR point $\mathbf{p}^b$ in the body frame is treated as subject to two independent noise sources: ranging noise $\sigma_r$ along the beam direction $\hat{\mathbf{d}} = \mathbf{p}^b / \|\mathbf{p}^b\|$, caused by time-of-flight quantization, and angular noise $\sigma_\theta$ perpendicular to the beam, absorbing encoder noise and beam-divergence spread into a single isotropic transverse term. These yield the body-frame point covariance:
$$ \mathbf{\Sigma}_{b}=\sigma_{r}^{2}\hat{\mathbf{d}}\hat{\mathbf{d}}^{\top}+r^{2}\sin^{2}\sigma_{\theta}\left(\mathbf{I}-\hat{\mathbf{d}}\hat{\mathbf{d}}^{\top}\right), $$
其中 $r = \|\mathbf{p}^b\|$。横向项的缩放比例为 $r^2$，因此远场点所携带的垂直方向不确定性明显大于近场点。将其转换到世界坐标系下并纳入当前状态的不确定性因素：
$$ \begin{array}{r}{\mathbf{\Sigma}_{w}=\underbrace{\mathbf{R}\mathbf{R}_{L I}\mathbf{\Sigma}_{b}\mathbf{R}_{L I}^{\top}\mathbf{R}^{\top}}_{\mathbf{\Sigma}_{w}^{s}}+\lfloor\mathbf{R}\mathbf{p}^{I}\rfloor_{\times}\mathbf{P}_{R}\lfloor\mathbf{R}\mathbf{p}^{I}\rfloor_{\times}^{\top}+\mathbf{P}_{t},}\end{array} $$
其中 $\mathbf{P}_R = \hat{\mathbf{P}}_{0:3,0:3}$、$\mathbf{P}_t = \hat{\mathbf{P}}_{3:6,3:6}$，而 $\mathbf{R}\mathbf{p}^I = \mathbf{p}^w - \mathbf{p}$ 是以世界为原点的IMU坐标系向量，该向量由右不变雅可比矩阵 $\partial \mathbf{p}^w / \partial \delta \phi = -\lfloor \mathbf{R} \mathbf{p}^I \rfloor_x$ 所要求（若此处使用 $\lfloor \mathbf{p}^I \rfloor_x$，在剧烈运动情况下会将旋转不确定性错误地投影到平面法线上）。当状态不确定性较大时，这种耦合机制会自动削弱信息贡献，从而避免产生过度自信的更新结果。
由少量、含噪或分布不均的点云拟合出的平面，其可靠性远低于由数百个分布均匀的点生成的平面；若将所有平面视为同等可信，便会导致信息贡献被过度高估，进而造成状态更新偏差。因此，每个被采纳的平面都对应一个关于 $6 \times 6$ 的参数协方差 $(\mathbf{n}, \mathbf{c})$：
$$ \mathbf{\Sigma _{plane}}=\frac{\lambda_{min}}{N_{eff}}\mathbf{I}_{6},\qquad N_{eff}=\min(N,\ N_{cap}), $$
where $\lambda_{\min}$ is the residual thickness of the accepted fit and N the number of returns aggregated over the support. The two factors carry the two failure modes directly: a thick
---
fit is uncertain however many points support it, and a fit from few points is uncertain however thin it is. Capping the count at $N_{cap}$ is what keeps the model honest under the adaptive support of Section V-G: without it, aggregating successively larger shells would drive $\Sigma_{plane}$ toward zero purely by accumulating points, manufacturing confidence the geometry does not support.
### F. 激光雷达信息矩阵组装
综合考虑平面不确定性与点云噪声后，总残余方差为：
$$ \begin{array}{r}{\sigma_{i}^{2}=\mathbf{J}_{n q,i}\mathbf{\Sigma}_{\mathrm{p l a n e}}\mathbf{J}_{n q,i}^{\top}+\mathbf{n}_{i}^{\top}\mathbf{\Sigma}_{w,i}^{s}\mathbf{n}_{i}+\varepsilon_{0},}\end{array} $$
其中 $\mathbf{J}_{nq,i} = [\mathbf{p}_i^w - \mathbf{c}_i; -\mathbf{n}_i]^\top$ 和 $\varepsilon_0 = 10^{-3}$。此处仅涉及传感器噪声项 $\boldsymbol{\Sigma}_w^s$，因为 $\boldsymbol{\Sigma}_w$ 中的状态不确定性部分已由联合求解过程中的先验 $\hat{\mathbf{P}}^{-1}$ 予以体现（见第VII-E节）；在没有此类先验的情况下，比如在平面关联和地图更新环节，则会使用完整的 $\boldsymbol{\Sigma}_w$。经过 $\chi^2$ 创新门控处理（$r_i^2 / \sigma_i^2 > \tau_L$、$\tau_L \approx 25$，均根据数据集调试而成，用于剔除异常值）后，激光雷达信息形式如下：
$$ \begin{array}{r}{\mathbf{\Lambda}_{L}=\sum_{i\in\mathcal{I}_{L}}\sigma_{i}^{-2}\mathbf{h}_{i}\mathbf{h}_{i}^{\top},}\end{array} $$
$$ \mathbf{b}_{L}=-\sum_{i\in\mathcal{I}_{L}}\sigma_{i}^{-2}\mathbf{h}_{i}r_{i}, $$
其中 $\mathcal{I}_{L}$ 是通过门控筛选后的点索引集。(15)式中的负号遵循梯度下降惯例（即负加权残差梯度），这一设定与视觉信息向量(30)保持一致。
(6)式中的体素搜索、法线查找以及雅可比矩阵 $\mathbf{h}_i$ 仅依赖于匹配到的平面 $(\mathbf{n}_i, \mathbf{c}_i)$，该平面在InEKF迭代过程中不会改变。因此这些对应关系会在首次迭代后被缓存起来（$(t = 0)$）；后续迭代仅需重新计算标量形式的点到平面距离 $r_i = \mathbf{n}_i^\top(\mathbf{R}\mathbf{p}_i^T + \mathbf{p}) + d_i$，从而将每次迭代的激光雷达计算成本从 $O(N \times V_{\text{search}})$ 降至 $O(N)$。
### G. 自适应支撑平面关联
每个下采样的激光雷达点都必须与地图上的平面相匹配，这就需要确定用于拟合该平面的支撑区域大小。若支撑区域过小，在稀疏的远场回波中拟合结果会欠定；若支撑区域过大，则会平均掉表面不连续性带来的影响。这两种失效模式决定了地图误差有多少会渗透到来自 $\Lambda_{L}$ 的激光雷达信息中。
我们并非固定支撑集的大小，而是采用自适应方式逐步扩大它，直到找到第一个经确认是平面的尺度为止。从包含查询点的体素开始，每次添加一个同心切比雪夫壳层，其中壳层 $\ell$ 指的是与中心相距切比雪夫距离 $\ell$ 的体素集合。当累计返回计数达到 $N_{\text{pca}} = 25$ 时便进行主成分分析，一旦满足 $\kappa < \kappa_{\text{th}}$ 条件就接受该支撑集；否则会继续添加下一个壳层，最多加到 $\ell_{\text{max}} = \text{clamp}(\lceil r_s/v_s \rceil, 1, 6)$（$r_s = 2\,\text{m}$）。由于每个壳层都是通过第V-D节中所述的每个体素的充分统计量来贡献信息的，因此内部单元格无需再次访问，特征分解也只在接受的尺度上执行。
只有当查询点在横向方向上位于支撑集质心到拟合平面半径的三倍范围内时，才会使用已接受的平面。这一规则能排除那些虽投影到平面无限延伸区域但却落在实际观测到的表面斑块之外的点。
接受规则刻意优先考虑质量而非时间顺序。一种自然的选择是偏好那些积累了大量返回数据的平面，理由是它们已经平均掉了短期的姿态漂移。然而，被大量观测到的体素仅能证明它采样充分，并不能保证其正确性：在初始姿态偏差下拟合的平面、受动态物体干扰的平面，或在闭环形成前构建的平面都属于稳定的误差源，若按观测次数排序，会让滤波器偏向这种累积的地图偏差。无量纲比值 $\kappa$ 则能直接验证拟合本身所包含的几何证据，无论该证据是在何种尺度下首次变得充足，这能让 $\Lambda_{L}$ 始终锚定在当前可观测的几何结构上，而非地图的历史信息上。
### VI. 基于激光雷达的直摄影测量视觉惯性导航
直摄影测量视觉惯性导航避免了特征提取和深度三角测量带来的不确定性。但若将其直接应用于迭代滤波器中，会在每次迭代时重新计算计算量庞大的雅可比矩阵，还会在滑动窗口帧之间积累时间相关性。本节将展示如何通过三项信息效率原则从设计层面消除这两类开销：在惯性扩展卡尔曼滤波循环之前一次性计算视觉雅可比矩阵并在迭代中重复使用（见第VI-D节）；引入每个观测点的使用次数去相关因子，防止重复跟踪的点导致 $\Lambda_{V}$ 膨胀（见第VI-E节）；以及多门限异常值过滤机制，结合场景级质量因子 q（见第VI-F节），仅采纳几何和光度上可靠的观测数据。
### A. 基于激光雷达的视觉地图点
在纯视觉系统中，3D地图点是通过图像对应关系三角测量得到的，其深度不确定性会随距离增加而增大，进而降低 $\Lambda_V$ 在远距离处的可靠性。我们通过直接将激光雷达扫描点用作视觉地图点来消除这种不确定性：每个视觉点 $\mathbf{p}^w$ 都是转换到世界坐标系下的激光雷达扫描点，其3D位置的精度达到激光雷达噪声水平，协方差则源自公式(10)–(11)（见图4）。激光雷达体素地图为每个视觉点提供了表面法线 $\mathbf{n}_{\text{vis}} \leftarrow \mathbf{n}_{\text{voxel}}$，用于下文所述的深度连续性和视角准入条件判断。在本节中，$\mathbf{R}_{ci}, \mathbf{p}_{ci}$ 表示通过离线标定得到的惯性测量单元到摄像机的外部参数，因此 $\mathbf{p}^c = \mathbf{R}_{cw} \mathbf{p}^w + \mathbf{p}_{cw}$ 与 $\mathbf{R}_{cw} = \mathbf{R}_{ci} \mathbf{R}^\top$、$\mathbf{p}_{cw} = -\mathbf{R}_{cw} \mathbf{p} + \mathbf{p}_{ci}$ 相关联，而摄像机中心在世界坐标系中的位置则为 $\mathbf{p}_{cam} = \mathbf{p} - \mathbf{R} \mathbf{R}_{ci}^\top \mathbf{p}_{ci}$。
### B. 点选择
基于激光雷达的地图点被存储在 0.5 米的体素网格中。随后，根据这些点在均匀图像网格中的投影对其进行分类，每个图像单元格中最多只有一个点能进入滑动窗口，选择依据是梯度能量得分。
$$ s_{\mathrm{g r a d}}=I_{x}^{2}+I_{y}^{2}, $$
---
Fig. 4. LiDAR-guided sparse photometric sampling. A LiDAR map point $\mathbf{p}_j^w$, carried by the voxel map with its normal $\mathbf{n}$, becomes the camera-frame vector $\mathbf{p}_j^c$ and projects to $\pi(\mathbf{p}_j^c)$; no triangulation is involved, so its depth is as accurate as the LiDAR measurement behind it. The 9-point pattern (20) of radius $\rho$ is sampled around this sub-pixel projection, so every tap is a bilinear blend of four pixels (highlighted cell, magnified); the nine differences average into the scalar residual $r_j$ of (21).
其中 $I_{x}, I_{y}$ 表示投影位置处的中心差分图像梯度。
在允许某个点进入窗口之前，会应用两种几何滤波器。
Depth continuity: a point is rejected if any pixel in its $9 \times 9$ neighborhood (radius $\rho_d = 4$ px) carries a valid depth that differs by more than $\delta_d = 0.5$ m from the point's own depth, preventing observations near occlusion boundaries from entering the window. View-angle: a point is admitted only when the angle between the surface normal n and the viewing direction $\hat{\mathbf{d}}_v = (\mathbf{p}_{\text{cam}} - \mathbf{p}^w)/\|\mathbf{p}_{\text{cam}} - \mathbf{p}^w\|$ stays below $\theta_{\text{max}} = 80^\circ$:
$$ \left|\mathbf{n}\cdot\hat{\mathbf{d}}_{v}\right|>\cos\theta_{\max}. $$
### C. 逐帧仿射亮度模型
时间上的亮度变化会引发系统性的光度偏差，若不进行补偿，就会破坏直接残差并导致 $\Lambda_{V}$ 发生偏移。我们将参考观测 $I_{ref}$ 与当前图像 $I_{cur}$ 之间的亮度转换建模为仿射函数：
$$ I_{\mathrm{c u r}}\approx\alpha_{j}I_{\mathrm{r e f}}+\beta_{j}, $$
其中 $(\alpha_{j},\beta_{j})$ 是针对每一帧 j 独立估计的参数。这些参数是通过对所有观测值的补丁像素进行梯度加权最小二乘拟合得到的：
$$ \left(\alpha_{j},\beta_{j}\right)=\arg\min_{\alpha,\beta}\sum_{i\in\mathcal{O}_{i}}\|\nabla I_{\mathrm{r e f}}^{(i)}\|\left(\bar{I}_{\mathrm{c u r}}^{(i)}-\alpha\bar{I}_{\mathrm{r e f}}^{(i)}-\beta\right)^{2}, $$
这里 $\mathcal{O}_j$ 是属于第 j 帧的滑动窗口观测值集合，$\overline{\ell}^{(i)}$ 是观测值 i 对应稀疏补丁上的平均强度，而 $\|\nabla I_{\mathrm{ref}}^{(i)}\|$ 是在参考帧（创建时即固定）下对观测值 i 计算得出的平均梯度幅值。每个观测值只需一个补丁均值即可，因为每帧的仿射模型只有两个自由度，且梯度权重本身就会使拟合结果集中于纹理丰富的观测值上，正是这些观测值的均值真正区分了 $\alpha$ 和 $\beta$。由此得到的 $2 \times 2$ 正规方程可通过闭式解法求解。为防止出现退化的估计结果，$\alpha$ 和 $\beta$ 会被分别限制在允许范围 $[\alpha_{\mathrm{min}}, \alpha_{\mathrm{max}}]$ 和 $[\beta_{\mathrm{min}}, \beta_{\mathrm{max}}]$ 内。来自亮度变化极端（$|\log \alpha| > \delta_\alpha$）的相机的观测值会受到指数级惩罚，以降低其对状态更新的影响。仿射参数会在传播状态阶段一次性估计出来，作为 InEKF 迭代循环（第 VI-D 节）之前的视觉预计算环节的一部分。
### D. 光度残差与循环前雅可比矩阵
在每次 InEKF 迭代时重新计算完整的光度雅可比矩阵是视觉惯性导航的主要计算开销，但带来的精度提升却微乎其微，因此本节提出了循环前雅可比矩阵冻结技术。每个观测值采用稀疏的 9 像素模式，即在投影位置 $3 \times 3$ 为中心、间距为 $\rho$ 的 $\pi(\mathbf{p}_j^c)$ 网格采样点：
$$ \mathcal{P}=\{(0,0)\}\cup\{(\pm\rho,0),(0,\pm\rho)\}\cup\{(\pm\rho,\pm\rho)\}, $$
采样半径设为 $\rho = \max(1, \lfloor s_p/2 \rfloor - 1)$（默认值为 $s_p = 8$，即 $\rho = 3$ 像素），每个像素通过双线性插值获取，再将各像素残差取平均值以得到标量 $r_j$（见图 4）。对于属于 3D 点 $\mathbf{p}_j^w$ 的第 j 个滑动窗口观测值，经仿射补偿后的光度残差为：
$$ r_{j}=\frac{1}{9}\sum_{k\in\mathcal{P}}\Bigl[I_{\mathrm{c u r}}(u_{k},v_{k})-\bigl(\alpha_{j}I_{\mathrm{r e f}}(u_{k}^{\mathrm{r e f}},v_{k}^{\mathrm{r e f}})+\beta_{j}\bigr)\Bigr], $$
其中 $\mathbf{p}_j^c = \mathbf{R}_{ci} \mathbf{R}^\top (\mathbf{p}_j^w - \mathbf{p}) + \mathbf{p}_{ci}$ 是该点在相机坐标系中的位置，而 $(u_k^{\mathrm{ref}}, v_k^{\mathrm{ref}})$ 则是在观测创建时固定的参考坐标系中的位置。
关于$1 \times 6$的$[\delta\phi; \delta\mathbf{p}]$雅可比矩阵可由链式法则$I(\pi(\mathbf{p}^c(\mathbf{R},\mathbf{p})))$推导得出。下述推导将$\delta\mathbf{p}^c$表示为相机坐标系下的量，如此一来图像梯度就能直接与状态扰动相关联，而无需再考虑那些每次迭代都需重新计算的机体坐标系旋转因素。此处用到的工具是$\mathrm{SO}(3)$伴随恒等式$\mathbf{R}[\mathbf{v}]_\times = [\mathbf{R}\mathbf{v}]_\times\mathbf{R}$，它可将世界坐标系下的斜对称矩阵$[\mathbf{p}^w - \mathbf{p}]_\times$用$[\mathbf{p}^c - \mathbf{p}_{ci}]_\times$来表示。在右不变扰动$\mathbf{R} \leftarrow \mathrm{Exp}(\delta\phi)\mathbf{R}$的作用下，可得到如下结果：
$$ \begin{align*}\delta\mathbf{p}^{c}&=\mathbf{R}_{ci}\mathbf{R}^{\top}\left[\left\lfloor\mathbf{p}_{j}^{w}-\mathbf{p}\right\rfloor\times\delta\phi-\delta\mathbf{p}\right]\\&=\left\lfloor\mathbf{p}_{j}^{c}-\mathbf{p}_{ci}\right\rfloor\times\mathbf{R}_{ci}\mathbf{R}^{\top}\delta\phi-\mathbf{R}_{ci}\mathbf{R}^{\top}\delta\mathbf{p},\end{align*} $$
其中第二行利用伴随恒等式将斜对称算子从机体坐标系转换到相机坐标系。再结合图像到三维空间的梯度$\nabla I_j = [I_x, I_y] \mathbf{J}_\pi \in \mathbb{R}^{1 \times 3}$（此处$\mathbf{J}_\pi \in \mathbb{R}^{2 \times 3}$为相机模型的投影雅可比矩阵，可以是针孔、等距或鱼眼模型），便可得到：
$$ {\mathbf{J}_{j}}=\nabla I_{j}\left[\left\lfloor{{\mathbf{p}_{j}^{c}}-{\mathbf{p}_{{c i}}}}\right\rfloor_{\times}{\mathbf{R}_{{c i}}}{\mathbf{R}^{\top}}\right.\left.\vphantom{{\mathbf{J}_{j}^{c}}}-{\mathbf{R}_{{c i}}}{\mathbf{R}^{\top}}\right]\in\mathbb{R}^{1\times6}. $$
整个视觉信息形式$(\Lambda_V, \mathbf{b}_V)$会在InEKF循环开始前，基于传播后的状态$\hat{\mathbf{x}}$一次性生成，并在循环迭代过程中保持不变，其上限为$K_{\mathrm{max}} (K_{\mathrm{max}} = 5)$。在本研究采用的收敛阈值$\epsilon_\phi = 0.01^\circ$下，
---
焦距为$f = 500$像素的相机，每次迭代中投影像素的位移最多仅为$f_{\epsilon_{\phi}} \approx 0.09$像素，这一数值远低于双线性插值的采样间隔。因此固定$\mathbf{J}_j$可将每次观测所需的冗余计算量最多减少$K_{\max} - 1$次，且对精度的影响微乎其微。与用于保证估计器一致性的首估雅可比矩阵（FEJ）技术[^27]、[^28]不同——该技术会在各帧间固定雅可比矩阵，而这种固定策略仅局限于单个更新循环中，其出发点纯粹是为了提升效率，利用的是光度残差及其雅可比矩阵对InEKF收敛过程中产生的亚度级、亚毫米级增量不敏感的特性。相比之下，激光雷达残差会在每次迭代时重新计算（见第VII-E节），因此联合更新仍能追踪到最新状态。
单次观测的光度噪声方差取决于深度：
$$ \sigma_{j}^{2}=\sigma_{\mathrm{p x}}^{2}+\frac{\partial I}{\partial\mathbf{p}^{c}}\mathbf{\Sigma}_{c}\left(\frac{\partial I}{\partial\mathbf{p}^{c}}\right)^{\top}, $$
其中$\sigma_{\mathrm{px}}^2$是固定的每像素光度噪声下限（以灰度平方为单位），$\partial I / \partial \mathbf{p}^c \in \mathbb{R}^{1 \times 3}$是图像到三维空间的梯度链，$\boldsymbol{\Sigma}_c = \mathbf{R}_{cw} \boldsymbol{\Sigma}_w \mathbf{R}_{cw}^\top$是从式(11)经相机外参传播得到的相机坐标系下的点不确定性。这种基于深度的约束确保了位置不确定性较大的远距离点对$\Lambda_V$的贡献相应减小。
### E. 带信息去相关的滑动窗口
The visual module maintains a sliding window $\mathcal{W}$ of the W most recent frames, with $W = 5$ used throughout our experiments as a typical value. Each frame stores up to $N_{obs}$ observations selected from the LiDAR-anchored map points, with a per-frame cap enforced by uniform stride downsampling to preserve spatial coverage.
滑动窗口是信息形式融合的结构性要求，而非启发式改进。单帧光度信息矩阵$\mathbf{\Lambda}_V^{\text{single}} = \sum_i w_i \mathbf{J}_i^\top \mathbf{J}_i$的条件数极差：由于所有图像雅可比矩阵$\mathbf{J}_i$共享同一个观测视角，信息集中在图像平面方向上，而沿光轴方向以及投影较弱的旋转方向则几乎接收不到任何信息。在SAIF线性限幅门控机制中（见第VII-D节），具有$\sqrt{\lambda_k} < \sigma_{\min}$的方向会被$\gamma(k) = \sqrt{\lambda_k} / \sigma_{\min}$衰减，因此$\mathbf{\Lambda}_L + q \mathbf{\Lambda}_V$的每个特征方向必须携带足够的联合信息才能发挥全部作用。在W个具有不同相机基线的帧上累积雅可比矩阵，能够拓展更丰富的子空间，从而提升单帧观测无法触及的那些方向上$\mathbf{\Lambda}_V$的特征值。因此，滑动窗口直接决定了SAIF所处理的$\mathbf{\Lambda}_V$的秩和方向覆盖范围。
重复帧抑制机制保证了窗口内的几何多样性：如果新帧相对于前一帧的旋转变化小于$\delta_{R}$且平移变化小于$\delta_{t}$，则该帧会替换掉旧帧而非被追加进去，这样窗口就能保留最新的观测数据，又不会因冗余视角占用存储槽位。当窗口内帧数超过W时，最旧的帧会被移除，其对应的观测数据也会被丢弃。与已移除帧ID相关的仿射参数也会被清理，以防止内存无限制增长。
同一3D点在多个滑动窗口帧中的观测结果会产生时间相关性，这违背了InEKF的测量独立性假设。为缓解这一问题，每次观测都会被赋予一个使用计数器$n_{used}$，每当它参与$\Lambda_{V}$的计算时该计数器就会递增。去相关因子
$$ w_{dec}=\frac{1}{n_{used}} $$
确保该点首次贡献时能以全权重参与计算，而后续重复使用时的影响则会逐步衰减。这一机制还得到了第VII-E节所述的收敛后协方差膨胀$\hat{\mathbf{P}}_{0:6} \leftarrow (1 + \varepsilon_P) \check{\mathbf{P}}_{0:6} (\varepsilon_P = 0.02)$的补充，后者能够共同补偿仅靠去相关无法消除的残余相关性。
### F. VIO质量因子
在信息形式进入融合阶段之前，场景级质量因子$q \in [0,1]$会对视觉信息形式进行筛选，防止质量较差或受损的VIO数据影响状态更新。该因子由三个相乘的分量构成，分别对应不同的失效模式：
$$ \begin{array}{r}{q=q_{\mathrm{r m s}}\cdot q_{\mathrm{c n t}}\cdot q_{\mathrm{r e j}},}\end{array} $$
其中
$$ q_{\mathrm{r m s}}=\begin{cases}{1}&{r_{\mathrm{r m s}}\leq\delta_{\mathrm{r m s}}}\\ {\exp\bigl(-(r_{\mathrm{r m s}}-\delta_{\mathrm{r m s}})/\sigma_{\mathrm{r m s}}\bigr)}&{r_{\mathrm{r m s}}>\delta_{\mathrm{r m s}},}\\ \end{cases} $$
用于惩罚较大的光度残差，此处$r_{rms}$为每帧内各观测值的均值$r_{rms,j}$（灰度单位）；阈值$\delta_{rms}$和$\sigma_{rms}$分别设为12和8灰度级，这些数值源自训练序列中的光度残差分布规律；$q_{cnt} = \min(N_{valid}/N_{q}, 1)$会随着有效观测数达到最小支持阈值$N_{q}$而线性增至1；以及
$$ q_{rej}=\begin{cases}1&\eta\leq\eta_{min}\\\frac{\eta_{\max}-\eta}{\eta_{\max}-\eta_{\min}}&\eta_{\min}<\eta\leq\eta_{\max}\\0&\eta>\eta_{\max},\end{cases} $$
其中$\eta = N_{\mathrm{rej}}/(N_{\mathrm{valid}} + N_{\mathrm{rej}})$表示异常值剔除比例。这些阈值在所有序列中保持不变，且设定得较为粗略：q仅提供场景级的阻尼效果，细致的方向级加权则由SAIF负责。当$q \approx 0$（例如在黑暗环境或严重运动模糊条件下）时，系统会平滑降级为仅使用LIO的更新模式，无需额外的模式切换逻辑。
### G. 视觉信息矩阵
$\Lambda_V$仅在通过顺序多门滤波器的观测值上累积： (i) 投影有效性（深度为$> d_{\min} = 0.5$米，且像素在有效范围内）；(ii) 视角门$(\mathbf{n} \cdot \mathbf{d}_v) > \cos \theta_{\max}$，会排除$> \theta_{\max}$这种掠射视角；(iii) 视差门$\left(\|\pi(\mathbf{p}_j^c) - \pi_{\text{ref},j}\right| \geq \delta_{\text{plx}}$，其中$\pi_{\text{ref},j}$为参考帧投影，$\delta_{\text{plx}}$为最小视差值。
---
以像素为单位；(iv) 绝对残差门($r_{\mathrm{rms},j} \leq \delta_{\mathrm{abs}}$，其中$r_{\mathrm{rms},j} = \sqrt{\frac{1}{9} \sum_{k} e_{k}^{2}}$为图像块像素的均方根误差)； (v) $\chi^{2}$创新门($r^{2}/\sigma_{i}^{2} \leq \tau_{V}$)。
符合条件的观测值会与信息去相关因子(24)、仿射置信度因子$w_{\mathrm{cam}} = \exp(-\max(|\log \alpha| - \delta_{\alpha}, 0))$以及Huber因子$w_{\mathrm{hub}} = \min(1, \tau_{\mathrm{hub}}/|r_j|)$结合，其中$\tau_{\mathrm{hub}} = \delta_{\mathrm{abs}}/2$用于削弱那些虽能通过第(iv)个门但残差仍较大的观测值的影响，最终得到复合标量：
$$ \tilde{w}_{j}=\frac{w_{\mathrm{h u b}}\cdot w_{\mathrm{c a m}}\cdot w_{\mathrm{d e c}}}{\sigma_{j}^{2}}. $$
视觉信息形式是在通过所有门的观测值集合$\mathcal{I}_{V}$上累积而成的：
$$ \begin{array}{r}{\mathbf{\Lambda}_{V}=\sum_{j\in\mathcal{I}_{V}}\tilde{w}_{j}\mathbf{J}_{j}^{\top}\mathbf{J}_{j},}\end{array} $$
$$ \mathbf{b}_{V}=-\sum_{j\in\mathcal{I}_{V}}\tilde{w}_{j}\mathbf{J}_{j}^{\top}r_{j}. $$
### 七、子空间感知的信息融合
与方向无关的融合方式会将每个传感器的信息均匀分配到所有姿态方向上，而不考虑各方向的可靠性。本节提出了一种解决方案：对联合信息矩阵进行特征分解，针对每个特征方向应用线性限幅软门，再重构出融合矩阵——该矩阵能在可靠的方向上保留每个传感器的全部贡献，同时平滑衰减退化方向的信息，并将其交由惯性测量单元先验来处理。
适用范围。SAIF作用于状态中的姿态模块，而非整个状态。所有激光雷达（6个）和光度传感器（22个）的雅可比矩阵都作用于6维的外感知姿态误差子空间$\mathcal{S}_{\text{ext}} \triangleq \{\delta \phi, \delta \mathbf{p}\} \subset \mathbb{R}^{18}$，在速度、偏差和重力维度上其值均为零；因此$\Lambda_L$和$\Lambda_V$按构造属于$6 \times 6$，特征分解、门控和重构操作都在$\mathcal{S}_{\text{ext}}$中进行。所以SAIF是对外感知测量子空间的光谱可观测性分析，而非对18维状态的分析：剩余的12个坐标从未经过门控，仅能间接被修正，因为$\mathbf{K}^{-1}$会沿着先验协方差传递经过门控的姿态信息（见第七节E部分）。衰减某个退化的姿态方向也会相应抑制对偏差和重力的间接修正，这正是设计初衷：这些状态只有在对应的姿态方向被观测到时才会被精细化。视觉信息在进入融合流程前会先乘以质量因子q（见第六节F部分），这会均匀降低$\Lambda_V$在六个姿态方向上的数值；随后SAIF会对经q缩放后的联合特征谱进行操作，仅衰减那些低于$\sigma_{\text{min}}$的方向。
### A. 信息不一致性问题
若不考虑$\Lambda_{L}$和$\Lambda_{V}$的光谱结构就直接合并二者，就会导致第一节中提到的两类问题：在激光雷达能可靠约束的方向上（$\Lambda_{L}$的特征值较大），无条件的视觉融合会将光度偏差引入本已精确的估计中；而在退化方向上（特征值接近零），视觉修正会分散到六个姿态维度上，恰好在最需要修正的地方被稀释。最终得到的融合估计值在各自能可靠约束的方向上，其精度可能还不如单个传感器的结果。
SAIF与质量因子q通过分工解决了这两类不一致性问题。第二类不一致性是光谱门的主要处理对象。由于该门作用于联合谱$\mathbf{\Lambda}_L + q\mathbf{\Lambda}_V$，因此只要任一模态对某一方向产生约束，该方向就会以全权重通过。视觉系统因此能将退化的激光雷达方向提升至门限之上（即VIO救援机制，见图9），而那些两种模态都无法解析的方向则会被衰减为惯性测量单元（IMU）的先验值。第一类不一致性并非该门的处理范畴，因为该门会保留观测良好的联合方向的完整权重。在此情况下，视觉噪声的干扰会通过激光雷达特征值的优势来抑制——这使得经q缩放后的视觉项仅成为微小的扰动；而当视觉数据流整体质量下降时，q本身也会起到抑制作用。需注意一点：该门的作用范围有限，它仅依据信息幅度而非正确性来判断。例如，即便某次激光雷达测量存在偏差但仍产生较大的特征值，在联合谱中它与正确观测到的结果无法区分，从而会以未衰减的状态通过；而剔除此类误差的任务则由各残差鲁棒门以及质量因子q来完成，它们会在测量数据进入$\mathbf{\Lambda}_L + q\mathbf{\Lambda}_V$之前对其进行筛选。SAIF仅负责决定这些留存下来的可信信息在各方向上的分配方式（见图5）。
Fig. 5. How SAIF resolves fusion one direction at a time (schematic; three of six directions). Top: the LiDAR scans a planar patch face-on, constraining the normal $\mathbf{u}_3$ but not position along the surface, where the camera supplies what the plane geometry lacks. Bottom: each bar is the joint amplitude $\sqrt{\lambda_k}$ of $\boldsymbol{\Lambda}_{\mathrm{sum}} = \boldsymbol{\Lambda}_L + q\boldsymbol{\Lambda}_V$, split into the LiDAR share and what vision adds; dashed line is $\sigma_{\min}$. Vision carries $\mathbf{u}_2$ back over the threshold so it passes at full weight, $\mathbf{u}_1$ is short in both modalities and is attenuated continuously ($0 < \gamma < 1$) toward the IMU prior $\hat{\mathbf{P}}^{-1}$, and $\mathbf{u}_3$ is never gated.
### B. 问题表述
我们旨在得到一个融合信息矩阵$\Lambda_f$，通过满足以下三个要求来解决信息不一致性问题：(D1)
---
保留那些组合测量能良好观测到的方向上的信息（即较大的联合特征值）；(D2) 衰减那些组合测量无法提供有效信息的方向（即接近零的联合特征值）；(D3) 融合过程需具备对称性（对两种传感器一视同仁），保证结果为半正定矩阵且频谱连续。
为了明确这些目标，我们在联合信息矩阵的特征基下展开分析。设
$$ \mathbf{\Lambda _{sum}}=\mathbf{\Lambda}_{L}+q\mathbf{\Lambda}_{V}=\mathbf{U}\operatorname{diag}(\lambda_{1},\ldots,\lambda_{6})\mathbf{U}^{\top}, $$
其中$q \in [0,1]$为VIO质量因子(25)，$\mathbf{U} = [\mathbf{u}_1, \ldots, \mathbf{u}_6]$为正交归一化向量。采用联合特征基而非仅基于激光雷达的基，是为了确保特征值$\{\lambda_k\}$能够反映激光雷达与视觉组合测量在该方向上的总体可观测性，而非仅偏向激光雷达的几何特性。每个特征值的平方根$\sqrt{\lambda_k}$即为沿$\mathbf{u}_k$方向的信息幅度：它等于该方向上的后验标准差的倒数$1/\sigma_k$，也是在特征基下通过平方根信息滤波（SRIF）分解$\mathbf{\Lambda}_{\text{sum}} = \mathbf{S}^\top \mathbf{S}$所得到的对角元素。将汇总后的信息向量投影到同一基上，
$$ \mathbf{b}_{\mathrm{s u m}}=\mathbf{b}_{L}+q\mathbf{b}_{V},\quad\mathbf{b}^{\prime}=\mathbf{U}^{\top}\mathbf{b}_{\mathrm{s u m}}, $$
便会得到一个6维的解耦系统，其中每个坐标对应位姿空间中一个具有几何意义的方向$\mathbf{u}_{k}$。
### C. 平方根信息线性限幅软门
阈值$\sigma_{min}$是一个信息幅度阈值：当某方向的信息幅度$\sqrt{\lambda_{k}}$达到$\sigma_{min}$时，该方向会获得全权重；若低于该阈值则会被线性衰减：
$$ \gamma(k)=\min\left(\frac{\sqrt{\lambda_{k}}}{\sigma_{\min}},1\right), $$
因此，当$\gamma(k) = 1$时$\sqrt{\lambda_k} \geq \sigma_{\min}$（可观测状态），而当$\gamma(k) = \sqrt{\lambda_k} / \sigma_{\min} \in [0,1)$时$\sqrt{\lambda_k} < \sigma_{\min}$（退化状态，部分衰减）。
我们称式(33)为线性限幅门，因为$\gamma$在信息振幅$\sqrt{\lambda_k}$上呈线性限幅关系，而非在特征值上：将其代入式(34)可得$\tilde{\lambda}_k = \lambda_k^{3/2} / \sigma_{\min}$低于阈值，因此信息本身的衰减在$\lambda_k$上表现为非线性；当振幅达到阈值的一半时，该方向保留的信息量为原来的二分之一而非四分之一。振幅是适合采用线性关系的变量，因为它具有逆标准差的单位（即SRIF对角线），所以一个方向的衰减程度与其后验精度低于$\sigma_{\min}$的程度成正比。这种连续渐变机制取代了每个传感器需设置四个手动调节阈值的软门控方式（每个传感器对应$\tau_{\text{abs}}$和$\tau_{\text{rel}}$），转而采用单一阈值，且该阈值的含义与信息所属的方向来自哪个传感器无关。无论信息是源自激光雷达平面几何、光度梯度还是两者的叠加，边界值$\sigma_{\min}$所对应的都是相同的信息振幅（逆后验标准差）。
在18维状态下，典型的设定值为$\sigma_{\min} = 1$。直观而言，这将可观测性边界设定在单位后验标准差处：由于$\sqrt{\lambda_k} = 1/\sigma_k$，当一个方向经单次更新后其精度优于1米（平移）或1弧度（旋转）时，便能以全强度通过；否则该方向的信息将被交由惯性测量单元（IMU）的先验模型处理。观测良好的方向可汇聚数百个激光雷达或光度约束条件$(\sqrt{\lambda_k} \gg 1)$，而退化的方向则会趋近于$\sqrt{\lambda_k} = O(1)$，因此联合谱隙会跨越单位值，这正是图10中灵敏度最小值出现在$\sigma_{\min} = 1$附近的原因。
由于$\Lambda_f$同时混合了旋转（$rad^{-2}$）和平移（$m^{-2}$）维度，因此$\sigma_{\min}$并非纯量纲的量，这一简化处理方式与以往基于特征谱的退化检测器[^9]、[^11]一致。其后果值得明确说明：SAIF的门控机制定义在固定的国际单位制坐标误差空间（米、弧度）中，并不具备对误差坐标缩放的不变性。若用厘米表示位置或用度表示角度，就会改变$\Lambda_{\text{sum}}$的各块数值及其特征值，进而需要重新设定$\sigma_{\min}$。这描述的是估计器所处的国际单位制误差空间特性，而非几何本身的无坐标属性。在固定单位下，不同场景间可共用的是默认情况下的灵敏度最小值位置（见图10），而非其周围的响应范围——随着退化程度的增加，该范围会逐渐变窄；这正是此处用一个数值即可适用于所有序列的原因。
当一个方向的信号强度低于$\sigma_{min}$时，其测量贡献会被按比例降低；随着$\hat{\mathbf{P}}^{-1}$的增加，惯性测量单元（IMU）的先验模型$\sqrt{\lambda_k} \to 0$对该方向的控制力逐渐增强，且协方差会在过程噪声作用下平滑增长。可观测方向$(\gamma(k) = 1)$则能完整传递其信息而无衰减，这与所有低于阈值方向均被压缩的S型门控机制截然不同。
### D. 特征基重构
经过门控处理后，融合信息的形态是通过将$\mathrm{diag}(\lambda_k)$的对角元素及$\mathrm{b'}$的对应元素乘以$\gamma(k)$，再执行逆向旋转来重构的：
$$ \tilde{\lambda}_{k}=\gamma(k)\lambda_{k},\qquad\tilde{\mathbf{b}}^{\prime}(k)=\gamma(k)\mathbf{b}^{\prime}(k), $$
$$ \mathbf{\Lambda}_{f}=\mathbf{U}\mathrm{diag}(\tilde{\lambda}_{1},\ldots,\tilde{\lambda}_{6})\mathbf{U}^{\top},\quad\mathbf{b}_{f}=\mathbf{U}\tilde{\mathbf{b}}^{\prime}. $$
从构造上而言，该重构矩阵是半正定的：由于 $\gamma(k) \in [0,1]$ 和 $\lambda_k \geq 0$，每个 $\tilde{\lambda}_k = \gamma(k)\lambda_k \geq 0$ 均满足此性质，因此无需进行正则化 $\varepsilon\mathbf{I}$。对特征值进行门控处理时，$\Lambda_{\text{sum}}$ 的特征向量保持不变，且每个特征向量会按 $\gamma(k)$ 的比例缩放——这是一种连续加权的投影，而非硬性的二元划分。
与针对每个传感器分别投影 $\Lambda_L$ 和 $\Lambda_V$ 的方案相比，此处仅对求和后的量 $\Lambda_{\text{sum}}$ 和 $\mathbf{b}_{\text{sum}}$ 进行特征分解/投影。这能将融合阶段中稠密 $6 \times 6$ 矩阵的计算量减半，也无需再追踪各个方向上来自激光雷达与视觉的信息占比。令 $\gamma(k)$ 由式(33)定义，$\Lambda_f$ 由式(34)–(35)定义。根据门控定义可直接得出三种运行模式：(i)
1) 完全可观测（$\sqrt{\lambda_k} \geq \sigma_{\min} \forall k$，故 $\gamma(k) = 1$）：$\Lambda_f = \Lambda_L + q\Lambda_V$。此时融合过程简化为带质量门控的直接求和；两种传感器均以全强度贡献数据。
2) Partial degeneracy (some $\sqrt{\lambda_k} < \sigma_{\min}$): the joint update along $\mathbf{u}_k$ is attenuated by $\gamma(k) = \sqrt{\lambda_k} / \sigma_{\min} \in (0,1)$; the IMU prior $\hat{\mathbf{P}}^{-1}$ increasingly governs that direction
---
```pseudo
// Algorithm 1: Subspace-Aware Information Fusion (Linear-Clamp Soft Gate)
Require: $(\Lambda_L, \mathbf{b}_L)$, $(q\Lambda_V, q\mathbf{b}_V)$, observability threshold $\sigma_{\min}$
1: $\Lambda_{\text{sum}} \leftarrow \Lambda_L + q\Lambda_V$; $\mathbf{b}_{\text{sum}} \leftarrow \mathbf{b}_L + q\mathbf{b}_V$
2: $[\mathbf{U}, \text{diag}(\lambda_k)] \leftarrow \text{SelfAdjointEig}(\Lambda_{\text{sum}})$ // joint eigenbasis, (31)
3: $\mathbf{b}' \leftarrow \mathbf{U}^\top \mathbf{b}_{\text{sum}}$ (32)
4: for $k = 1$ to 6 do
5: $\gamma(k) \leftarrow \min(\sqrt{\lambda_k} / \sigma_{\min}, 1)$ // linear-clamp gate, (33)
6: $\tilde{\lambda}_k \leftarrow \gamma(k) \lambda_k$; $\tilde{\mathbf{b}'}(k) \leftarrow \gamma(k) \mathbf{b}'(k)$
7: end for
8: $\Lambda_f \leftarrow \mathbf{U} \text{diag}(\tilde{\lambda}_1, \ldots, \tilde{\lambda}_6) \mathbf{U}^\top$; $\mathbf{b}_f \leftarrow \mathbf{U}\tilde{\mathbf{b}}'$ (35)
Ensure: Fused $(\Lambda_f, \mathbf{b}_f)$, guaranteed PSD
```
as observability weakens. Strong directions ( $\gamma(k)=1$) retain their summed information unchanged.
3) 单传感器或无传感器极限情况：若激光雷达失效（$\Lambda_L \approx 0$），则门控仅依赖视觉信号的幅度，融合结果简化为带质量门控的视觉惯性里程计更新；若两者性能均下降（$q \approx 0$ 且激光雷达信号微弱），则对大多数 k 而言 $\sqrt{\lambda_k} \ll \sigma_{\min}$，此时IMU先验占主导地位——这是一种无需额外逻辑处理的优雅故障模式。
这些状态转换会随着联合信息幅度 $\sqrt{\lambda_k}$ 相对于物理阈值 $\sigma_{\min}$ 的变化而连续过渡，从而呈现出可解释的场景驱动型衰减特性，而非需要手动调参的传感器专属二元开关。
完整的融合流程在算法1中进行了总结。
### E. 带光谱门控的统一联合惯性扩展卡尔曼滤波更新
我们并非针对每种模态单独更新状态，而是分别整理激光雷达信息形式（$\mathbf{A}_L, \mathbf{b}_L$）与视觉信息形式（$\mathbf{A}_V, \mathbf{b}_V$），再通过算法1进行融合，得到具有方向选择性的结果（$\mathbf{A}_f, \mathbf{b}_f$）。该融合结果被嵌入到完整的18维状态空间中，随后仅需求解一次惯性扩展卡尔曼滤波更新即可。此处 $\mathbf{K}$ 代表总后验信息，即测量值与先验信息的叠加，因此 $\mathbf{K}^{-1}$ 即为对应的后验协方差，可作为经精度缩放的增益使用：
$$ \mathbf{K}\triangleq\tilde{\mathbf{\Lambda}}_{f}+\hat{\mathbf{P}}^{-1},\quad\tilde{\mathbf{\Lambda}}_{f}=\begin{bmatrix}\mathbf{\Lambda}_{f}&\mathbf{0}\\ \mathbf{0}&\mathbf{0}\end{bmatrix}\in\mathbb{R}^{18\times18}, $$
$$ \mathbf{G}_{6}=\mathbf{K}_{:,0:6}^{-1}\mathbf{\Lambda}_{f},\quad\mathbf{\bar{G}}=\left[\begin{matrix}{\mathbf{G}_{6}}&{\mathbf{0}_{18\times12}}\\ \end{matrix}\right], $$
$$ \delta\mathbf{x}=\mathbf{K}_{:,0:6}^{-1}\mathbf{b}_{f}+(\hat{\mathbf{x}}^{0}\boxminus\hat{\mathbf{x}}^{t})-\mathbf{G}_{6}\left(\hat{\mathbf{x}}^{0}\boxminus\hat{\mathbf{x}}^{t}\right)_{0:6}. $$
式(38)中的三项具有不同的物理作用。第一项 $\mathbf{K}_{..:0:6}^{-1} \mathbf{b}_f$ 就是测量更新本身：它通过经精度缩放的增益，将带门控的融合梯度 $\mathbf{b}_f$ 注入到完整的18维状态中。第二项 $\hat{\mathbf{x}}^0 \boxminus \hat{\mathbf{x}}^t$ 是原传播状态 $\hat{\mathbf{x}}^0$ 与当前迭代值 $\hat{\mathbf{x}}^t$ 之间的位移，该位移通过流形上的 $\boxminus$ 算子表示；它使迭代过程锚定在IMU先验上，避免后续线性化过程偏离该基准。第三项 $-\mathbf{G}_6\left(\hat{\mathbf{x}}^0 \boxminus \hat{\mathbf{x}}^t\right)_{0:6}$ 用于减去测量更新已通过紧凑增益 $\mathbf{G}_6$ 解释掉的位移部分，防止先验偏移被同时计为先验与观测值。这种单次求解的联合更新具备两项结构特性。
```pseudo
// Algorithm 2: SA-LIVO Joint InEKF Update
Require: Propagated $(\hat{\mathbf{x}},\hat{\mathbf{P}})$; LiDAR points $\{\mathbf{p}_i^b\};$ image I; sliding window $\mathcal{W}$
1: $(\mathbf{\Lambda}_V,\mathbf{b}_V), q \leftarrow \text{VISUALINFOFORM}(\mathcal{W},\hat{\mathbf{x}})$ // pre-loop, frozen across iterations
2: $\hat{\mathbf{x}}^0 \leftarrow \hat{\mathbf{x}}$; $t \leftarrow 0$
3: repeat
4: $(\mathbf{\Lambda}_L,\mathbf{b}_L) \leftarrow \text{LIDARINFOFORM}(\hat{\mathbf{x}}^t)$
5: $(\mathbf{\Lambda}_f,\mathbf{b}_f) \leftarrow \text{SUBSPACE} \text{AWAREFU-SION}(\mathbf{\Lambda}_L,\mathbf{b}_L,q\mathbf{\Lambda}_V,q\mathbf{b}_V)$
6: Solve (38) by $18 \times 18$ Cholesky (LLT) solve; $\hat{\mathbf{x}}^{t+1} \leftarrow \hat{\mathbf{x}}^t$ 田 $\delta\mathbf{x}$; $t \leftarrow t+1$
7: until convergence (twice consecutive) or $t = K_{\max}$
8: $\hat{\mathbf{P}} \leftarrow (\mathbf{I}_{18} - \bar{\mathbf{G}})\hat{\mathbf{P}}$; inflate $\hat{\mathbf{P}}_{0:6}$ by $(1 + \varepsilon_P)$
9: Update voxel map; advance $\mathcal{W}$
Ensure: $(\hat{\mathbf{x}}^t,\hat{\mathbf{P}})$
```
First, both sensors enter a single information-form solve rather than sequential per-modality updates: LiDAR plane correspondences and per-point covariance terms are prepared from the propagated state and reused after the first iteration, while the compact LiDAR information form is reassembled as residuals change; the visual information form is assembled once before the loop at $\hat{x}^0$ and reused. This avoids the inter-sensor ordering mismatch of sequential passes, where the visual update is linearized after a completed LiDAR update at $\hat{x}^{\text{LIO}} \neq \hat{x}^0$. Second, all modalities are handled within a single iteration loop of at most $K_{\text{max}}$ steps; sequential per-sensor loops would multiply the iteration budget by the number of modalities. With correspondences cached at $t = 0$ and only point-to-plane distances recomputed for $t > 0$, the per-iteration cost is dominated by a single $18 \times 18$ linear solve and $O(N)$ LiDAR residual re-evaluation. Convergence is declared when both $\|\delta \phi\| < \epsilon_\phi$ and $\|\delta \mathbf{p}\| < \epsilon_p$ hold for two consecutive iterations. Upon convergence, the covariance is updated as $\hat{\mathbf{P}} \leftarrow (\mathbf{I}_{18} - \hat{\mathbf{G}}) \hat{\mathbf{P}}$, and the pose block is inflated $\hat{\mathbf{P}}_{0:6} \leftarrow (1 + \varepsilon_P) \hat{\mathbf{P}}_{0:6}$ to compensate for temporal correlation in sliding-window observations. The complete procedure is given in Algorithm 2.
### VIII. EXPERIMENTS
### A. Experimental Setup
SA-LIVO is implemented in C++ with Robot Operating System (ROS) and evaluated on a laptop (Intel i9-13900HX, 32 GB RAM) and an ARM platform (NVIDIA Jetson Orin, 12-core ARM Cortex-A78AE @ 2.2 GHz, 64 GB RAM).
Our self-collected sequences were acquired with the handheld platform shown in Fig. 6. It comprises a Livox AVIA solid-state LiDAR (10 Hz, non-repetitive scan pattern) with built-in IMU (200 Hz) and an industrial camera (10 Hz), all rigidly mounted on a handheld enclosure with an on-board embedded PC. The LiDAR is time-stamped via pulse-per-second (PPS) and NMEA GPRMC messages, and the camera
---
TABLE II
ABSOLUTE TRANSLATIONAL ROOT-MEAN-SQUARE ERROR (RMSE, M) ON ALL DATASETS. × MARKS A RUN THAT DID NOT PRODUCE A COMPLETE TRAJECTORY FOR THE SEQUENCE, THROUGH EITHER PROCESS TERMINATION OR TRACKING LOSS; SUCH RUNS ARE EXCLUDED FROM ALL REPORTED AVERAGES. BOLD MARKS THE BEST OR TIED-BEST RESULT AMONG THE BASELINES AND OURS; THE ABLATION COLUMNS ARE DIAGNOSTIC AND EXCLUDED FROM THIS COMPARISON.
| Baselines | Ablations |
| Sequence | FAST-LIO2 | FAST-LIVO | FAST-LIVO2 | R3LIVE | SR-LIVO | Ours | w/o VIO | w/o affine | w/o sub. | w/o s.w. |
| exp01 construction ground level | 0.014 | 0.019 | 0.011 | 0.084 | 0.015 | 0.010 | 0.014 | 0.012 | 0.012 | 0.011 |
| exp02 construction multilevel | 0.037 | 0.033 | 0.020 | 0.086 | 0.050 | 0.020 | 0.022 | 0.020 | 0.020 | 0.020 |
| exp03 construction stairs | 0.392 | 0.789 | 0.415 | $\times$ | $\times$ | 0.029 | 0.044 | 0.051 | 0.052 | 0.059 |
| exp04 construction upper level | 0.045 | 0.082 | 0.039 | 0.072 | 0.024 | 0.024 | 0.024 | 0.037 | 0.024 | 0.024 |
| exp05 construction upper level 2 | 0.044 | 0.010 | 0.012 | 0.080 | 0.012 | 0.007 | 0.008 | 0.008 | 0.009 | 0.009 |
| exp06 construction upper level 3 | 0.048 | 0.053 | 0.010 | 0.083 | 0.018 | 0.011 | 0.013 | 0.011 | 0.011 | 0.010 |
| exp07 long corridor | 0.079 | 0.112 | 0.044 | 0.095 | 0.041 | 0.044 | 0.055 | 0.055 | 0.055 | 0.053 |
| exp09 cupola | $\times$ | $\times$ | 0.167 | $\times$ | $\times$ | 0.130 | 0.140 | 0.139 | 0.152 | 0.184 |
| exp10 cupola 2 | $\times$ | $\times$ | 0.218 | $\times$ | $\times$ | 0.075 | 0.119 | 0.129 | 0.135 | 0.140 |
| exp11 lower gallery | 0.397 | 0.474 | 0.015 | $\times$ | 0.989 | 0.015 | 0.016 | 0.015 | 0.017 | 0.015 |
| exp14 basement 2 | 0.140 | 0.070 | 0.035 | 0.098 | $\times$ | 0.024 | 0.024 | 0.023 | 0.026 | 0.022 |
| exp15 attic to upper gallery | 2.227 | 0.741 | 0.192 | $\times$ | $\times$ | 0.122 | 0.134 | 0.200 | 0.195 | 0.148 |
| exp16 attic to upper gallery 2 | 1.927 | 1.211 | 0.102 | $\times$ | $\times$ | 0.069 | 0.079 | 0.104 | 0.252 | 0.205 |
| exp18 corridor lower gallery 2 | 0.366 | $\times$ | 0.194 | $\times$ | $\times$ | 0.022 | 0.029 | 0.029 | 0.038 | 0.127 |
| exp21 outside building | 0.121 | 0.076 | 0.036 | 0.088 | 0.048 | 0.022 | 0.025 | 0.037 | 0.036 | 0.041 |
| Quad Easy | 0.092 | 0.099 | 0.081 | 0.086 | 0.028 | 0.026 | 0.074 | 0.028 | 0.027 | 0.029 |
| Quad Medium | 0.101 | 0.123 | 0.073 | 0.060 | 0.071 | 0.068 | 0.077 | 0.079 | 0.091 | 0.079 |
| Quad Hard | $\times$ | 5.024 | 0.080 | 0.057 | 0.058 | 0.055 | 0.108 | 0.084 | 0.078 | 0.066 |
| Stairs | $\times$ | 0.118 | 0.057 | 0.189 | 0.121 | 0.036 | 0.061 | 0.039 | 0.042 | 0.041 |
| Underground Easy | 0.155 | 0.125 | 0.047 | 0.037 | 0.050 | 0.047 | 0.058 | 0.047 | 0.048 | 0.048 |
| Underground Medium | 0.153 | 2.649 | 0.043 | 0.047 | 0.053 | 0.040 | 0.062 | 0.041 | 0.040 | 0.041 |
| Underground Hard | 0.140 | 0.194 | 0.054 | 0.067 | 0.052 | 0.051 | 0.100 | 0.051 | 0.053 | 0.053 |
| blenheim-palace-01 | 0.187 | 0.662 | 0.110 | 0.111 | 0.131 | 0.112 | 0.121 | 0.123 | 0.156 | 0.123 |
| blenheim-palace-05 | 0.322 | 0.785 | 0.186 | 0.364 | 0.372 | 0.152 | 0.177 | 0.178 | 0.202 | 0.195 |
| christ-church-03 | 0.091 | 0.586 | 0.015 | 0.046 | 0.014 | 0.015 | 0.022 | 0.016 | 0.015 | 0.018 |
| keble-college-02 | 0.088 | 0.637 | 0.026 | 0.037 | 0.047 | 0.024 | 0.035 | 0.038 | 0.039 | 0.042 |
| keble-college-05 | 0.204 | 1.141 | 0.843 | 0.149 | 0.175 | 0.110 | 0.224 | 0.148 | 0.147 | 0.147 |
| observatory-quarter-01 | 0.136 | 0.710 | 0.297 | 0.080 | 0.058 | 0.055 | 0.099 | 0.082 | 0.087 | 0.085 |
| observatory-quarter-02 | 0.127 | 1.117 | 0.322 | 0.056 | 0.067 | 0.051 | 0.101 | 0.069 | 0.076 | 0.067 |
is externally triggered, providing hardware-level synchronization across all sensors.
We evaluate SA-LIVO on 29 sequences from three public datasets: HILTI 2022 [^5] (15 sequences), the Newer College Dataset (NCD) [^6] (7 sequences), and Oxford Spires [^4] (7 sequences). Baselines are FAST-LIO2 [^8], FAST-LIVO [^37], FAST-LIVO2 [^1], R3LIVE [^2], and SR-LIVO [^40], using official open-source implementations. For all three public datasets, image streams are subsampled to 10 Hz to match the LiDAR scan rate; at higher image rates the LiDAR scan would be split into image-aligned sub-intervals each carrying too few points for stable plane fitting. This preprocessing is applied uniformly to all methods, including the baselines. HILTI uses the official sparse-survey-point evaluator; the remaining datasets are evaluated by absolute pose error (APE) with SE(3) trajectory alignment.
Fig. 6. Handheld data-collection platform used for the self-collected dataset.
### B. Quantitative Accuracy
Table II reports absolute translational RMSE (m); “×” marks a run that did not produce a complete trajectory for the sequence, through either process termination or tracking loss, and such runs are excluded from all reported averages.
在HILTI'22测试中，只有SA-LIVO和FAST-LIVO2能完成全部15个序列；R3LIVE和SR-LIVO在室内退化场景下各有7次失败记录（两者均在exp03、exp09–10、exp15–16、exp18中失败，R3LIVE还在exp11失败，SR-LIVO则在exp14失败）。最难的场景是建筑楼梯（exp03）和两次穹顶场景测试（exp09–10），这些场景下激光雷达和摄像头性能会同时下降：扫描线近乎共面，且墙壁缺乏特征，照明条件也较差。在exp10中，SA-LIVO的误差仅为0.075米，不到FAST-LIVO2的0.218米的三分之一——后者是唯一完成测试的基准算法；而FAST-LIO2、FAST-LIVO、R3LIVE和SR-LIVO均未能通过测试。在其余退化序列（exp15–16、exp18）中，FAST-LIVO的误差达到0.74–1.21米，甚至直接失败，而SA-LIVO的均方根误差始终低于0.13米。
在新学院的场景中，SA-LIVO在两种复杂条件下都表现稳定：一是开阔草坪上的Quad环形路线，此时激光雷达结构稀疏，需依靠视觉信息提供横向观测数据；二是照明不足的地下酒窖场景。在七个测试序列中，它有五个序列排名首位（Quad Easy序列误差0.026米，Quad Hard序列误差0.055米，楼梯序列误差0.036米，地下中等难度序列误差0.040米，地下高难度序列误差0.051米），其余序列的误差也与最佳结果相差不超过0.010米。FAST-LIVO2、R3LIVE和SR-LIVO的表现也较为接近；唯有FAST-LIVO例外（Quad Hard序列误差达5.024米，地下中等难度序列误差为2.649米）。
---
Fig. 7. Qualitative mapping comparison on the indoor-chairs sequence from our self-collected dataset. (a1)–(a2) Two viewpoints of the complete SA-LIVO colored point cloud map; (a3) close-up of the pillar-and-chair region. (b1)–(b4) The same region from FAST-LIVO, FAST-LIVO2, SR-LIVO, and R3LIVE, respectively; SR-LIVO and R3LIVE diverge before the end of the sequence, so their maps are shown up to the point of failure. Point clouds are colorized by camera RGB.
牛津尖塔场景对序列长度要求极高：每次测试都是漫长的室内外穿越过程，不仅容易累积误差，光照条件还会随视角变化反复改变。SA-LIVO在七个序列中有五个排名首位，其余两个序列的误差也与最佳结果相差不到0.002米，整个过程中均方根误差始终低于0.16米。FAST-LIVO在每个序列中的误差都介于0.586到1.141米之间（在keble-college-05场景中误差最大）；FAST-LIVO2、R3LIVE和SR-LIVO在大多数场景下仍具备竞争力。
### C. 消融实验
表II右侧四列分别列出了四种消融变体的各序列均方根误差：去掉视觉更新的变体记为w/o VIO；禁用每帧亮度补偿机制的变体记为w/o affine（$\alpha = 1$，$\beta = 0$）；用简单的信息求和替代子空间感知融合的变体记为w/o sub.；不使用滑动窗口去相关处理、仅采用单帧视觉更新的变体记为w/o s.w.。下文提到的所有平均值均为基于这四列数据得出的数据集均值。没有单一组件能在所有场景下都起主导作用：滑动窗口和子空间融合在HILTI'22场景中最为重要，视觉更新在新学院和牛津尖塔场景中作用显著；若取三个数据集的平均值，去除这些组件会导致性能下降26%–47%。在HILTI'22的走廊和画廊场景中，子空间融合与滑动窗口的影响最为突出。去除子空间融合（w/o sub.）会使HILTI'22的平均均方根误差上升64%（从0.042米增至0.069米），在exp16场景中误差更是达到$3.6 \times$；原本系统的误差为0.069米，去除子空间融合后却增至0.252米：简单的信息求和会在观测不足的方向引入传感器间干扰，而按方向进行门控处理则能限制这种效应。滑动窗口导致的误差增幅在HILTI'22场景中最大（+69%）；在exp18场景中，采用单帧回退机制会使均方根误差从0.022米升至0.127米，因为单个视角的视差信息不足以稳定沿走廊方向的运动。
在New College校区，VIO项的影响最为显著（平均提升67%），这证实了在激光雷达平面结构稀疏的开放户外场景中，视觉约束的重要性最为突出。仿射亮度补偿导致的跨数据集性能下降最小（+26%），但这种效果并不均匀：在New College的固定曝光室内摄像头上仅提升14%，而在HILTI'22数据集上则达到39%；从绝对数值来看，Oxford Spires场景的提升最为明显（+0.019米，范围从0.074米到0.093米），该场景的视点间光照变化最大。消融实验支持了这种分工模式：子空间融合用于解决方向退化问题，滑动窗口机制增加了时间视差信息，VIO在开放环境中提供约束条件，而亮度补偿则在光照变化时稳定光度残差。
### D. 自采集数据集上的建图质量分析
图7展示了我们自采集数据集上的定性建图结果（平台如图6所示）。当传感器靠近中心柱体时，激光雷达几何结构几乎退化，扫描线近乎共面且在切向方向上约束不足。FAST-LIVO和FAST-LIVO2会导致表面模糊，且由于无约束漂移而丢失椅子的结构信息；SR-LIVO和R3LIVE甚至无法完成整个序列的建图。SA-LIVO能够减弱条件不良的激光雷达方向数据，并按比例融入视觉约束，从而生成更密集、更清晰的椅子座位和柱体重建结果。
### E. 运行时间与信息效率分析
点云下采样叶尺寸（HILTI'22数据集为0.2米，其他数据集为0.5米）以及地图体素尺寸（HILTI'22数据集为0.5米，其他数据集为1.0米）直接决定了点数和地图复杂度，进而影响运行时间和内存占用。所有序列的其他参数均保持一致。测量内存时，所有方法均关闭了屏幕可视化功能（RViz）；完整累积地图会保留在进程内存中，因此属于密集存储。
---
TABLE III
PER-SEQUENCE RUNTIME (MS/FRAME) AND PEAK MEMORY RESIDENT SET SIZE (RSS, MB) ACROSS ALL DATASETS. × MARKS A RUN THAT DID NOT PRODUCE A COMPLETE TRAJECTORY FOR THE SEQUENCE, AS DEFINED IN TABLE II; TIME AND RSS COLUMNS ARE OMITTED FOR SUCH RUNS.
| Sequence | FAST-LIVO2 | R3LIVE | SR-LIVO | Ours | Ours (ARM) |
| Time | RSS | Time | RSS | Time | RSS | Time | RSS | Time | RSS |
| exp01_construction_ground_level | 47.6 | 7152 | 43.5 | 8699 | 40.7 | 8873 | 14.2 | 1392 | 34.5 | 1464 |
| exp02_construction_multilevel | 34.5 | 9687 | 41.5 | 17191 | 34.2 | 12949 | 11.9 | 1709 | 28.0 | 1741 |
| exp03_construction_stairs | 14.0 | 2695 | $\times$ | $\times$ | $\times$ | $\times$ | 10.3 | 1149 | 20.3 | 1202 |
| exp04_construction_upper_level | 25.1 | 2485 | 26.3 | 4038 | 27.4 | 3895 | 13.5 | 867 | 27.0 | 927 |
| exp05_construction_upper_level_2 | 23.8 | 2298 | 26.3 | 3767 | 27.1 | 3632 | 13.6 | 862 | 25.2 | 919 |
| exp06_construction_upper_level_3 | 19.5 | 2440 | 22.9 | 4586 | 20.8 | 5129 | 11.6 | 928 | 23.0 | 990 |
| exp07_long_corridor | 10.5 | 1052 | 13.8 | 3860 | 25.5 | 1615 | 8.2 | 822 | 17.9 | 923 |
| exp09_cupola | 10.0 | 3942 | $\times$ | $\times$ | $\times$ | $\times$ | 7.6 | 1443 | 15.8 | 1512 |
| exp10_cupola_2 | 11.8 | 3356 | $\times$ | $\times$ | $\times$ | $\times$ | 8.4 | 1456 | 20.4 | 1490 |
| exp11_lower_gallery | 20.8 | 1811 | $\times$ | $\times$ | 30.9 | 2361 | 10.4 | 1036 | 25.6 | 1097 |
| exp14_basement_2 | 8.9 | 600 | 13.5 | 2289 | $\times$ | $\times$ | 6.9 | 396 | 12.8 | 466 |
| exp15_attic_to_upper_gallery | 14.8 | 2546 | $\times$ | $\times$ | $\times$ | $\times$ | 8.2 | 1387 | 19.4 | 1466 |
| exp16_attic_to_upper_gallery_2 | 15.1 | 1958 | $\times$ | $\times$ | $\times$ | $\times$ | 8.1 | 1126 | 18.3 | 1178 |
| exp18_corridor_lower_gallery_2 | 12.3 | 1312 | $\times$ | $\times$ | $\times$ | $\times$ | 7.4 | 742 | 15.2 | 799 |
| exp21_outside_building | 31.9 | 3020 | 46.0 | 6884 | 33.8 | 6170 | 12.9 | 1185 | 31.9 | 1238 |
| keble-college-02 | 19.7 | 2636 | 30.3 | 14590 | 59.3 | 12885 | 16.5 | 1338 | 44.0 | 1654 |
| keble-college-05 | 25.2 | 4789 | 54.7 | 26898 | 65.5 | 25137 | 19.8 | 2203 | 52.6 | 2474 |
| observatory-quarter-01 | 25.0 | 5623 | 35.8 | 15732 | 60.9 | 16555 | 20.0 | 1877 | 54.6 | 1702 |
| observatory-quarter-02 | 25.8 | 2935 | 99.9 | 29447 | 61.4 | 16385 | 20.2 | 1862 | 56.2 | 1850 |
| blenheim-palace-01 | 19.9 | 3932 | 26.2 | 18887 | 54.8 | 20026 | 15.9 | 2131 | 43.0 | 2547 |
| blenheim-palace-05 | 19.2 | 3350 | 25.6 | 15411 | 53.5 | 17616 | 15.6 | 2000 | 40.9 | 2029 |
| christ-church-03 | 24.1 | 2209 | 34.6 | 14624 | 69.7 | 6880 | 17.4 | 1119 | 45.5 | 1147 |
| quad-easy | 43.8 | 3506 | 72.3 | 16559 | 111.3 | 10475 | 23.9 | 670 | 60.0 | 643 |
| quad-medium | 37.7 | 1718 | 60.6 | 15533 | 112.0 | 10176 | 21.7 | 641 | 53.7 | 656 |
| quad-hard | 31.5 | 2258 | 60.5 | 14913 | 88.5 | 10951 | 17.8 | 691 | 43.0 | 737 |
| stairs | 13.5 | 247 | 21.7 | 6496 | 33.7 | 826 | 6.9 | 576 | 14.0 | 704 |
| ug-easy | 18.0 | 654 | 43.0 | 12447 | 87.4 | 4180 | 11.9 | 548 | 29.1 | 633 |
| ug-medium | 17.7 | 637 | 42.3 | 12610 | 85.7 | 4329 | 11.6 | 536 | 27.7 | 594 |
| ug-hard | 17.6 | 829 | 42.2 | 16222 | 89.6 | 5639 | 11.7 | 644 | 29.3 | 718 |
R3LIVE和SR-LIVO算法中存储的颜色特征图也包含在报告数据中。所有方法均禁用了地图滑动机制，包括SA-LIVO在内，因此所有系统都保留完整的累积地图，所报告的内存占用反映的是地图本身的表现而非滑动窗口的限制。所有报告的SA-LIVO耗时数据均为纯CPU计算的结果；无论是笔记本电脑还是嵌入式ARM平台，均未使用GPU加速。
Fig. 8. Runtime vs. peak memory on the 7 HILTI'22 sequences completed by all methods (log-log; lower-left is better). SA-LIVO is both the fastest and the lowest-memory method on every sequence, by $1.3-3.6\times$ in runtime and $1.3-10.1\times$ in peak memory over the individual baselines ( $2.2-2.6\times$ and $3.6-6.3\times$ on the subset average). Diverged runs are excluded; see Table III.
TABLE IV
表III列出了各数据集上的逐序列详细数据。在HILTI'22数据集上，SA-LIVO每帧处理时间为6.9–14.2毫秒（逐序列平均值），峰值内存占用从未超过
PER-MODULE SHARE OF THE PER-FRAME BUDGET, BY DATASET. COLUMN LABELS MATCH THE METHODOLOGY SUBSECTIONS: (I) SEC. VII-E, (II) SEC. VI-D, (III) SEC. V-G.
| Dataset | (i) Joint InEKF | (ii) Pre-loop Jac. | (iii) Adapt. support | $\Sigma$ |
| HILTI'22 | 46.9% | 12.4% | 12.7% | 72.0% |
| New College | 33.0% | 7.5% | 11.8% | 52.3% |
| Oxford Spires | 39.4% | 8.6% | 17.0% | 65.0% |
1.8 GB，而竞争方法的处理时间最长可达47.6毫秒，内存占用高达17.2 GB。在大型户外序列（如Oxford Spires）中，竞争方法会累积密集的特征图，内存差距进一步拉大：在observatory-quarter-02场景下，SA-LIVO仅占用1.9 GB内存，而R3LIVE则占用29.4 GB内存（$15\times$ 的缩减幅度）。在New College Quad场景中，SR-LIVO每帧处理时间长达112毫秒，而SA-LIVO则低于24毫秒（$4.7\times$ 的速度提升倍数）。在ARM平台上，SA-LIVO能在60毫秒/帧的时间内完成所有序列的处理，内存占用不超过2.5 GB，无需修改算法即可满足嵌入式硬件上100毫秒/10 Hz的实时性要求。
在所有基线算法均能完成运行的通用HILTI'22子集（exp01–exp02、exp04–exp07和exp21）中，SA-LIVO处理一帧的时间为12.3毫秒，比FAST-LIVO2快2.2倍，比SR-LIVO快2.4倍，比R3LIVE快2.6倍，峰值内存占用减少了3.6–6.3倍（见图8）；在同一子集上，ARM平台的平均处理时间为26.8毫秒/帧。
这种效率提升源于三项设计选择。
---
Fig. 9. LIO/VIO information complementarity on the HKU_Cultural_Center_01 sequence (FAST-LIVO2 dataset). (a) Point cloud map with the VIO-leading (Vn), LIO-leading (Ln) and high-gain (Gn) spans marked where they occur. (b) Stacked LIO/VIO contribution fraction (blue = LIO, amber = effective VIO); dashed right axis (log) is the fused information trace. (c) SAIF spectrum: $\sqrt{\lambda_k}$ of $\Lambda_L + q\Lambda_V$ for the six joint eigendirections (k=1, weakest, at bottom). Dark marks directions below $\sigma_{min}$, i.e. attenuated by the gate; hatching marks VIO rescue, where LiDAR alone falls below $\sigma_{min}$ but the joint matrix recovers.
表四按数据集分组，报告了各模块所占的每帧处理时间预算比例。这三个模块合计占每帧预算的52%到72%，这一结果证实优化措施确实针对了最主要的热点路径。
(i) 联合InEKF更新（见第VII-E节）占每帧预算的33%到47%，是占比最大的部分；它将FAST-LIVO2中的两个顺序迭代EKF循环合并为一个。(ii) 利用缓存的激光雷达对应点进行循环前雅可比矩阵组装（见第VI-D节），尽管需要承担完整的LIO和VIO测量成本，但信息形式构建仅占预算的7.5%到12.4%。(iii) 自适应支撑平面关联（见第V-G节）使体素地图更新占预算的11.8%到17.0%，即便地图从HILTI'22数据集扩展到Oxford Spires数据集也是如此；这也是长序列场景下RSS性能优势显著的原因。
这些设计决策按照边际信息产出比例来分配计算资源，从而能以更低的资源消耗实现具有竞争力的精度。
### F. 退化场景分析
图9展示了在HKU_Cultural_Center_01序列中，两种模态在状态空间的互补区域。区间类型由迹分数$\text{tr}(q\Lambda_V)/\text{tr}(\Lambda_L+q\Lambda_V)$划分：高于0.5时为视觉主导区（$\mathbf{V}_n$），低于该值则为激光雷达主导区（$\mathbf{L}_n$）；$\mathbf{G}_n$则表示视觉在激光雷达方向退化时发挥救援作用。
图(c)揭示了两种不同的退化模式。持续退化表现为k=1和k=2方向上的连续深色带，反映出室内平面的几何特征；瞬时退化则体现为$\mathbf{L}n$区间内的实心暗块：这是特定姿态轴上方向信息的短暂崩溃，而非总体信息量偏低所致。
---
Fig. 10. Sensitivity of ATE to the SAIF gate threshold $\sigma_{\min}$ on three HILTI 2022 sequences of increasing LiDAR degeneracy. ATE is normalized by its value at the default $\sigma_{\min}=1$ (dashed line); the shaded band [0.7, 2] marks the safe range on the hardest sequence. The curves pinch at the default and fan out with degeneracy, yet the minimum stays at $\sigma_{\min}=1$ throughout.
G1区间具体体现了跨模态互补性。传感器扫描文化中心的室外立面——一面带有印刷文字和标识的大型平面墙，即现场中的图5所示的正对几何结构，其中k=1行进入了遮挡区域。但图(c)中的VIO救援图案显示，联合矩阵的恢复程度超过了$\sigma_{min}$：摄像头捕捉到的印刷字符和标识提供了激光雷达几何结构无法提供的沿墙约束条件。图(a)中G1位置的重建结果也印证了这一点：墙面上的文字依然清晰可辨。
对比图(b)和图(c)可发现标量融合指标存在根本性局限。图(b)中迹分数看似普通的几个区间，在图(c)中已显示出深色下行区域，这表明存在聚合质量评分无法察觉的方向性信息崩溃现象。
最后，同样的光谱特性也解释了为何该门限无需针对每个场景重新调整。在三个退化程度逐渐加深的HILTI 2022序列上（exp01建筑场景、exp07走廊场景、exp10圆顶场景）将$\sigma_{\min}$扫描两个数量级后，最优值始终保持在默认的$\sigma_{\min}=1$水平（见图10）。
综合来看，上述分析证实了SAIF背后的架构选择：可靠的多模态融合需要在单个方向层面解析信息，而非仅做整体聚合。
### IX. 结论
我们提出了SA-LIVO，这是一种激光雷达-惯性-视觉里程计系统，其关键组件均经过高效性设计。SAIF通过单一阈值按观测方向进行数据融合，从而省去了模式切换和针对各传感器的参数调优步骤，同时还能保证更新过程的稳定性；统一的单环联合InEKF在一个线性化点处融合激光雷达与视觉残差，无论传感器数量多少都能限制迭代次数；而预循环雅可比矩阵缓存、逐观测去相关处理、恒定时间增量式PCA、自适应支撑平面关联算法以及以激光雷达为基准的地图点构建等技术，各自都消除了每帧计算中的主要开销。
在三大资源维度上，SA-LIVO的优势十分明显。在精度方面，SA-LIVO在结构化基准测试中的表现可与现有系统媲美，在R3LIVE和SR-LIVO会出现偏差的退化走廊和穹顶区域，SA-LIVO仍能保持稳定的计算结果。在运行时间方面，在所有基线算法均能处理的HILTI'22子集上，该算法在笔记本CPU上的帧处理时间为12.3毫秒，在无需GPU加速的Jetson Orin上仅为26.8毫秒，速度比同类LIVO系统快2~3倍；在全部29个序列中，其帧处理时间分别控制在23.9毫秒和60.0毫秒以内。在内存占用方面，在同一子集上其峰值RSS占用仅为现有系统的3.6~6.3分之一，在大型户外地图场景下这一比例更是达到15分之一；且无需修改算法，它就能满足嵌入式硬件上10Hz的实时处理要求。下一步的自然举措是用严谨的边缘化处理取代当前的测量值复用策略。通过仅允许在多种传感器共同观测到的方向上使用融合信息，并将计算量限制在与场景复杂度无关的范围内，SA-LIVO将激光雷达与视觉同时观测不足的情况从故障模式转变为可常规处理的运行状态；不过能通过残差门限检测的受损测量数据仍不在SAIF的处理范围内。
## 参考文献
[^1]: C. Zheng, W. Xu, Q. Guo, and F. Zhang, "FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry," IEEE Trans. Robot., vol. 40, pp. 1529–1546, 2024, doi: 10.1109/TRO.2024.3502198.
[^2]: 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. IEEE Int. Conf. Robot. Autom. (ICRA), Philadelphia, PA, USA, May 2022, pp. 10672–10678, doi: 10.1109/ICRA46639.2022.9812253.
[^3]: 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), Xi'an, China, May 2021, pp. 5692–5698, doi: 10.1109/ICRA48506.2021.9561996.
[^4]: Y. Tao et al., “The Oxford Spires Dataset: Benchmarking large-scale LiDAR-visual localisation, reconstruction and radiance field methods,” Int. J. Robot. Res., 2025, doi: 10.1177/02783649251369905.
[^5]: L. Zhang, M. Helmberger, L. F. T. Fu, D. Wisth, M. Camurri, D. Scaramuzza, and M. Fallon, “Hilti-Oxford dataset: A millimetre accurate benchmark for simultaneous localization and mapping,” IEEE Robot. Autom. Lett., vol. 8, no. 1, pp. 408–415, Jan. 2023, doi: 10.1109/LRA.2022.3226077.
[^6]: M. Ramezani, Y. Wang, M. Camurri, D. Wisth, M. Mattamala, and M. Fallon, “The Newer College Dataset: Handheld LiDAR, inertial and vision with ground truth,” in Proc. IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS), Las Vegas, NV, USA, Oct. 2020, pp. 4353–4360, doi: 10.1109/IROS45743.2020.9340849.
[^7]: J. Zhang and S. Singh, "LOAM: Lidar odometry and mapping in real-time," in Proc. Robot.: Sci. Syst. (RSS), Berkeley, CA, USA, Jul. 2014, doi: 10.15607/RSS.2014.X.007.
[^8]: W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, "FAST-LIO2: Fast direct LiDAR-inertial odometry," IEEE Trans. Robot., vol. 38, no. 4, pp. 2053–2073, Aug. 2022, doi: 10.1109/TRO.2022.3141876.
[^9]: J. Zhang, M. Kaess, and S. Singh, “On degeneracy of optimization-based state estimation problems,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), Stockholm, Sweden, May 2016, pp. 809–816, doi:10.1109/ICRA.2016.7487211.
[^10]: J. Zhang and S. Singh, "Laser-visual-inertial odometry and mapping with high robustness and low drift," J. Field Robot., vol. 35, no. 8, pp. 1242–1264, 2018, doi: 10.1002/rob.21809.
[^11]: A. Hinduja, B.-J. Ho, and M. Kaess, “Degeneracy-aware factors with applications to underwater SLAM,” in Proc. IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS), Macau, China, Nov. 2019, pp. 1293–1299, doi: 10.1109/IROS40897.2019.8968577.
[^12]: T. Tuna, J. Nubert, Y. Nava, S. Khattak, and M. Hutter, “X-ICP: Localizability-aware LiDAR registration for robust localization in extreme environments,” IEEE Trans. Robot., vol. 40, pp. 452–471, 2024, doi: 10.1109/TRO.2023.3335691.
## [^13]: J. Lee, R. Komatsu, M. Shinozaki, T. Kitajima, H. Asama, Q. An, and A. Yamashita, “Switch-SLAM: Switching-based LiDAR-inertial-visual SLAM for degenerate environments,” IEEE Robot. Autom. Lett., vol. 9, no. 8, pp. 7270–7277, Aug. 2024, doi: 10.1109/LRA.2024.3421792
---
[^14]: Z. Chen, C. Zheng, J. Wen, X. Zhang, J. Xu, F. Pan, and Y. Cui, "FAST-LIVGO: A degeneracy-robust LiDAR-inertial-visual-GNSS fusion odometry," arXiv:2606.19190, 2026.
[^15]: C. Yuan, W. Xu, X. Liu, X. Hong, and F. Zhang, “Efficient and probabilistic adaptive voxel mapping for accurate online LiDAR odometry,” IEEE Robot. Autom. Lett., vol. 7, no. 3, pp. 8518–8525, 2022, doi: 10.1109/LRA.2022.3185439.
[^16]: I. Vizzo, T. Guadagnino, B. Mersch, L. Wiesmann, J. Behley, and C. Stachniss, "KISS-ICP: In defense of point-to-point ICP – simple, accurate, and robust registration if done the right way," IEEE Robot. Autom. Lett., vol. 8, no. 2, pp. 1029–1036, Feb. 2023, doi: 10.1109/LRA.2023.3236571.
[^17]: 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. (ICRA), Roma, Italy, Apr. 2007, pp. 3565–3572, doi:10.1109/ROBOT.2007.364024.
[^18]: 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), Paris, France, May 2020, pp. 4666–4672, doi: 10.1109/ICRA40945.2020.9196524.
[^19]: 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, 2017, doi: 10.1177/0278364917728574.
[^20]: 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, doi: 10.1109/TRO.2018.2853729.
[^21]: C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. M. Montiel, and J. D. Tardós, “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, doi: 10.1109/TRO.2021.3075644.
[^22]: 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, 2015, doi: 10.1177/0278364914554813.
[^23]: C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” IEEE Trans. Robot., vol. 33, no. 1, pp. 1–21, Feb. 2017, doi:10.1109/TRO.2016.2597321.
[^24]: J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 40, no. 3, pp. 611–625, 2018, doi: 10.1109/TPAMI.2017.2658577.
[^25]: C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi-direct monocular visual odometry,” in Proc. IEEE Int. Conf. Robot. Autom. (ICRA), Hong Kong, China, May 2014, pp. 15–22, doi: 10.1109/ICRA.2014.6906584.
[^26]: J. Engel, T. Schöps, and D. Cremers, "LSD-SLAM: Large-scale direct monocular SLAM," in Proc. Eur. Conf. Comput. Vis. (ECCV), Zürich, Switzerland, Sep. 2014, pp. 834–849, doi: 10.1007/978-3-319-10605-2_54.
[^27]: G. P. Huang, A. I. Mourikis, and S. I. Roumeliotis, “Observability-based rules for designing consistent EKF SLAM estimators,” Int. J. Robot. Res., vol. 29, no. 5, pp. 502–528, 2010, doi: 10.1177/0278364909353640.
[^28]: J. A. Hesch, D. G. Kottas, S. L. Bowman, and S. I. Roumeliotis, "Consistency analysis and improvement of vision-aided inertial navigation," IEEE Trans. Robot., vol. 30, no. 1, pp. 158–176, Feb. 2014, doi: 10.1109/TRO.2013.2277549.
[^29]: A. Barrau and S. Bonnabel, “The invariant extended Kalman filter as a stable observer,” IEEE Trans. Autom. Control, vol. 62, no. 4, pp. 1797–1812, Apr. 2017, doi: 10.1109/TAC.2016.2594085.
[^30]: M. Brossard, A. Barrau, and S. Bonnabel, “Exploiting symmetries to design EKFs with consistency properties for navigation and SLAM,” IEEE Sensors J., vol. 19, no. 4, pp. 1572–1579, Feb. 2019, doi: 10.1109/JSEN.2018.2882714.
[^31]: R. Hartley, M. Ghaffari, R. M. Eustice, and J. W. Grizzle, “Contact-aided invariant extended Kalman filtering for robot state estimation,” Int. J. Robot. Res., vol. 39, no. 4, pp. 402–430, 2020, doi: 10.1177/0278364919894385.
[^32]: P. Shi, Z. Zhu, S. Sun, X. Zhao, and M. Tan, “Invariant extended Kalman filtering for tightly coupled LiDAR-inertial odometry and mapping,” IEEE/ASME Trans. Mechatron., 2023, doi:10.1109/TMECH.2022.3233363.
[^33]: H. Zhang, R. Xiao, J. Li, C. Yan, and H. Tang, "A high-precision LiDAR-inertial odometry via invariant extended Kalman filtering and efficient surfel mapping," IEEE Trans. Instrum. Meas., vol. 73, pp. 1–11, 2024, Art no. 8502911, doi: 10.1109/TIM.2024.3382751.
[^34]: J. Graeter, A. Wilczynski, and M. Lauer, “LIMO: Lidar-monocular visual odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS), Madrid, Spain, Oct. 2018, pp. 7872–7879, doi: 10.1109/IROS.2018.8594394.
[^35]: X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, "LIC-Fusion: LiDAR-inertial-camera odometry," in Proc. IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS), Macau, China, Nov. 2019, pp. 5848–5854, doi: 10.1109/IROS40897.2019.8967746.
[^36]: 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, doi: 10.1109/LRA.2021.3095515.
[^37]: 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. Robot. Syst. (IROS), Kyoto, Japan, Oct. 2022, pp. 4003–4009, doi: 10.1109/IROS47612.2022.9981107.
[^38]: 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), Paris, France, May 2020, pp. 1689–1696, doi: 10.1109/ICRA40945.2020.9196885.
[^39]: D. Wisth, M. Camurri, and M. Fallon, “VILENS: Visual, inertial, lidar, and leg odometry for all-terrain legged robots,” IEEE Trans. Robot., vol. 39, no. 1, pp. 309–326, Feb. 2023, doi: 10.1109/TRO.2022.3193788.
[^40]: Z. Yuan, J. Deng, R. Ming, F. Lang, and X. Yang, “SR-LIVO: LiDAR-inertial-visual odometry and mapping with sweep reconstruction,” IEEE Robot. Autom. Lett., vol. 9, no. 6, pp. 5110–5117, 2024, doi: 10.1109/LRA.2024.3385654.