FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry

Code: https://github.com/hku-mars/FAST-LIVO2

Paper: https://arxiv.org/abs/2408.14035

改进

相比 FAST-LIVO v1 的主要改进:

  1. 引入更鲁棒的光度误差建模，改进像素选择策略，减少动态物体干扰
  2. 支持更多类型LiDAR（Livox、Ouster、Velodyne、Robosense、Hesai等），并优化特征提取速度
  3. 改进时间同步与外参标定流程（ FAST-Calib ）
  4. 引入退化检测与自适应权重调整机制（动态调整视觉/LiDAR约束权重）
  5. 优化数据结构与并行计算，提升高分辨率点云处理速度

框架

多传感器数据预处理

LiDAR：点云去畸变（基于IMU）、时间同步、下采样。

IMU：预积分（Pre-integration），为优化提供初值。

相机：稀疏直接法跟踪（不依赖特征描述子），利用光度误差。

前端（Front-end）

视觉前端：基于稀疏直接法（Sparse Direct Method），跟踪关键帧光度信息，减少特征提取与匹配的计算量。

LiDAR前端：基于点到平面/点到线的几何约束，提取结构化特征（如边缘、平面）。

IMU前端：提供高频运动约束，保证短时精度。

紧耦合优化（Tightly-coupled Optimization）

在同一优化问题中联合优化 位姿、速度、IMU偏置、地图点。

代价函数包含：

视觉光度误差（Photometric Error）

LiDAR几何误差（Geometric Error）

IMU预积分误差

使用滑动窗口（Sliding Window）优化，窗口内保留最近若干关键帧。

地图构建与回环检测

实时构建稠密彩色点云地图（LiDAR点云 + 相机颜色）。

回环检测可选（论文重点在前端与局部优化）。

Sequential Update

在多传感器紧耦合 SLAM 中（LiDAR + IMU + 相机），常见做法是一次性联合优化所有约束（视觉光度误差、LiDAR几何误差、IMU预积分误差）

优点：信息利用最充分，理论上精度最高

缺点：计算量大，尤其是 LiDAR 残差数量远多于视觉残差，导致优化矩阵规模庞大

在退化场景（例如低纹理或稀疏点云）中，某一传感器的约束可能弱化甚至退化，如果直接联合优化，可能会引入数值不稳定

核心思想

Sequential Update 采用 分步优化（Two-stage / Multi-stage Update）将多传感器信息分阶段顺序融合：

  1. 第一阶段：用 LiDAR 测量 与 IMU 传播得到的先验分布融合。IMU 提供了从上一时刻状态传播到当前时刻的预测（先验分布），LiDAR 提供几何约束（点到平面/点到线残差），这一步得到一个更新后的状态分布（后验1）

  2. 第二阶段：在第一步的后验1基础上，再融合相机测量（光度误差），相机提供纹理信息补充，得到最终的后验2（当前时刻的最优状态估计）

数学形式

假设状态向量为： \(\mathbf{x} = [\mathbf{p}, \mathbf{q}, \mathbf{v}, \mathbf{b}_a, \mathbf{b}_g, \dots]\) 其中包含位姿、速度、IMU偏置等

地图表示

  • 统一体素地图： 同一份地图既给 LiDAR 注册提供几何平面约束，又给视觉稀疏对齐提供参考图像块与几何先验（平面法向）——避免双地图不同步与尺度/拓扑不一致问题。

  • 直接法一体化： LiDAR 端直接用点到面残差，视觉端用光度误差；两者都基于同一体素地图构图与查询，减少重复计算与跨模块传参。

  • 服务顺序更新： ESIKF 里先 LiDAR 更新再视觉更新；每次更新后地图及时注入新几何/视觉信息，保证下一步对齐的先验质量与可见性查询效率

数据结构

哈希 + 八叉树： 地图以哈希表管理“根体素”（固定 0.5 m 立方），每个根体素内部用八叉树进一步自适应细化直到叶体素

叶体素即局部平面： 每个叶体素代表一块局部平面，存下列“平面要素”：

  • 中心与法向：平面中心点$q$、法向量$n$
  • 不确定度：$\Sigma_{n,q}$（同时吸收姿态不确定性与点测量噪声）
  • 点集： 体素内的原始 LiDAR 点（用于后续几何更新与视觉点候选）

视觉挂载： 体素中的“可选点”可升级为“视觉地图点”，并附着三层图像块金字塔（典型 8×8/11×11），用于稀疏直接法对齐。已收敛的视觉点只保留参考块；未收敛点可同时保留参考与可见块以便后续筛选与更新

可变叶体素尺度： 不同结构复杂度区域适配不同叶体素大小，从而以尽量少的叶子拟合尽量大的局部平面，提高存储与计算效率

内存管理

局部滑窗（环形缓冲）： 仅维护以当前 LiDAR 为中心、边长为 L 的立方区域；当探测范围触界时，整体“平移”地图窗口并复用移出区域的内存给新进入区域，保证内存上限固定。

每次 ESIKF 更新后检查并执行本地地图平移与内存复用，确保估计与建图的时间一致性与资源可控

几何层

几何构建与维护（LiDAR 主导） 点落体素与注册： LiDAR 更新后将当前扫描点去畸变并投到世界系，按坐标散入哈希根体素/子体素中（不存在则创建）。

新体素建面： 对新体素内点做 SVD 平面性检验；若满足则估计 （$q, n \Sigma_{n,q}$）；若不满足则继续细分为 8 个子体素，直至满足或达到最大层数（如 3 层，届时丢弃该叶体素点，保证平面一致性）。

既有体素更新： 新增点并重检平面性；若破坏平面性则继续细分，否则更新（$q, n \Sigma_{n,q}$）。当平面参数收敛，即记为“成熟平面”，后续新点将被丢弃且参数固定，稳定几何参考并控量。

视觉候选点池：

  • 成熟平面： 仅保留最近的 50 个点作为视觉点候选；
  • 未成熟平面： 全部点可为候选； 以便后续在视觉流程中挑选可见且对齐价值高的点升级为视觉地图点

视觉层

视觉地图点的生成与更新

选择条件： 候选点需满足当前帧可见，且在当前图像上具有足够灰度梯度。

深度优先： 在每个体素的局部平面上，仅保留“同一投影区域里深度最小”的候选，提升遮挡鲁棒性。

网格均匀化： 将图像划分为 30×30 像素栅格；若某格尚无视觉点，就以该格梯度最大的候选生成一个视觉点，并附上当前估计状态（位姿与曝光时间）和该点的 LiDAR 平面法向先验。

块金字塔与触发更新： 每个视觉点附三层同尺寸金字塔块（如 11×11，逐层下采样 2×）；若相邻两次添加跨越超过 20 帧或像素位移超过 40 像素，则为该点再添加一组新块，以覆盖更多视角并增强光度对齐稳定性

参考patch的动态选择

评分目标： 既要与其它patch外观相似，避免选到动态物体；又要“正视”平面，提升纹理分辨率与对齐精度

评分函数： 以 NCC 与视角余弦相结合，并按法向不确定度自适应加权：

\[\mathrm{NCC}(f,g) = \frac{\sum_{x,y} [f(x,y) - \bar{f}]\,[g(x,y) - \bar{g}]}{\sqrt{\sum_{x,y} [f(x,y) - \bar{f}]^2} \; \sqrt{\sum_{x,y} [g(x,y) - \bar{g}]^2}}\] \[c = \frac{\mathbf{n} \cdot \mathbf{p}}{\|\mathbf{p}\|}\] \[\psi_1 = \frac{1}{1 + e^{\mathrm{tr}(\Sigma_n)}}\] \[S = (1 - \psi_1) \cdot \frac{1}{n} \sum_{i=1}^{n} \mathrm{NCC}(f, g_i) + \psi_1 \cdot c\]

规则： 选择 $S$ 最高者作为参考块，用于接下来多层金字塔的稀疏直接对齐

法向细化

直接法对齐需做仿射扭曲；假设“块内等深”会导致透视误差。把 LiDAR 提供的局部平面法向作为几何先验，再用多视角的光度一致性去细化该法向，从而提升稀疏直接法的配准精度与稳定性。它直接服务于视觉块的仿射扭曲与光度误差建模，是连接几何与光度两条信息链的关键桥梁

  • 核心目标: 细化每个视觉地图点所依附的局部平面法向，使块级仿射扭曲更符合真实透视几何，降低光度残差。

  • 动机来源: 仅用“块内等深”的假设会在视差较大、斜视角或有平面倾角时产生系统误差；利用 LiDAR 平面先验 + 视觉光度一致性可显著提升直接法对齐质量。

  • 使用位置: 在相机更新阶段，针对可见的视觉地图点，以固定参考块对多个观测块做光度对齐，并联合优化法向（和少量块内尺度/亮度仿射）

思路要点：

  • 仿射扭曲矩阵： 由参考帧与目标帧的相对位姿、平面法向与块中心点共同决定；
  • 重参数化： 将法向 $n$ 投影为一个无约束的 2 维变量进行最小二乘优化，降低计算负担；
  • 异步执行： 细化在独立线程进行，稳定后固定参考块与法向并清理冗余块，保证前端里程计的实时性
  • 触发与选择: 仅对“参考块 + 多观测块”数量充足、且法向协方差较大（不确定）或光度残差显著的视觉点触发细化，保证算力集中在收益高的点上。
  • 一致性维护: 若法向更新幅度大，重评估该体素的平面一致性与不确定度；必要时触发体素细分或合并，保持几何-光度模型的一致与稳定。
  • 失败与回退: 若优化出现退化（条件数大、步长爆炸、鲁棒核下有效像素过少），放弃更新并保留 LiDAR 先验；统计多次失败可降低该点的参与权重或移除其参考块

一旦平面正常收敛，该视觉地图点的参考贴片和正常向量就可以固定，而无需进一步细化，所有其他贴片均被删除

优点

  • 一套地图，多模态通用： LiDAR 的几何与视觉的光度统一在同一结构里，避免跨图冗余与对齐误差积累。

  • 计算受控且鲁棒： 自适应体素“只留平面”，成熟平面冻结；环形缓冲限内存；视觉点网格均匀且按需补块，稳定收敛。

  • 几何先验喂给视觉： LiDAR 的平面法向直接作为视觉扭曲的几何先验，并可由视觉再细化，信息闭环、互补增益

雷达测量噪声模型

机械式或固态 LiDAR 的激光束具有有限发散角（beam divergence），光斑在远处会变大，导致：测距方向（径向）噪声随距离增加而增大、横向方向（垂直于光束）噪声受光斑尺寸影响，也随距离增加。如果不建模这种各向异性噪声，优化时会错误地信任远距离点，导致配准偏差。

噪声

光斑尺寸

假设激光束发散半角为 $\theta_d$（通常在 0.1–0.5 mrad 量级），在距离 $R$ 处的光斑直径近似为：

\[D_{\text{spot}}(R) \approx 2 R \cdot \tan\left(\frac{\theta_d}{2}\right) \approx R \cdot \theta_d\]

径向测距噪声

由时间飞行测距精度决定，通常与反射率、信噪比相关，可近似为常数或随距离缓慢增加

横向噪声

由光斑覆盖范围和表面法向引起，近似与光斑半径成比例: \(\sigma_t(R) \approx \frac{D_{\text{spot}}(R)}{2\sqrt{3}} \quad (\text{假设均匀分布取标准差})\)

各向异性协方差建模

在 LiDAR 光束坐标系下（z 轴沿测距方向）：

\[\Sigma_{\text{beam}}(R) = \begin{bmatrix} \sigma_t^2(R) & 0 & 0 \\ 0 & \sigma_t^2(R) & 0 \\ 0 & 0 & \sigma_r^2(R) \end{bmatrix}\]

其中:

  • $\sigma_r(R)$：径向测距噪声（通常 mm 级）
  • $\sigma_t(R)$：横向噪声，随 $R$ 线性增长

如果当前 LiDAR 位姿为 $\mathbf{T}_{WL}$，则点的测量噪声协方差在世界系下为：

\[\Sigma_W = \mathbf{R}_{WL} \, \Sigma_{\text{beam}}(R) \, \mathbf{R}_{WL}^\top\]

其中 $\mathbf{R}_{WL}$ 是 LiDAR 到世界的旋转矩阵

在优化中的使用

残差加权：在点到平面残差 $r = \mathbf{n}^\top \mathbf{p}_W + d$ 中，权重可由测量协方差推导：

\[w = \frac{1}{\mathbf{n}^\top \Sigma_W \mathbf{n}}\]

这样，远距离、横向不确定性大的点会自动降低权重。

  • 鲁棒性提升：结合鲁棒核（Huber/Cauchy）进一步抑制外点。
  • 多特征融合：边缘点、平面点都可用该协方差模型计算权重

稀疏直接视觉测量

视觉测量模型是 ESIKF顺序更新 的第二步，用于在 LiDAR 更新之后，通过稀疏直接法（Sparse Direct Method）利用图像光度信息进一步优化状态。
核心思想：

  • 不提取 ORB/FAST 等特征点，直接利用 LiDAR 地图点作为视觉地图点（Visual Map Points）
  • 在当前帧与参考帧之间做稀疏图像块（patch）光度对齐
  • 最小化光度误差直接优化相机位姿（以及曝光时间等参数）

视觉地图点选择（Visual Map Point Selection）

  1. 可见体素查询（Visible Voxel Query）:通过当前 LiDAR 扫描击中的体素快速筛选可能在相机视野内的地图点, 结合上一帧可见的地图点所在体素，利用相邻帧视野重叠性提高召回率,得到初步视觉子地图（Visual Submap）

  2. 按需光线投射（On-demand Raycasting）:解决 LiDAR 近距离盲区或相机 FoV 与 LiDAR FoV 不完全重叠的问题. 将图像划分为 30×30 像素网格，对未被地图点覆盖的网格中心像素向场景中发射射线, 在深度范围 [d_min, d_max] 内采样，找到包含地图点的体素并加入子地图

  3. 外点剔除（Outlier Rejection）

    • 遮挡剔除：同一网格只保留深度最近的点
    • 深度不连续剔除：与当前 LiDAR 深度图比较，若邻域深度差异过大则剔除
    • 视角约束：参考 patch 或当前 patch 的视角与法向夹角 > 80° 则剔除

稀疏直接视觉测量模型

Sparse-Direct Visual Measurement Model

光度一致性假设

对于一个视觉地图点 $ Gp_i $，在真实位姿下，它在当前帧 $ I_k $ 与参考帧 $ I_r $ 的投影 patch 应满足光度一致性（考虑曝光时间差）：

\[\tau_k \cdot I_k(u_i + \Delta u) - \tau_r \cdot I_r(u'_i + A^r_i \Delta u) = 0\]

其中：

  • $ \tau_k, \tau_r $：当前帧与参考帧的逆曝光时间
  • $ u_i $：地图点在当前帧的像素中心
  • $ u’_i $：地图点在参考帧的像素中心
  • $ A^r_i $：由平面先验计算的仿射变换矩阵（Affine Warping）
  • $ \Delta u $：patch 内相对中心的像素偏移

仿射变换（Affine Warping）

  • 假设地图点所在局部平面法向为 $ n $，中心点为 $ p $
  • 由 LiDAR 平面先验（可进一步优化）计算 $ A^r_i $，将参考 patch 像素坐标映射到当前帧
  • 支持多种相机模型（针孔、鱼眼等），通过投影矩阵 $ P $ 与反投影矩阵 $ P^{-1} $ 实现

逆组合公式（Inverse Compositional Formulation）

将位姿增量 $\delta T$ 从当前帧投影位置 $ u_i $ 转移到参考帧位置 $ u’_i $，迭代优化时，雅可比矩阵只需计算一次，大幅减少计算量

曝光时间估计

在优化中同时估计当前帧的逆曝光时间 $\tau_k$，第一帧固定 $\tau_0 = 1$ 作为基准，用于抵消光照变化对光度一致性的影响

多层金字塔优化

在三层图像金字塔上逐层优化（由粗到细），每层迭代收敛后，将结果作为下一层的初值

残差与雅可比

  • 残差：参考 patch 与当前 patch 对应像素的光度差（考虑曝光时间）
  • 状态变量：
    • 当前帧位姿（6 自由度）
    • 曝光时间（1 自由度）
  • 雅可比：
    • 对位姿的导数：通过链式法则从像素坐标对位姿的偏导得到
    • 对曝光时间的导数：直接由光度方程求偏导

FAST-LIVO2 on Resource-Constrained Platforms: LiDAR-Inertial-Visual Odometry with Efficient Memory and Computation

Paper: https://arxiv.org/abs/2501.13876

总体框架变化

模块 FAST‑LIVO (v1) FAST‑LIVO2 改进意义
融合框架 误差状态迭代卡尔曼滤波（ESIKF），LiDAR 点到面残差 + 视觉光度残差 + IMU 先验，批量更新 仍用 ESIKF，但改为顺序更新（Sequential Update）：先 LiDAR 再视觉 顺序更新减少一次性构造大雅可比矩阵的开销，视觉可按需跳过，节省计算
地图结构 LiDAR 全局地图 + 视觉全局地图（视觉点附着在 LiDAR 点上，但管理上分离） 统一 LiDAR‑视觉本地地图（Unified Local Map），直接在同一数据结构中管理几何与光度信息 避免跨地图数据同步，减少冗余存储，提高查询效率
视觉更新策略 固定频率使用视觉帧（例如每帧或固定间隔） 退化感知 + 自适应视觉帧选择（LiDAR 退化时用更多视觉帧，正常时稀疏用） 在 LiDAR 约束足够时减少视觉计算，退化时增强视觉约束，兼顾效率与鲁棒性
退化检测 无专门退化检测 基于 LiDAR 平面法向的奇异值分解（SVD）归一化最小奇异值 $\tilde{\sigma}_{\min}$ 检测退化 让系统能动态调整视觉参与度，应对长廊、平面等退化场景
地图滑动策略 大尺寸本地地图，低频滑动 小尺寸本地地图，高频滑动 + 长期视觉地图（Long-term Visual Map）存储历史视觉点 减少本地地图内存占用，长期地图保证长时间回访时的视觉约束

ESIKF 顺序更新

  • v1：LiDAR 和视觉残差一次性堆叠更新，视觉模块每帧都运行。
  • v2：先用 LiDAR 残差更新，再用视觉残差更新；视觉模块可按退化状态跳过。
  • 好处：避免大规模矩阵运算、视觉计算量显著下降（在 Hilti 数据集上视觉部分耗时减少 80%+）

LiDAR 退化检测

方法：

  1. 从当前 LiDAR 扫描提取平面法向集合
  2. 计算 $n_{\text{est}} n_{\text{est}}^\mathsf{T}$ 的 SVD
  3. 归一化奇异值向量 $[\tilde{\sigma}{\min}, \tilde{\sigma}{\text{mid}}, \tilde{\sigma}_{\max}]$
  4. 若 $\tilde{\sigma}_{\min}$ 连续多帧低于阈值（如 0.07），判定退化

用途：决定视觉帧选择策略、阈值可跨 LiDAR 型号通用

自适应视觉帧选择

规则：

  • 退化时：用所有可用图像（最大化视觉约束）
  • 正常时：按位姿变化阈值 $\tau$ 选取关键帧
  • 阈值自适应缩放：$ \tau = \sqrt{3} \cdot \tilde{\sigma}{\min} \cdot \tau{\text{predefined}} $

效果：

  • LiDAR 约束强 → 视觉帧稀疏，节省计算
  • LiDAR 退化 → 视觉帧密集，防止漂移

地图结构优化

v1：全局 LiDAR 地图（ikd-tree）+ 全局视觉地图（体素哈希）

v2：

  • 统一本地地图：三层八叉树存储平面特征，部分点附带三层 patch 金字塔
  • 长期视觉地图：稀疏存储历史视觉点，低内存占用
  • 滑动策略：
    • 本地地图边长小（如 200m），高频滑动
    • 超出范围的视觉点转移到长期地图
    • 长期地图边长大（如 800m），低频滑动