FAST-LIVO: Fast and Tightly-coupled Sparse-Direct LiDAR-Inertial-Visual Odometry

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

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

FAST-LIVO 是一套“稀疏-直接”的紧耦合 LiDAR-IMU-视觉里程计系统。它把两条子系统（直接LIO与稀疏直接VIO）用同一个误差态迭代卡尔曼滤波器（ESIKF）在量测层面融合：LIO用原始点云做帧-到-地图的点到平面配准，VIO复用由 LIO 构建的点云地图并为点附着图像小块（patch），在新图像上做稀疏直接光度对齐，无需提取/匹配任何图像特征。

架构

  • 【核心结构】系统包含一条直接 LIO 与一条稀疏直接 VIO。LIO 对新扫的原始点先做运动畸变补偿，再以帧-到-地图方式计算点到平面残差；VIO 从“视觉全局地图”中取出当前视场（FoV）的子图，做遮挡/深度不连续剔除后，用附着在地图点上的参考 patch 与当前图像进行稀疏直接对齐，最小化光度误差。

  • 【紧耦合融合】LIO 的几何残差与 VIO 的光度残差与 IMU 的前向传播一起，统一进入误差态迭代卡尔曼滤波器。更新后的位姿再用于将新点并入全局地图并同步更新视觉地图的 patch 信息。

  • 【复用与高效】VIO 直接复用 LIO 已经构建的点地图并为点附上图像 patch，因此避免了图像特征的检测、匹配、三角化与滑窗优化，降低计算量并在量测层耦合两类传感器。

  • 【稳健性设计】提出了一个鲁棒的外点剔除：将地图点投影到当前图像网格，仅保留每格最小深度点，并用最近一帧 LiDAR 投影检查遮挡，剔除被遮挡与深度不连续点，从而显著提高视觉对齐的稳定性。

状态转移模型

FAST-LIVO 在 IMU 机体系 $I$ 下定义状态向量 $x$为：

\[x \in \mathcal{M} = SO(3) \times \mathbb{R}^{15}, \quad \dim(\mathcal{M}) = 18\] \[x = \begin{bmatrix} {}^{G}\!R_{I} \\ {}^{G}\!p_{I} \\ {}^{G}\!v_{I} \\ b_g \\ b_a \\ {}^{G}\!g \end{bmatrix}\]

其中:

  • $ {}^{G}!R_{I} \in SO(3) $: IMU 在全局坐标系 $G$ 下的姿态
  • $ {}^{G}!p_{I} \in R^3 $: IMU 在全局坐标系下的位置
  • $ {}^{G}!v_{I} \in R^3 $: IMU 在全局坐标系下的速度
  • $ b_g, b_a $: 陀螺仪和加速度计的零偏
  • $ {}^{G}!g $: 全局重力向量

离散时间状态转移模型（IMU 采样周期为 $\Delta t$）为：

\[x_{i+1} = x_i \ \boxplus \ \left( \Delta t \cdot f(x_i, u_i, w_i) \right)\]

其中：

  • $ u_i $: IMU 原始测量（角速度、加速度）
  • $ w_i $: 过程噪声
  • $ \boxplus $: 表示在流形上的状态增量运算（姿态部分用指数映射，向量部分直接相加）

动力学函数 $ f(\cdot) $:

\[f(x, u, w) = \begin{bmatrix} {}^{G}\!R_{I} \cdot \left( \omega_m - b_g - n_g \right) \\ {}^{G}\!v_{I} \\ {}^{G}\!R_{I} \cdot \left( a_m - b_a - n_a \right) + {}^{G}\!g \\ n_{bg} \\ n_{ba} \\ \mathbf{0}_{3\times 1} \end{bmatrix}\]

其中：

  • 姿态部分通过角速度积分更新
  • 位置通过速度积分更新
  • 速度由加速度测量（去偏置、去噪声）加上重力更新
  • 零偏按随机游走模型更新
  • 重力向量假设恒定

前向传播

在 IMU 传播阶段，假设无噪声 $ w_i=0 $, 状态预测为：

\[x_{i+1} = x_i \ \boxplus \ \left( \Delta t \cdot f(x_i, u_i, 0) \right)\]

协方差预测:

\[P_{i+1} = F_x \, P_i \, F_x^\top + F_w \, Q \, F_w^\top\]

其中:

  • $ F_x = \frac{\partial x_{i+1}}{\partial x_i} $: 状态转移雅可比
  • $ F_w = \frac{\partial x_{i+1}}{\partial w_i} $: 噪声输入雅可比
  • $ Q $: 过程噪声协方差矩阵

状态预测与协方差从时间点$t_{k−1}$（接收最后一次激光雷达或图像测量值时）传播至时间点$t_k$（接收当前激光雷达或图像测量值时），期间每接收一次IMU测量值$u_i$即进行一次传播

状态预测与协方差中的初始状态与协方差分别为$x_{k−1}$和$P_{k−1}$，由融合最后一次激光雷达或图像测量数据获得。

模型未假设激光雷达扫描与图像接收同步。当激光雷达扫描或图像抵达时，系统状态根据其他机制进行更新。

流形运算

对于旋转部分： \(R_a \ \boxplus \ r = R_a \cdot \mathrm{Exp}(r), \quad r \in \mathbb{R}^3\)

\[R_a \ \boxminus \ R_b = \mathrm{Log}(R_b^\top R_a)\]

其中 $ \mathrm{Exp}(\cdot) $ 和 $ \mathrm{Log}(\cdot) $ 分别是 $SO(3)$ 上的指数映射与对数映射（Rodrigues 公式）

对数映射

输入：旋转向量 $r \in \mathbb{R}^3$

输出：旋转矩阵 $R \in SO(3)$

\[\mathrm{Exp}(\boldsymbol{r}) = \mathbf{I}_{3} + \frac{\sin\theta}{\theta} \, \hat{\boldsymbol{r}} + \frac{1 - \cos\theta}{\theta^2} \, \hat{\boldsymbol{r}}^2\]

其中 \(\theta = \|\boldsymbol{r}\|, \quad \hat{\boldsymbol{r}} = \begin{bmatrix} 0 & -r_3 & r_2 \\ r_3 & 0 & -r_1 \\ -r_2 & r_1 & 0 \end{bmatrix}\)

当 $\theta \to 0$ 时，使用泰勒展开近似： \(\mathrm{Exp}(\boldsymbol{r}) \approx \mathbf{I}_{3} + \hat{\boldsymbol{r}}\)

指数映射

输入：旋转矩阵 $R \in SO(3)$

输出：旋转向量 $r \in \mathbb{R}^3$

\[\mathrm{Log}(\mathbf{R}) = \frac{\theta}{2\sin\theta} \left( \mathbf{R} - \mathbf{R}^\top \right)^\vee\]

其中:

\[\theta = \cos^{-1} \left( \frac{\mathrm{trace}(\mathbf{R}) - 1}{2} \right)\]

符号 $(\cdot)^\vee$ 表示从反对称矩阵 $\hat{\boldsymbol{r}}$ 恢复向量 $\boldsymbol{r}$ 的运算：

\[\begin{bmatrix} 0 & -r_3 & r_2 \\ r_3 & 0 & -r_1 \\ -r_2 & r_1 & 0 \end{bmatrix}^\vee = \begin{bmatrix} r_1 \\ r_2 \\ r_3 \end{bmatrix}\]

测量模型

符号与刚体变换定义

使用以下坐标系：全局 $G$，IMU/机体 $I$，LiDAR $L$，相机 $C$。定义：

\[{}^{A}\!T_{B} \,=\, \begin{bmatrix} {}^{A}\!R_{B} & {}^{A}\!p_{B} \\ \mathbf{0}^\top & 1 \end{bmatrix}, \quad {}^{A}\!R_{B}\in SO(3), \; {}^{A}\!p_{B}\in\mathbb{R}^3,\]

表示 $B$ 坐标系在 $A$ 坐标系下的位姿。

IMU 在全局系下的位姿为 ${}^{G}!T_{I}$，LiDAR 与相机的外参分别为 ${}^{I}!T_{L}$ 与 ${}^{I}!T_{C}$。

反对称矩阵（skew）运算定义为：

\[\mathrm{skew}(v) \triangleq \begin{bmatrix} 0 & -v_3 & v_2 \\ v_3 & 0 & -v_1 \\ -v_2 & v_1 & 0 \end{bmatrix}\]

LiDAR 帧到地图测量（点到平面约束）

LiDAR测量通过将当前帧点云与全局地图中的局部平面进行匹配，形成点到平面的残差。这种约束能直接反映几何对齐误差，是LIO的核心观测模型。

LiDAR 全局图：采用 ikd-Tree 做增量点云图

前向模型

对于经过运动畸变补偿的 LiDAR 点 ${}^{L}!p_j$，其在全局系下的坐标为：

\[{}^{G}\!p_j \,=\, {}^{G}\!R_{I,k}\,\bigl({}^{I}\!R_{L}\,{}^{L}\!p_j + {}^{I}\!p_{L}\bigr) + {}^{G}\!p_{I,k}\]

假设在全局地图中找到的局部平面法向量为 $u_j$，平面上一点为 $q_j$，则点到平面残差为：

\[r_{\ell,j}(x_k;\,{}^{L}\!p_j) \,=\, u_j^\top\bigl({}^{G}\!p_j - q_j\bigr) \,\approx\, 0\]

加权目标函数

对所有选中的点，LiDAR 残差的加权最小二乘形式为：

\[\mathcal{L}_{\ell}(x_k) \,=\, \sum_{j\in\mathcal{J}_{\ell}} w_{\ell,j}\, r_{\ell,j}(x_k)^2, \quad w_{\ell,j} \,=\, \frac{1}{\sigma_{\ell,j}^2}\]

其中 $\sigma_{\ell,j}$ 为测量噪声标准差，$w_{\ell,j}$ 为权重。

线性化与雅可比

在误差态卡尔曼滤波中，需要对残差进行一阶线性化：

\[\delta r_{\ell,j} \,=\, \underbrace{\bigl(-u_j^\top\,{}^{G}\!R_{I,k}\,\mathrm{skew}(s_j)\bigr)}_{H_{\ell,j}^{(\theta)}} \delta\theta \;+\; \underbrace{u_j^\top}_{H_{\ell,j}^{(p)}} \delta p,\]

其中 $s_j = {}^{I}!R_{L}\,{}^{L}!p_j + {}^{I}!p_{L}$

视觉帧到地图测量（稀疏直接光度约束）

视觉测量直接利用地图点的参考图像patch与当前图像进行光度对齐，避免特征提取与匹配，直接最小化像素灰度差。

patch 是指从某一帧参考图像中，以地图点在该帧的投影位置为中心，裁剪出来的一个小的图像区域（例如 8*8 像素），并且通常会构建一个 金字塔（pyramid） 版本，用于多尺度配准

patch 来自 视觉全局地图（Visual Global Map） 中的地图点。每个地图点可能被多个历史图像观测过，因此会存储多个 patch（及其金字塔）和对应的相机位姿

存储内容：

  • patch 图像数据（灰度值）
  • patch 金字塔（多分辨率版本）
  • 采集该 patch 时的相机位姿

对于当前帧 FoV 内的每个地图点：从该点存储的多个 patch 中，选择与当前相机视角夹角最接近的那一个作为参考 patch $Q_i$. 这一步得到的点数可能很多，尤其是地图密集时. 为了保证直接法的稳定性和计算效率，对 FoV 内的点做进一步筛选:

  1. 遮挡剔除: 将地图点投影到当前图像的像素网格（例如 $40 \times 40$ 像素一格）, 每个网格只保留深度最近的一个点，其他点视为被遮挡或冗余

  2. 深度不连续剔除: 利用最近一帧 LiDAR 投影检查深度在 9×9 邻域比对，遮挡点与深度不连续点剔除，即当前点在局部邻域内被 LiDAR 点“挡住”

  3. 梯度与稳定性筛选：对新增地图点，会优先选择图像梯度较大的像素位置（纹理丰富），避免低纹理区域导致光度对齐不稳定

为快速获得“当前相机视场内的地图点”，系统用体素哈希管理视觉全局图，再用“最新一帧 LiDAR 扫描点所在体素”作为候选，随后进行 FoV 检查得到视觉子图，减少搜索开销

结构

对于保留下来的每个地图点，系统会绑定一个参考 patch（来自历史帧）。在当前图像中，将该地图点投影到像素平面，取出对应位置的 patch 区域，与参考 patch 做光度对齐，计算残差。一个 patch 对应一个地图点，不会出现一个 patch 内混合多个地图点的情况

视觉全局图：本质是“LiDAR 地图点 + 多个patch小块金字塔 + 其观测位姿”。帧对齐后对高残差点或视差较大的点补充新 patch，以维持覆盖与视角多样性。新图像还会在 40×40 网格内选取“投影梯度最大”的 LiDAR 点作为新增视觉点，跳过高曲率“边缘点”以避免不稳定贴图

投影模型

当前相机位姿为

\[{}^{G}\!T_{C,k} = {}^{G}\!T_{I,k}\,{}^{I}\!T_{C}\]

地图点 ${}^{G}!p_i$ 在相机系下的坐标为：

\[{}^{C}\!p_{i,k} \,=\, {}^{C}\!R_{G,k}\,{}^{G}\!p_i + {}^{C}\!p_{G,k}\]

针孔投影模型为：

\[\pi({}^{C}\!p_{i,k}) \,=\, \begin{bmatrix} f_x\,X_i/Z_i + c_x \\ f_y\,Y_i/Z_i + c_y \end{bmatrix}\]

光度残差

对于地图点 $i$ 的参考 patch $Q_i$，当前图像 $I_k$，光度残差为：

\[r_{c,i}(\mathbf{u};x_k, a_i, b_i) \,=\, I_k\!\bigl(\pi({}^{C}\!p_{i,k}) + \mathbf{u}\bigr) - \bigl(a_i\,Q_i(\mathbf{u}) + b_i\bigr),\]

其中 $(a_i,b_i)$ 为亮度仿射补偿参数，$\mathbf{u}$ 为 patch 内像素偏移

加权目标函数

所有点和像素的加权光度误差为

\[\mathcal{L}_{c}(x_k, \{a_i,b_i\}) \,=\, \sum_{i\in\mathcal{J}_{c}} \sum_{\mathbf{u}\in\Omega_i} w_{c,i}(\mathbf{u})\, r_{c,i}(\mathbf{u};x_k, a_i, b_i)^2\]

线性化与雅可比

光度残差对位姿和亮度参数的偏导为：

\[\delta r_{c,i}(\mathbf{u}) \,=\, H_{c,i}^{(\theta)}(\mathbf{u})\,\delta\theta \;+\; H_{c,i}^{(p)}(\mathbf{u})\,\delta p \;+\; H_{c,i}^{(a)}(\mathbf{u})\,\delta a_i \;+\; H_{c,i}^{(b)}(\mathbf{u})\,\delta b_i,\]

其中：

\[H_{c,i}^{(a)}(\mathbf{u}) = -Q_i(\mathbf{u}), \quad H_{c,i}^{(b)}(\mathbf{u}) = -1.\]

统一的帧到地图优化目标

将IMU先验、LiDAR几何残差、视觉光度残差统一到一个最小化问题中，在误差态空间中迭代求解，实现紧耦合融合。给定 IMU 先验，统一的优化问题为：

\[\min_{\delta x_k,\,\{a_i,b_i\}} \;\; \|\delta x_k\|_{P_k^{-1}}^2 \;+\; \sum_{j\in\mathcal{J}_{\ell}} w_{\ell,j}\, r_{\ell,j}(x_k \boxplus \delta x_k)^2 \;+\; \sum_{i\in\mathcal{J}_{c}} \sum_{\mathbf{u}\in\Omega_i} w_{c,i}(\mathbf{u})\, r_{c,i}(\mathbf{u}; x_k \boxplus \delta x_k, a_i, b_i)^2\]

地图表示

在每次 ESIKF 更新完状态后，维护一份“带图像信息的 LiDAR 地图”，供下一帧视觉直接法对齐使用

核心目标：让视觉子系统（VIO）能直接在 LiDAR 地图上找到可见的、带参考图像 patch 的 3D 点，从而用光度误差做稀疏直接配准

关键思想：LiDAR 地图点是几何基准，视觉地图是在其上附加多视角图像 patch 的“增强版”地图

地图更新

输入：

  • 当前优化后的位姿：（全局到当前相机）
  • 当前图像帧（灰度或彩色）
  • 最新一帧 LiDAR 扫描点云（已去畸变、配准到全局）
  • 现有视觉全局地图（包含地图点、patch 金字塔、多视角信息）
[ESIKF更新位姿]
       ↓
[选取可见地图点] → [遮挡/深度剔除] → [更新已有点patch]
       ↓
[选取新LiDAR点] → [梯度筛选+曲率剔除] → [添加新视觉点]
       ↓
[更新空间索引 & 内存管理]
       ↓
[输出更新后的视觉全局地图]

选择地图点

  1. 体素哈希/八叉树索引：快速定位当前相机视场内的地图点
  2. FoV 检查：剔除不在相机视锥内的点
  3. 遮挡剔除：将候选点投影到当前图像平面，按 40×40 像素网格保留深度最近的一个点。用最新 LiDAR 扫描投影深度在 9×9 邻域比对，剔除被遮挡或深度跳变的点。

更新已有地图点的图像 patch

  1. 对于当前可见的地图点，检查它是否已有与当前视角相近的参考 patch
  2. 如果视角差异较大（例如视线夹角超过阈值），则从当前图像中截取该点的 patch（多层金字塔，通常 3 层），存入该地图点的 patch 列表，作为新的参考视角

这样保证地图点在不同视角下都有可用的参考 patch，提升直接法匹配的鲁棒性

添加新的视觉地图点

  1. 在当前帧的 LiDAR 扫描点中，投影到图像平面。
  2. 在每个 40×40 像素网格内，选择图像梯度最大的 LiDAR 点（保证光度信息丰富）。
  3. 跳过高曲率的“边缘点”（避免 patch 光度不稳定）。
  4. 为这些新点创建视觉地图点：存储全局 3D 坐标、附加当前帧的 patch 金字塔、记录当前帧位姿作为参考视角

维护地图结构

空间索引更新：将新点插入视觉地图的体素哈希结构，保证后续查询高效。

内存管理：可设置最大 patch 数量，超出时按时间或质量淘汰旧 patch。

同步 LiDAR 地图：视觉地图点的几何位置直接引用 LiDAR 全局地图的点，保证几何一致性。

ESIKF

Error-state Iterated Kalman Filter Update 误差状态迭代卡尔曼滤波

目标是 MAP：把 IMU 的先验、LiDAR 的几何残差、视觉的光度残差联合最小化；高斯-牛顿迭代与 iterated EKF 等价，迭代在切空间中进行以满足流形约束，收敛解作为下一时段 IMU 传播的起点，同时用于更新两类地图

在流形状态空间上进行紧耦合多传感器估计。设系统名义状态为：

\[x \in \mathcal{M} = SO(3) \times \mathbb{R}^{3} \times \mathbb{R}^{3} \times \mathbb{R}^{3} \times \mathbb{R}^{3},\] \[x \triangleq \{ R,\, \mathbf{p},\, \mathbf{v},\, \mathbf{b}_g,\, \mathbf{b}_a \},\]

其中：

  • $R \in SO(3)$ ：全局姿态
  • $\mathbf{p}, \mathbf{v} \in \mathbb{R}^3$：位置与速度
  • $\mathbf{b}_g, \mathbf{b}_a \in \mathbb{R}^3$：陀螺与加计偏置

误差状态采用最小维欧式表示：

\[\delta x \triangleq \begin{bmatrix} \delta\boldsymbol{\theta} & \delta\mathbf{p} & \delta\mathbf{v} & \delta\mathbf{b}_g & \delta\mathbf{b}_a \end{bmatrix}^\mathsf{T} \in \mathbb{R}^{15}.\]

ESIKF 流程：

  1. IMU 高频传播形成先验
  2. 测量线性化并进行卡尔曼更新
  3. 流形组合名义状态与误差增量
  4. 重新线性化并迭代直到收敛

状态表示与误差映射

流形组合与差分

姿态采用左扰动模型：

\[R \boxplus \delta\boldsymbol{\theta} \triangleq R\, \mathrm{Exp}([\delta\boldsymbol{\theta}]_\times),\] \[R_2 \boxminus R_1 \triangleq \mathrm{Log}(R_1^{-1} R_2).\]

欧氏分量： \(\mathbf{p} \boxplus \delta\mathbf{p} = \mathbf{p} + \delta\mathbf{p}, \quad \mathbf{v} \boxplus \delta\mathbf{v} = \mathbf{v} + \delta\mathbf{v}.\)

误差重置与协方差映射

一次更新后： \(P \leftarrow \Gamma\, P\, \Gamma^\mathsf{T},\)

\[\Gamma \approx \mathrm{blkdiag}\!\left( I_3 - \frac{1}{2}[\delta\boldsymbol{\theta}]_\times,\, I_3,\, I_3,\, I_3,\, I_3 \right).\]

IMU 传播（先验）

连续时间模型： \(\dot{R} = R[\boldsymbol{\omega}_m - \mathbf{b}_g - \mathbf{n}_g]_\times,\) \(\dot{\mathbf{v}} = \mathbf{g} + R(\mathbf{a}_m - \mathbf{b}_a - \mathbf{n}_a),\) \(\dot{\mathbf{p}} = \mathbf{v}, \quad \dot{\mathbf{b}}_g = \mathbf{n}_{wg}, \quad \dot{\mathbf{b}}_a = \mathbf{n}_{wa}.\)

离散化后： \(x_k^- = f(x_{k-1}^-,\, \mathcal{U}_{k-1:k}),\) \(P_k^- = F_k P_{k-1}^+ F_k^\mathsf{T} + G_k Q_k G_k^\mathsf{T}.\)

单轮测量更新

测量模型： \(\mathbf{z} = h(x) + \mathbf{n}, \quad \mathbf{n} \sim \mathcal{N}(\mathbf{0}, R).\)

线性化： \(\mathbf{r}^{(i)} \triangleq \mathbf{z} - h(x_k^{(i)}) \approx \mathbf{r}_0^{(i)} + H^{(i)}\,\delta x.\)

最小二乘形式： \(\min_{\delta x} \; \|\delta x\|_{(P_k^-)^{-1}}^2 + \|\mathbf{r}^{(i)} + H^{(i)}\delta x\|_{R^{-1}}^2.\)

等价 Kalman 更新： \(S^{(i)} = H^{(i)} P_k^- H^{(i)\mathsf{T}} + R,\) \(K^{(i)} = P_k^- H^{(i)\mathsf{T}} (S^{(i)})^{-1},\) \(\delta x^{(i)} = K^{(i)} \mathbf{r}^{(i)}.\)

状态更新： \(x_k^{(i+1)} = x_k^{(i)} \boxplus \delta x^{(i)}.\)

迭代（ESIKF/IEKF）

迭代流程：

  1. 初始化： \(x_k^{(0)} \leftarrow x_k^{-}, \quad P_k^{(0)} \leftarrow P_k^{-}.\)
  2. 重复：
    • 计算残差与雅可比
    • 计算增益与状态增量
    • 更新名义状态
  3. 收敛后输出： \(x_k^{+} \leftarrow x_k^{(i^\star)}, \quad P_k^{+} \leftarrow (\mathbf{I} - K^{(i^\star)} H^{(i^\star)}) P_k^- (\mathbf{I} - K^{(i^\star)} H^{(i^\star)})^\mathsf{T} + K^{(i^\star)} R K^{(i^\star)\mathsf{T}}.\)

多传感器联合

多源测量堆叠： \(\mathbf{r} = \begin{bmatrix} \mathbf{r}_1 \\ \vdots \\ \mathbf{r}_m \end{bmatrix}, \quad H = \begin{bmatrix} H_1 \\ \vdots \\ H_m \end{bmatrix}, \quad R = \mathrm{blkdiag}(R_1, \dots, R_m).\)

更新： \(S = H P_k^- H^\mathsf{T} + R, \quad K = P_k^- H^\mathsf{T} S^{-1}, \quad \delta x = K\, \mathbf{r}.\)

实现要点

  • 姿态部分使用李群雅可比，减少大角度线性化误差
  • 协方差更新建议使用 Joseph 形式
  • 每次迭代后需进行误差重置映射
  • 对视觉/激光残差可引入鲁棒核函数进行加权