SLAM文献之Unified Multi-Modal Landmark Tracking for Tightly Coupled Lidar-Visual-Inertial Odometry

《Unified Multi-Modal Landmark Tracking for Tightly Coupled Lidar-Visual-Inertial Odometry》是一篇融合激光雷达、视觉和惯性测量（LiDAR, Visual, IMU）三种传感器的紧耦合状态估计算法论文，旨在提升 VIO 在动态/复杂环境下的鲁棒性与精度。该方法统一处理点、线、面三类特征，实现多模态地标（multi-modal landmarks）的融合建图与定位。在这里插入图片描述

核心思想概览

在紧耦合 VIO 框架中，引入 LiDAR 深度信息辅助视觉特征定位，并同时支持平面与 线特征，实现 统一的多模态因子图优化。通过 factor graph + iSAM2 进行高效、渐进式状态估计。

一、算法核心概念

目标：在滑动窗口的因子图（factor graph）中，同时融合激光雷达、视觉与惯性传感器，实现紧耦合里程计。
关键创新：将从 LiDAR 提取的 3D 线段和平面特征建为“地标”（landmarks），而非仅帧间匹配，并在多个扫描之间进行跟踪，克服了传统帧间跟踪方法的次优性；利用被动同步策略实现 LiDAR 与相机的松耦合；无硬切换机制，根据传感器可靠性自然发挥优势。

二、算法流程概述

1. 特征提取模块

LiDAR 端：从连续扫描中提取稳定的 3D 线和平面特征，并作为长期地标进行跟踪。具体如下：

算法的一个关键特性是，从与相机图像同步的 LiDAR 点云中提取特征元素，使得所有传感器的观测都可以在同一个图优化过程中联合处理。整个处理流程包括以下几个步骤：点云去畸变与同步、滤波、特征元素提取与追踪，以及因子构建。

1）去畸变与同步（Undistortion and Synchronization）：

图 6 展示了不同传感器的输出频率。IMU 与相机是“瞬时采样”，而 LiDAR 是通过其内部镜面围绕 z 轴旋转，连续获取点数据。一旦一圈扫描完成，所有的激光回波就会被转换为点云，并立即开始下一圈扫描。

由于 LiDAR 点是在运动中采集的，因此需要利用运动先验对点云进行去畸变，并将其关联到一个统一的时间戳——通常是扫描开始的时间 [27]。这种处理方式会导致 LiDAR 和相机数据具有不同的时间戳，进而对应于图优化中的不同节点。

本的方法是将每次 LiDAR 扫描去畸变到“扫描开始后最接近的相机图像帧时间戳”。例如，在图 6 中，扫描 L2 会被去畸变到关键帧 C3 的时间戳。利用从 IMU 模块前向传播的状态，依据每个点的采样时间进行线性外推以形成运动先验（为简洁起见，没有采用高斯过程插值 [28] 或带时间偏移的状态扩展方法 [29]）。由于该点云现在与关键帧 C3 关联，LiDAR 特征就可以直接连接到该节点，而不需要创建新的图节点。

这个“微妙”的设计既保证了图中新增节点与因子的数量一致，又使得优化可以在 IMU、相机与 LiDAR 三种传感器之间联合进行。它也确保了输出频率是固定的：即相机帧率（或在相机缺失时为 LiDAR 帧率），而不是两者的混合。

2）滤波（Filtering）：

完成点云去畸变后，使用 [30] 中的方法对点云进行聚类分割。小于 5 个点的聚类被认为是噪声并被剔除。

接着，采用 [2] 中的局部曲率计算方法来评估每个点的曲率。曲率最低和最高的点分别归入平面候选集 $C_P$ 和线段候选集 $C_L$ 。

这种基于分割与曲率的双重滤波，通常能将点云数据量减少约 90%，大大节省了后续平面与线特征处理的计算资源。

3）平面与线特征提取与追踪（Plane and Line Extraction and Tracking）：

在时间序列中，对候选集中提取出的平面与线段特征进行持续追踪，这类似于视觉中局部特征的跟踪方法，即在预测位置的局部邻域内进行特征匹配。

**首先取前一帧中已跟踪的平面 $p_{i-1}$ 和线段 $l_{i-1}$ ，并使用 IMU 的前向传播状态预测其在当前帧中的位置 $p^i\hat{p}_i$ 、 $l^i\hat{l}_i$ 。然后，在预测位置周围，从候选集 $C_P$ 和 $C_L$ 中选取距离模型较近的点进行局部分割。

接下来，采用欧几里得聚类（对于平面还加上法线过滤）来剔除离群点，然后对分割后的点云使用 PROSAC（Progressive Sample Consensus）[31] 进行鲁棒模型拟合。

最后，评估预测特征与检测到的新特征是否足够相似**。如果满足以下阈值判断条件，即认为匹配成功：

两个平面 $p_i$ 和 $p_j$ 的法向量夹角与原点距离差异满足：

$δn=∥arccos⁡(n^i⋅n^j)∥<αp(16) \delta_n = \| \arccos(\hat{n}_i \cdot \hat{n}_j) \| < \alpha_p \quad (16)$

$δd=∥n^idi−n^jdj∥<βp(17) \delta_d = \| \hat{n}_i d_i - \hat{n}_j d_j \| < \beta_p \quad (17)$
两条线段 $l_i$ 和 $l_j$ 的方向夹角与中心点距离满足：

$δn=∥arccos⁡(v^i⋅v^j)∥<αl(18) \delta_n = \| \arccos(\hat{v}_i \cdot \hat{v}_j) \| < \alpha_l \quad (18)$

$δd=∥(di−dj)−((di−dj)⋅v^i)v^i∥<βl(19) \delta_d = \| (d_i - d_j) - ((d_i - d_j) \cdot \hat{v}_i) \hat{v}_i \| < \beta_l \quad (19)$

其中，实验中设置阈值为： $αp=αl=0.35\alpha_p = \alpha_l = 0.35$ 弧度， $βp=βl=0.5\beta_p = \beta_l = 0.5$ 米。

一旦一个特征被成功追踪，该特征的内点会从候选集中移除，然后对剩余候选特征重复上述过程。

追踪完成后，再对剩余的候选点云检测新的特征。在点云中通过欧式聚类（针对线特征）与基于法向量的区域生长（针对平面特征）来生成新聚类，然后在每个聚类中用与追踪相同的方式检测新特征。

注意：点云特征只有在被连续跟踪达到一定帧数后，才会被加入图优化中。此外，为了尽可能延长特征的生命周期，优先处理历史最久远的特征。

视觉端：检测并跟踪传统的图像角点，如 ORB、FAST 等，生成视觉地标。
采用FAST 角点检测器进行特征点提取，并利用 KLT 特征追踪器在连续帧之间对这些特征进行跟踪，同时通过 RANSAC 方法剔除外点，每两帧图像中选择一帧作为关键帧，实现 15 Hz 输出频率。

2. 里程计管线与因子图构造

形成一个延迟窗口（fixed-lag smoother），滑动优化：

1、包含 IMU 预积分约束；

预积分 IMU 因子

采用当前已成为标准的 IMU 测量预积分方法 [23]，用于约束图中两个连续节点之间的位姿、速度和偏置，并在节点之间提供高频状态更新。该残差项形式如下：

$r_{I_{ij}} = \left[ r_{\Delta R_{ij}}^T,\; r_{\Delta v_{ij}}^T,\; r_{\Delta p_{ij}}^T,\; r_{b_a}^{ij},\; r_{b_g}^{ij} \right] \tag{5}$

2、LiDAR 平面/直线到地图中 landmark 的观测约束；

A. 平面路标因子（Plane Landmark Factor）

我们使用 Hessian 法向量形式来参数化一个无限平面 $p$ ，该形式由单位法向量 $n^∈R3\hat{n} \in \mathbb{R}^3$ 和一个表示其到原点距离的标量 $d$ 构成：

$p=⟨n^,d⟩∈R4满足n^⋅(x,y,z)+d=0(8) p = \left\langle \hat{n}, d \right\rangle \in \mathbb{R}^4 \quad \text{满足} \quad \hat{n} \cdot (x, y, z) + d = 0 \tag{8}$

令 $⊗\otimes$ 表示对平面 $p$ 上所有点应用一个齐次变换 $T$ 的操作符，令 $⊖\ominus$ 表示两个平面 $p_i, p_j)$ 之间误差的操作符，其定义为：

$pi⊖pj=[BpTξ^, di−dj]T∈R3(9) p_i \ominus p_j = \left[ B_p^T \hat{\xi},\; d_i - d_j \right]^T \in \mathbb{R}^3 \tag{9}$

其中 $Bp∈R3×2B_p \in \mathbb{R}^{3 \times 2}$ 是 $n^i\hat{n}_i$ 的切空间的一组正交基， $ξ^\hat{\xi}$ 定义为 [24]：

$ξ^=−arccos⁡(n^i⋅n^j)1−(n^i⋅n^j)2(n^j−(n^i⋅n^j)n^i)∈R3(10) \hat{\xi} = -\frac{ \arccos(\hat{n}_i \cdot \hat{n}_j) }{ 1 - (\hat{n}_i \cdot \hat{n}_j)^2 } (\hat{n}_j - (\hat{n}_i \cdot \hat{n}_j)\hat{n}_i) \in \mathbb{R}^3 \tag{10}$

当一个平面 $p~i\tilde{p}_i$ 在时刻 $t_i$ 被测量时，其对应的残差是估计平面 $pℓp^\ell$ 经过变换后与该测量之间的差值：

$r_{x_i, p^\ell} = \left( T_i^{-1} \otimes p^\ell \right) \ominus \tilde{p}_i \tag{11}$

B. 直线路标因子（Line Landmark Factor）

采用文献 [25] 中的方法来参数化无限直线。该方法使用一个旋转矩阵 $\in SO(3)$ 和两个标量 $\in \mathbb{R}$ 。其中，直线的方向向量为 $v^=Rz^\hat{v} = R\hat{z}$ ，与原点最近点的位置为 $d = R (a x + b y)$ 。因此，一条直线 $l$ 可以定义为：

$\left\langle R, (a, b) \right\rangle \in SO(3) \times \mathbb{R}^2 \tag{12}$

令 $⋆\star$ 表示将一个变换 $T_{ij} = (R_{ij}, p_{ij})$ 应用于一条直线 $l_i$ 得到 $l_j$ 的操作，具体变换关系如下：

$\begin{aligned} R_j &= R_{ij} R_i \\ a_j &= a_i - [1\; 0\; 0] R_i^T p_{ij} \\ b_j &= b_i - [0\; 1\; 0] R_i^T p_{ij} \end{aligned} \tag{13}$

两个直线 $l_i, l_j$ 的误差操作符 $⊖\ominus$ 定义为：

$l_i \ominus l_j = \left[ \begin{pmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 0 \end{pmatrix} ^T \log(R_i^T R_j),\; a_i - a_j,\; b_i - b_j \right]^T \in \mathbb{R}^4 \tag{14}$

根据公式 (13) 和 (14)，当测量直线为 $l~i\tilde{l}_i$ ，其预测为 $lℓl^\ell$ 时，对应的残差定义为：

$r_{x_i, l^\ell} = \left( T_i^{-1} \star l^\ell \right) \ominus \tilde{l}_i \tag{15}$

在优化过程中，对公式 (11) 与 (15) 使用数值导数，采用对称差分法（symmetric difference method）计算雅可比矩阵。

3、视觉特征的重投影约束；

A. 融合激光深度的单目路标因子

为了充分利用视觉与激光传感模态的融合，对单目视觉特征进行跟踪，同时使用激光雷达的重叠视场来提供深度估计，这种方式参考了文献 [16]。由于激光雷达和摄像头分别以 10 Hz 和 30 Hz 的频率运行。

设 $mℓ∈R3m^{\ell} \in \mathbb{R}^3$ 为三维空间中的一个视觉路标， $π:SE(3)×R3→R2\pi : SE(3) \times \mathbb{R}^3 \rightarrow \mathbb{R}^2$ 是一个投影函数，给定平台姿态 $T_i$ 后将路标投影到图像平面（为简洁起见，这里省略了基座、激光雷达与摄像头之间的固定变换）， $(uℓ,vℓ)∈R2(u^{\ell}, v^{\ell}) \in \mathbb{R}^2$ 是 $mℓm^{\ell}$ 在图像平面上的检测位置（见图 4 右图中的黄色点）。首先将激光雷达在时刻 $t_i$ 到 $t_{i+1}$ 之间获取的所有点 $x~m∈Li\tilde{x}_m \in L_i$ 投影到图像平面 $π(Ti,x~m)\pi(T_i, \tilde{x}_m)$ （图中绿色点）。然后，在 3 像素范围内寻找与视觉点 $(uℓ,vℓ)(u^{\ell}, v^{\ell})$ 最接近的投影点 $π(x~ℓ)\pi(\tilde{x}^{\ell})$ 。

最终，残差项计算如下：

$r_{x_i, m^{\ell}} = T_i^{-1} m^{\ell} - \tilde{x}^{\ell} \tag{6}$

当无法将激光深度与视觉特征关联（如激光和相机分辨率不一致）或关联不稳定（即深度在帧之间变化超过 0.5 米，通常由于动态障碍或噪声所致）时，将退回使用立体匹配方法。

B. 立体视觉路标因子

在状态 $x_i$ 处，对路标 $mℓm^{\ell}$ 的残差项如下：

$r_{x_i, m^{\ell}} = \begin{pmatrix} \pi_u^L(T_i, m^{\ell}) - u_i^{L, \ell} \\ \pi_u^R(T_i, m^{\ell}) - u_i^{R, \ell} \\ \pi_v(T_i, m^{\ell}) - v_i^{\ell} \end{pmatrix} \tag{7}$

其中， $u^L, v)$ 、 $u^R, v)$ 分别是左目和右目图像中检测到的路标像素位置，观测协方差 $Σm\Sigma_m$ 是基于 0.5 像素的不确定性计算得到的。

最后，如果仅使用单目相机，则只使用式 (7) 中的第一项和最后一项。

LiDAR 和视觉地标共同构成节点，统一优化位姿与 landmark。

3. 同步机制与无硬切换策略

通过被动同步，在同一个 SLAM 轨迹图中同时处理视觉、LiDAR 与 IMU 数据；
无需显式检测退化切换，而是通过传感器提供约束自然调整：在无纹理环境视觉地标少时，视觉约束不强，LiDAR 发挥主导；在几何退化区域，视觉提供补充。

三、核心创新亮点

地标式 LiDAR 特征跟踪
将 LiDAR 的平面和线段视觉特征当作长期地标进行跟踪，相比传统帧间 ICP 更鲁棒 ([arxiv.org][1])。
真融合机制
所有三种传感器以统一因子图方式处理，无需场景判定或开关逻辑。
轻量高效
地标表示仅需少量参数，适合全实时运行，无需单独线程处理。