【AMR从SLAM到VLA】LIO-SAM紧耦合多传感器融合：因子图优化与IMU预积分完整推导（Point-LIO/FAST-LIVO对比）

摘要：第02篇讲了单激光SLAM，第03篇讲了纯视觉SLAM，两篇各有一个致命短板——激光怕退化场景，视觉怕弱纹理。本篇讲工业巡检机器人真正在用的形态：紧耦合多传感器融合。我们以 LIO-SAM 为教学主线，完整推导它的四因子图代价函数（先验+IMU预积分+激光里程计+回环）、IMU预积分的旋转/速度/位置增量公式与协方差传播，并简述 GTSAM iSAM2 的增量求解。代码部分手写一个 C++ ImuPreintegrator 类（输入 gyro/accel，输出 ΔR,Δv,Δp 及协方差），含 CMakeLists 与运行预期。最后横向对比 LIO-SAM / FAST-LIO2 / Point-LIO，并落到 FAST-LIVO2、R3LIVE 的激光-视觉-惯性前沿。

1. 问题引入

先把两条单传感器路线的"死法"摆出来，读者就明白融合为什么不是锦上添花而是必需品。

纯激光会死在退化场景与动态物体上。 我们羊场巡检机器人实测：进入一条长直饲料通道时，两侧是高度一致的金属围栏，16线点云在水平面上几乎是两堵平行的墙——scan matching 沿通道方向的约束退化为零（点到面残差对前进方向不敏感，详见第02篇 ICP 损失分析），里程计沿通道方向以每秒数厘米的速度"盲漂"。同时，羊场里动态物体是常态：羊、饲养员、推拉料车从激光前经过，被错误地当成静态特征钉进地图，污染后续所有配准。

纯视觉会死在弱纹理与尺度上。 第03篇已经分析过：白墙、光滑地坪、低光环境下 ORB 特征点数量断崖式下跌；单目视觉连尺度都不可观，必须靠IMU补。

于是问题变成：能不能让激光、视觉、IMU 各司其职、互相兜底？答案就是紧耦合多传感器融合SLAM。所谓"紧耦合"，是指把不同传感器的观测残差写进同一个优化问题的同一个状态向量上联合求解，而不是各自跑完里程计再在后处理里松组合相加。本篇的主角 LIO-SAM（LIDAR-Inertial Odometry via Smoothing and Mapping）就是这套思想最经典的工程实现；它的"表弟" LVI-SAM 再并联一路视觉子系统，就构成了激光-视觉-惯性三路紧耦合。

2. 原理推导

2.1 为什么用因子图，而不是滤波器

回顾第02篇：FAST-LIO 用迭代误差状态卡尔曼滤波（iEKF）递推，优点是快，缺点是"只记得现在"——回环检测出来要修正过去几十个位姿时，滤波器没有现成的全局结构可改。LIO-SAM 选了另一条路：因子图（factor graph）。把每个关键帧的位姿当作图上的一个变量节点，把每一种"观测"当作一条带噪声模型的边（因子）。这样回环因子就是一条把"现在"和"很久以前"连起来的边，一旦插入，图优化器一次性全局重解所有位姿——回环校正自然成立。代价是计算量随图规模增长，所以用 iSAM2 做增量平滑，只重解受新因子影响的局部。

2.2 LIO-SAM 四因子图

LIO-SAM 在关键帧序列 {xi}\{x_i\}{xi​}（$x_i$ 含位置、姿态、速度、IMU零偏）上连了四类因子：

1. 先验因子（prior）：首帧位姿固定为原点，或上一次优化结束后作为下一窗口的初值先验。
2. IMU预积分因子：相邻两关键帧之间，用这段时间的 IMU 数据积分出相对运动约束。它的巨大价值是——优化器每次调整关键帧位姿后，不需要重新读 IMU 数据重算积分，因为预积分量是"从关键帧 i 的状态出发"积分出来的，位姿微调时只需在预积分量上做一阶修正。这就是预积分相对"直接积分"的本质优势，2.3 节完整推导。
3. 激光里程计因子：两帧激光点云配准（scan-to-map）得到的相对位姿约束，提供几何观测。
4. 回环因子：DBoW2 或 scan context 检测到重访后，配准得到的两帧之间的全局约束，负责把长程漂移拉回来。

四类因子拼成的代价函数见公式3-1。这套"先验+IMU+激光+回环"的因子图结构，是理解一切紧耦合 LIO/LVI 系统的母版——FAST-LIO2 换了求解器（滤波代替图优化），但状态变量与残差构造思路同源；LVI-SAM 只是在这张图上再并联一条视觉因子子图。

2.3 IMU预积分完整推导

IMU 以极高频率（通常 100~400 Hz）输出角速度测量 $\tilde{\omega }$ 与比力测量 $\tilde{a}$，含零偏与噪声：

$$\tilde{\omega }=\omega +b_g+n_g,\tilde{a}=R^{\top }(a_w-g)+b_a+n_a$$

直接积分的问题在于：积分式以世界系下的旋转 $R_t$、速度 $v_t$、位置 $p_t$ 为状态，而后端优化器每次迭代都会改关键帧的 $R_i,v_i,p_i$，一改就得把这段IMU全部重积分，计算爆炸。预积分的诀窍是在关键帧 i 的本体坐标系下做积分，得到一个只依赖于两帧之间增量、与 $R_i,v_i,p_i$ 绝对值无关的观测量。

设在 $[t_i,t_{i+1}]$ 内，关键帧 i 到 j 的预积分量定义为：

$$\Delta R_j^i=\prod _k\exp (({\tilde{\omega }}_k-b_g)\Delta t)$$

$$\Delta v_j^i=\sum _k\Delta R_k^i({\tilde{a}}_k-b_a)\Delta t$$

$$\Delta p_j^i=\sum _k\left[\Delta v_k^i\Delta t+\frac{1}{2}\Delta R_k^i({\tilde{a}}_k-b_a)\Delta t^2\right]$$

由上式反推世界系下的状态递推关系（公式3-2），可以看到 $R_i,v_i,p_i$ 只出现在等号右侧第一项，后端优化微调 $x_i$ 时，$\Delta R,\Delta v,\Delta p$ 三个预积分量本身不变，只需对零偏 $b_g,b_a$ 做一阶修正即可——这就是预积分让后端可实时优化的全部秘密。

同时，IMU噪声通过连续时间误差状态方程 $\delta {\dot{x}}_t=F_t\delta x_t+G_t{n}_t$ 传播，离散化后递推协方差（公式3-3），得到预积分量自身的不确定度 ${\Sigma }_{\mathcal{I}}$——它就是因子图里 IMU 因子的马氏权重。预积分协方差矩阵通常是 15×15（旋转3+速度3+位置3+陀螺零偏3+加计零偏3），在 iSAM2 里直接作为该因子的信息矩阵使用。

2.4 GTSAM iSAM2 增量求解

因子图的通用求解是批量非线性最小二乘（第02篇公式6.6）：$\min \sum \Vert r{\Vert }_{\Sigma }^2$。但关键帧是随时间不断追加的，每来一帧就把全图重解一遍不可行。GTSAM 的 iSAM2（incremental Smoothing and Mapping）做三件事：新因子插入时只做局部贝叶斯树更新（类似增量消元），已收敛区域保持不变，只有受新回环影响的分支才重线性化。这就是 LIO-SAM 能在笔记本上实时跑 3D 建图的工程基础。

2.5 Point-LIO：另一条路线的极端化

与 LIO-SAM 的图优化路线平行，Point-LIO 走纯滤波路线（继承 FAST-LIO 的迭代误差状态卡尔曼），但把激光残差的处理带宽推到 4–8 kHz：逐点反向传播 IMU 运动补偿，论文实测即便激光角速度超过 IMU 量程（75 rad/s 的极限工况）仍能稳定输出里程计。它的另一个工程要点是用 Huber/鲁棒核函数抑制动态物体（羊、人）产生的外点残差——这对羊场这种动态环境极其对症。代价是 CPU 占用高、逐点时序同步复杂，不适合低算力嵌入式。

2.6 FAST-LIVO2 / R3LIVE：再加一路视觉

当激光几何退化（长廊）和视觉纹理退化（白墙）在同一空间交替出现时，就要上三路紧耦合：

• R3LIVE：LIO 子系统（FAST-LIO 思路）做几何里程计，视觉子系统给激光点云实时上 RGB 颜色，输出带真彩色的稠密点云地图；R3LIVE++ 进一步做可微辐射重建。适合数字孪生、巡检可视化。
• FAST-LIVO2（2024，IEEE T-RO）：用迭代误差状态卡尔曼+顺序更新策略统一激光与图像两类异构观测，平均 ATE 约 0.044 m，可在机载嵌入式实时运行，是当前 LIVO 方向的 SOTA 之一。

LVI-SAM 则是更经典的"并联双系统"结构：视觉惯性子系统与激光惯性子系统互为备份——视觉退化时激光顶上，几何退化时视觉顶上，任一子系统失效另一者可独立工作。

3. 公式推导

公式3-1：LIO-SAM 四因子图代价函数。 在所有关键帧状态 {xi}\{x_i\}{xi​} 上，最小化四类因子的马氏距离平方和：

$$\mathcal{F}=\underset{\text{先验因子}}{\underbrace{\Vert r_p-H_p{x}_0{\Vert }_{{\Sigma }_p}^2}}+\underset{\text{IMU预积分因子}}{\underbrace{\sum _i\left[r_{\mathcal{I}}\right(x_i,x_{i+1},{\bar{z}}_{i+1}^i){]}_{{\Sigma }_{\mathcal{I}}}^2}}+\underset{\text{激光里程计因子}}{\underbrace{\sum _i\left[r_{\mathcal{L}}\right(x_i,x_{i+1}){]}_{{\Sigma }_{\mathcal{L}}}^2}}+\underset{\text{回环因子}}{\underbrace{\sum _{(i,j)\in \mathcal{C}}\left[r_{\mathcal{R}}\right(x_i,x_j){]}_{{\Sigma }_{\mathcal{R}}}^2}}$$

其中 $\Vert e{\Vert }_{\Sigma }^2=e^{\top }{\Sigma }^{-1}e$ 为马氏距离；${\bar{z}}_{i+1}^i=[\Delta R_{i+1}^i,\Delta v_{i+1}^i,\Delta p_{i+1}^i]$ 为 IMU 预积分观测量（见公式3-2）；$\mathcal{C}$ 为回环检测给出的关键帧对集合；$r_{\mathcal{L}}$ 为 scan-to-map 配准残差（本质是第02篇点到面 ICP 残差的因子图形式）；$r_{\mathcal{R}}$ 为回环配准给出的相对位姿残差。iSAM2 增量地最小化该式。

公式3-2：IMU预积分与状态递推关系。 世界系下状态 $R_i,v_i,p_i$ 与预积分量 $\Delta R_{i+1}^i,\Delta v_{i+1}^i,\Delta p_{i+1}^i$ 满足：

$$R_{i+1}=R_i\Delta R_{i+1}^i\cdot \mathrm{Exp}\left(J_{\Delta R}\delta b_g\right)$$

$$v_{i+1}=R_i(\Delta v_{i+1}^i+J_{\Delta v}^g\delta b_g+J_{\Delta v}^a\delta b_a)+v_i+g\Delta t_i$$

$$p_{i+1}=R_i(\Delta p_{i+1}^i+J_{\Delta p}^g\delta b_g+J_{\Delta p}^a\delta b_a)+p_i+v_i\Delta t_i+\frac{1}{2}g\Delta t_i^2$$

式中 $\mathrm{Exp}(\cdot )$ 为 $SO(3)$ 指数映射，$J_{\Delta R},J_{\Delta v}^g,J_{\Delta p}^a$ 等为预积分量对零偏扰动的一阶雅可比（零偏更新时的修正项），$g$ 为世界系重力向量。三个式子把"关键帧状态"与"预积分观测量"线性耦合起来——IMU残差 $r_{\mathcal{I}}$ 就是"用右端预测的 $x_{i+1}$ 与预积分观测值之差"，从而可以直接塞进因子图。

公式3-3：预积分协方差传播。 预积分误差状态 $\delta z=[\delta \varphi ,\delta v,\delta p,\delta b_g,\delta b_a{]}^{\top }$（15维）服从连续时间线性化方程 $\delta \dot{z}=F\delta z+Gn$，离散化后每步递推：

$${\Sigma }_{k+1}=(I+F\Delta t){\Sigma }_k(I+F\Delta t{)}^{\top }+GQ{G}^{\top }\Delta t$$

其中 $Q=\mathrm{diag}({\sigma }_g^{2}I,{\sigma }_a^{2}I,{\sigma }_{bg}^{2}I,{\sigma }_{ba}^{2}I)$ 为 IMU 噪声与随机游走功率谱密度矩阵，$F,G$ 为在当前预积分轨迹处线性化得到的雅可比。递推结束得到的 ${\Sigma }_N$ 即因子图中 ${\Sigma }_{\mathcal{I}}$ 的取值——预积分时间越长、协方差越大，说明这段IMU约束越"软"，这与"走廊里纯IMU积分会发散"的物理直觉一致。

公式3-4：迭代误差状态更新（Point-LIO/FAST-LIO2 路线）。 以激光点云残差 $e_k$ 为观测，迭代 iEKF 更新：

$$\delta x_{k+1}=K_{k+1}(e_k-H_{k+1}\delta x_k),K=P{H}^{\top }(HP{H}^{\top }+R{)}^{-1}$$

$$\hat{x}\leftarrow \hat{x}\oplus \delta x,P\leftarrow (I-KH)P$$

Point-LIO 在此基础上对激光残差施加 Huber 鲁棒核 $\rho (e)=e^2(|e|\le \delta ),2\delta |e|-{\delta }^2(|e|>\delta )$，把动态物体造成的大残差自动降权——这是它在高动态羊场环境下比 LIO-SAM 更抗"羊撞激光"的关键工程技巧。

4. 代码验证

下面手写一个不依赖 GTSAM 的最小 IMU 预积分类 ImuPreintegrator：喂入一段 gyro/accel 测量（带采样间隔），输出这段区间的 $\Delta R$（3×3旋转矩阵）、$\Delta v$、$\Delta p$ 以及 15×15 预积分协方差。它是理解 LIO-SAM/FAST-LIO 内部预积分因子的教学切片，可单独跑单元测试，也可作为你自己 VIO/LIO 项目的起点。

// imu_preintegrator.hpp
#pragma once
#include <Eigen/Dense>

// 最小IMU预积分类：在关键帧i本体坐标系下积分 gyro/accel，
// 输出 ΔR(3x3), Δv(3), Δp(3) 及 15x15 预积分协方差。
// 状态误差顺序: [δφ(3), δv(3), δp(3), δbg(3), δba(3)]
class ImuPreintegrator
{
public:
  ImuPreintegrator(const Eigen::Vector3d & bg, const Eigen::Vector3d & ba,
                   double sigma_g, double sigma_a,
                   double sigma_bg_walk, double sigma_ba_walk);

  // 喂入一帧IMU：角速度(rad/s)、比力(m/s^2)、采样间隔(s)
  void integrate(const Eigen::Vector3d & gyro,
                 const Eigen::Vector3d & accel, double dt);

  // 复位到新关键帧（零偏更新后调用）
  void reset(const Eigen::Vector3d & bg, const Eigen::Vector3d & ba);

  Eigen::Matrix3d delta_R() const { return dR_; }
  Eigen::Vector3d delta_v() const { return dv_; }
  Eigen::Vector3d delta_p() const { return dp_; }
  Eigen::Matrix<double, 15, 15> covariance() const { return cov_; }

private:
  // SO(3)指数映射：旋转向量 -> 旋转矩阵
  static Eigen::Matrix3d exp_so3(const Eigen::Vector3d & w);

  Eigen::Matrix3d dR_ = Eigen::Matrix3d::Identity();
  Eigen::Vector3d dv_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d dp_ = Eigen::Vector3d::Zero();
  Eigen::Vector3d bg_, ba_;                 // 关键帧时刻的IMU零偏
  Eigen::Matrix<double, 15, 15> cov_ = Eigen::Matrix<double, 15, 15>::Zero();
  Eigen::Matrix<double, 12, 12> noise_Q_;  // 噪声协方差(陀螺/加计/零偏游走)
};

// imu_preintegrator.cpp
#include "imu_preintegrator.hpp"

Eigen::Matrix3d ImuPreintegrator::exp_so3(const Eigen::Vector3d & w)
{
  const double theta = w.norm();
  if (theta < 1e-8) return Eigen::Matrix3d::Identity();
  const Eigen::Matrix3d W =
    Eigen::AngleAxisd(theta, w / theta).toRotationMatrix();
  return W;
}

ImuPreintegrator::ImuPreintegrator(const Eigen::Vector3d & bg,
                                   const Eigen::Vector3d & ba,
                                   double sg, double sa, double sbg, double sba)
: bg_(bg), ba_(ba)
{
  noise_Q_.setZero();
  noise_Q_.block<3, 3>(0, 0) = sg * sg * Eigen::Matrix3d::Identity();       // 陀螺噪声
  noise_Q_.block<3, 3>(3, 3) = sa * sa * Eigen::Matrix3d::Identity();       // 加计噪声
  noise_Q_.block<3, 3>(6, 6) = sbg * sbg * Eigen::Matrix3d::Identity();      // 陀螺零偏随机游走
  noise_Q_.block<3, 3>(9, 9) = sba * sba * Eigen::Matrix3d::Identity();      // 加计零偏随机游走
}

void ImuPreintegrator::integrate(const Eigen::Vector3d & gyro,
                                 const Eigen::Vector3d & accel, double dt)
{
  // 1. 去除零偏后的角速度/比力
  const Eigen::Vector3d w = gyro - bg_;
  const Eigen::Vector3d a = accel - ba_;

  // 2. 预积分正向传播：ΔR/Δv/Δp（对应公式2.3的离散求和）
  const Eigen::Matrix3d dR_step = exp_so3(w * dt);
  const Eigen::Vector3d dv_new = dv_ + dR_ * a * dt;
  const Eigen::Vector3d dp_new = dp_ + dv_ * dt + 0.5 * dR_ * a * dt * dt;
  dR_ = dR_ * dR_step;
  dv_ = dv_new;
  dp_ = dp_new;

  // 3. 协方差传播（公式3-3）：F 为误差状态雅可比，这里给出教学版一阶形式
  Eigen::Matrix<double, 15, 15> F = Eigen::Matrix<double, 15, 15>::Identity();
  // δφ 对 δφ 的导数：~ I；δφ 对 δbg 的导数：-dR_·dt（负号因 w=gyro-bg）
  F.block<3, 3>(0, 9) = -dR_ * dt;
  // δv 对 δφ 的导数：-[dR_·a]_× ·dt
  F.block<3, 3>(3, 0) = -dR_ * skew(a) * dt;
  F.block<3, 3>(3, 12) = -dR_ * dt;            // δv 对 δba
  // δp 对 δv 导数 = I·dt；δp 对 δφ/δba 的二阶项此处省略（教学简化）
  F.block<3, 3>(6, 3) = Eigen::Matrix3d::Identity() * dt;

  Eigen::Matrix<double, 15, 12> G = Eigen::Matrix<double, 15, 12>::Zero();
  G.block<3, 3>(0, 0) = -dR_ * dt;            // 陀螺噪声 -> δφ
  G.block<3, 3>(3, 3) = dR_ * dt;             // 加计噪声 -> δv
  G.block<3, 3>(6, 3) = 0.5 * dR_ * dt * dt;  // 加计噪声 -> δp
  G.block<3, 3>(9, 6) = Eigen::Matrix3d::Identity() * dt;   // 零偏游走
  G.block<3, 3>(12, 9) = Eigen::Matrix3d::Identity() * dt;

  cov_ = F * cov_ * F.transpose() + G * noise_Q_ * G.transpose();
}

void ImuPreintegrator::reset(const Eigen::Vector3d & bg,
                             const Eigen::Vector3d & ba)
{
  dR_ = Eigen::Matrix3d::Identity();
  dv_ = Eigen::Vector3d::Zero();
  dp_ = Eigen::Vector3d::Zero();
  cov_.setZero();
  bg_ = bg; ba_ = ba;
}

注：skew(v) 为取反对称矩阵的工具函数，工程实现里用 Eigen::Vector3d(v).hat() 或自写三行即可；上面省略了 δp 对 δφ 的二阶耦合项，生产级实现请参考 LIO-SAM/GTSAM 的 PreintegratedCombinedMeasurements。

CMakeLists.txt 要点：

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(Eigen3 REQUIRED)

add_library(imu_preint src/imu_preintegrator.cpp)
target_include_directories(imu_preint PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  ${EIGEN3_INCLUDE_DIR})
ament_target_dependencies(imu_preint rclcpp)

add_executable(imu_preint_demo src/imu_preint_demo.cpp)
target_link_libraries(imu_preint_demo imu_preint)
install(TARGETS imu_preint_demo DESTINATION lib/${PROJECT_NAME})

运行方式：

colcon build --packages-select imu_preint
source install/setup.bash
ros2 run imu_preint imu_preint_demo

预期输出：喂入一段静止 IMU 数据（理想情况 gyro≈0, accel≈g），ΔR 应接近单位阵、Δv≈0、Δp≈0；喂入一段沿 x 轴匀速平移的仿真数据，Δp.x 应≈位移真值；协方差矩阵对角元随积分步数单调增大——静止 10 秒后位置协方差可达 0.01 m² 量级，这就是"纯 IMU 长时积分必漂移"的定量证据。

5. 工程落地与对比

工业选型一张表收尾：

维度	LIO-SAM	FAST-LIO2	Point-LIO	FAST-LIVO2 / R3LIVE
后端	因子图 iSAM2（图优化派）	iEKF（滤波派）	iEKF+4~8kHz逐点	iEKF顺序更新
回环	内置位姿图回环	需外挂闭环模块	需外挂	需外挂
传感器	多线机械LiDAR+9轴IMU	Livox固态/多线均可	高振动高机动平台	LiDAR+IMU+相机三路
算力需求	中（Jetson Orin可跑）	低（ARM/Jetson友好）	高（CPU吃满）	中高（需相机标定）
退化抗性	中（依赖激光几何）	较强（IMU紧耦合）	强（抗75rad/s极限）	最强（三路互为备份）
典型场景	户外巡检、3D建图	无人机、手持、嵌入式	无人机激烈机动、强振底盘	弱纹理+动态混合场景、彩色重建

结合我们的羊场项目选型：16线机械激光+9轴IMU+Jetson Orin，半室外巡检——主线用 LIO-SAM（多线机械雷达适配好、内置回环、工程资料最全）；如果未来上 Livox 固态雷达或对实时性要求苛刻，迁移到 FAST-LIO2；遇到羊场强振动底盘或激烈转向工况，参考 Point-LIO 的逐点补偿与鲁棒核；需要"看懂墙上的湿度计/看板"时，再叠加视觉子系统走向 FAST-LIVO2/R3LIVE 的彩色语义地图。

调参坑点三条：第一，IMU-LiDAR 时间同步与外参标定是成败关键——外参平移误差 5 cm，点云去畸变就全错；第二，先验因子与回环因子的协方差不能拍脑袋，用本文 ImuPreintegrator 跑出来的协方差量级去标定；第三，动态物体（羊、人）永远是点云地图的污染源，点云下采样后加一个距离/反射率阈值过滤动态点，比换算法立竿见影。

6. 小结

1. 单激光死于退化场景与动态物体，单视觉死于弱纹理与尺度不可观，紧耦合多传感器融合是工业 AMR 的必由之路。
2. LIO-SAM 用四因子图（先验+IMU预积分+激光里程计+回环）把所有约束写进同一个非线性最小二乘问题，iSAM2 增量求解。
3. IMU 预积分的本质是"在关键帧本体坐标系下积分"，使后端优化调整关键帧位姿时无需重积分，且预积分协方差随时间单调发散——纯 IMU 不能长时独立。
4. 滤波派（FAST-LIO2/Point-LIO）实时但难回环，图优化派（LIO-SAM）能回环但计算重；三路紧耦合（FAST-LIVO2/R3LIVE）是 2024–2026 的前沿方向。
5. 对羊场巡检机器人：LIO-SAM 为主线，FAST-LIO2/Point-LIO 为工况备选，LIVO 为语义扩展方向。

到此为止，SLAM 已经解决了"机器人在哪、世界长什么样"。但知道位置只是第一步——机器人拿到目标点之后，怎么在地图上规划一条不撞羊、不撞围栏的路径？下一篇进入 第05篇：ROS2 Humble Nav2 导航栈架构拆解——行为树编排、Costmap2D 三层代价地图、生命周期节点与插件开发实战。

参考资料

1. T. Shan et al., LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping, IROS 2020. https://arxiv.org/pdf/2007.00258v2
2. LIO-SAM 开源仓库. https://github.com/TixiaoShan/LIO-SAM
3. W. Xu, F. Zhang, FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter, 2020. https://arxiv.org/pdf/2010.08196
4. W. Xu et al., FAST-LIO2: Fast Direct LiDAR-inertial Odometry, IEEE T-RO 2023. https://arxiv.org/pdf/2107.06829
5. Point-LIO 开源仓库（HKU MARS Lab）. https://github.com/hku-mars/Point-LIO
6. T. Shan et al., LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping, ICRA 2021. https://arxiv.org/html/2104.10831v1
7. J. Lin, F. Zhang, R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package, IROS 2021. https://arxiv.org/html/2109.07982v1
8. FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry, IEEE T-RO 2024. https://arxiv.org/html/2408.14035