当前位置：首页 > java >正文

LIO-SAM框架理解

java 2025/5/15 5:41:37

LIO-SAM系统为四大线程的运行：

1.IMU预积分线程（IMU Pre-integration）

2.点云去畸变线程（Point Cloud Deskewing）

3.前端里程计线程（Odometry）

4.后端优化线程（Mapping / Factor Graph Optimization）

但是我考虑到与其他slam系统（如cartographer vins mono等）做对比，还是按照初始化，前端，后端的结构来总结分析。

一、初始化

LIO-SAM初始化阶段跟orbslam3类似，跟cartographer不一样。前者需要先有视觉/激光里程计，后面在与imu做松耦合的初始化。而cartographer的imu初始化则是隐式的，独立的，并不需要借助激光里程计，包括bias也是后续做帧间位姿估计的时候再独立优化。

阶段	ORB-SLAM3	LIO-SAM
初始传感器独立运行	视觉单独跑一段，先估 pose	激光 scan-matching 独立运行，先建地图
IMU bias 初始化方式	用视觉位姿轨迹 + IMU 预积分对齐求 bias	用激光位姿轨迹 + IMU 预积分对齐求 bias
是否联合优化初始化	❌ 否，初始化阶段没有联合优化	❌ 否
是否估计重力方向	✅ 是（通过对齐）	✅ 是
是否估计速度	✅ 是	✅ 是（一般设为 0）
后端优化是否紧耦合	✅ 是（IMU + 视觉 + map point 进入因子图）	✅ 是（IMU + scan to map 进入 GTSAM）

特性	LIO-SAM	Cartographer
初始化是否显式	✅ 显式初始化	❌ 没有显式初始化步骤
IMU bias 估计方式	显式估计初始 bias	初期设为 0，靠滤波器逐渐估
是否构建因子图	✅ GTSAM 因子图	❌ 不建因子图，使用滤波器
是否预积分	✅ 使用预积分（GTSAM IMU 因子）	❌ 使用实时积分，无预积分
是否使用紧耦合优化	✅ 后端因子图紧耦合	❌ 前端滤波器松耦合为主
地图维护方式	保存关键帧点云 + 后端回环优化	Submap + sparse pose graph

二、前端

一、激光点云预处理（Feature Extraction）

位于模块 imageProjection.cpp

对原始点云进行畸变补偿（用 IMU 提供的角速度和线速度补偿旋转和运动）
将点云按扫描线投影到 range image
提取特征点：
- 边缘特征（角点）：曲率大的点
- 平面特征（面点）：曲率小的点
降采样、过滤异常点

📌 输出：cornerCloud 和 surfaceCloud（当前帧的角点和平面点）

二、IMU 预积分预测（IMU Preintegration）

位于模块 imuPreintegration.cpp

将 IMU 数据进行预积分，构建当前帧到上一帧的初始运动估计（预测变换）
这一步用于：
- 提供 scan-matching 的初值
- 构建后端的 IMU 因子

📌 输出：预测位姿、速度、bias 状态等

三、激光里程计匹配：Scan-to-Map Matching

位于模块 mapOptimization.cpp 中的 laserOdometryHandler()

将当前帧的角点和平面点，与局部地图（由历史关键帧构建）进行匹配
优化目标是最小化：
- 当前角点 → 局部地图边缘线的距离
- 当前面点 → 局部地图平面的距离
优化器（Levenberg-Marquardt）迭代求解当前帧的位姿

📌 输出：当前帧精确位姿估计（T_w_curr）

四、关键帧选择与地图维护

每隔一定距离或角度，将当前帧设为关键帧
将关键帧点云（降采样后的）加入局部地图（kd-tree）

地图是由关键帧组成的滑动窗口点云集合，包括：

边缘点地图（局部角点）
面点地图（局部平面点）

五、前端数据输出

前端的最终输出是：

当前帧的实时估计位姿（odom）
关键帧及其点云（传给后端）
优化初值（预测来自 IMU）

需要注意一个点，LIO-SAM的局部地图概念跟vins mono类似又不完全一样，LIO-SAM的局部地图只用于当前关键帧的scan to map,但是呢局部地图范围内是没有图优化的(vins mono是有的)。然后这两者的局部地图都不会影响到全局地图的优化，全局都是在优化一个位姿图。

然后cartographer和orbslam3的子图的概念更加接近，两者的子图都会融合到全局地图里面。不同的点是orbslam3的子图也是有做图优化的，而cartographer的子图仅仅是做scan matching后就确定位置了，后续只在全局地图优化那里再做优化。

三、后端

✅ 1. 构建因子图（Factor Graph）

后端使用 GTSAM 构建因子图，节点为关键帧状态，边为多种约束：

每个关键帧的状态变量包括：
- 位姿 Pose3
- 速度 Vector3
- IMU Bias imuBias::ConstantBias

✅ 2. 添加不同因子（Factors）

LIO-SAM 后端将三种信息来源转化为图中的因子（约束）：

因子类型	来自	用途
IMU 因子（`ImuFactor`）	IMU 预积分	约束帧间运动
里程计因子（`BetweenFactor`）	轮速/视觉	提供额外约束（可选）
激光因子（`LidarFactor`）	scan-to-map 匹配结果	提供当前位置观测值
回环因子（`BetweenFactor`）	回环检测	提供闭环位姿差约束

✅ 3. 关键帧插入逻辑

每当前端判断当前帧为关键帧，后端就将其作为节点插入因子图；
同时添加 IMU 和激光因子；
IMU bias 也作为变量加入图中，并加入 BetweenFactor 维持连续性。

✅ 4. 增量优化（ISAM2）

LIO-SAM 使用 GTSAM 的 iSAM2（增量优化）：

每加入一个关键帧，调用 isam->update(graph, initialEstimate)；
优化后会输出当前关键帧集合的全局最优状态（Pose/Vel/Bias）；
增量优化非常快，适合实时建图。

✅ 5. 回环检测与闭环优化（Loop Closure）

后端周期性检测是否存在回环：
- 使用 scan context 或距离检查；
- 若发现回环，执行 scan-to-map 匹配，计算相对位姿；
将回环约束添加为 BetweenFactor 到因子图中；
再次触发全局优化，校正历史轨迹和地图，消除漂移。

✅ 6. 状态更新与地图拼接

优化后提取关键帧的全局位姿；
将关键帧点云根据优化后的姿态拼接为全局地图；
发布 /lio_sam/mapping/map 话题或保存地图文件。

✅ 7. 维护滑动窗口

虽然 LIO-SAM 默认不做窗口限制，但可以根据内存或性能做裁剪，例如：

只保留最近 N 帧在因子图中；
或保留所有帧但限制局部地图范围。

前端输出：
└─ 当前帧位姿 + IMU预积分 + 特征点云
           ↓
    【后端因子图模块】
┌────────────────────────────┐
│ 1. 添加关键帧为图节点        │
│ 2. 加入 IMU/Lidar/Loop 因子 │
│ 3. 使用 GTSAM iSAM2 优化    │
│ 4. 更新全局状态估计         │
│ 5. 输出地图、轨迹等         │
└────────────────────────────┘

✅ 总结表：后端关键操作一览

步骤	名称	作用	使用的库
1	接收关键帧数据	获取初估位姿/预积分	ROS
2	构建因子图	表示各类约束	GTSAM
3	添加IMU因子	提供连续运动约束	IMU预积分
4	添加Lidar因子	当前帧scan-to-map匹配结果	前端输出
5	添加回环因子	闭环检测	Scan Context
6	优化	得到最优轨迹与地图	iSAM2
7	发布地图与轨迹	给其他模块使用	ROS 发布器

📌 最终结果

全局一致轨迹；
去漂移地图（支持回环）；
可保存并复用

liosam是用因子图，理论上也是图优化的一种。这里对比一下几个优化工具的区别。

特性	GTSAM	g2o	Ceres Solver
建模风格	因子图（Factor Graph）	图优化（顶点+边）	自动求导非线性最小二乘
主要目标	SLAM/BA/Smoothing，强调多传感器融合	SLAM 中 pose-graph/BA	泛化数值优化器
支持的优化算法	批量优化 + iSAM/iSAM2（增量）	Gauss-Newton、LM	GN、LM、Dogleg、信赖域
增量式优化	✅（iSAM2）	❌	❌（但可近似实现）
IMU预积分支持	✅（强支持）	手动实现	可实现，但较复杂
地图点优化支持	✅（支持BA）	✅（经典支持）	✅（多数BA系统使用）
自动求导	✅（强）	❌（需手写雅可比）	✅（强）
接口风格	类似 Probabilistic Graph	C++ 图类封装	类似 C++ 函数优化接口
用户负担	中等（理解因子图模型）	高（需手写雅可比等）	低（非常通用，易上手）
适合谁？	SLAM、滤波/图优化研究者	图优化研究者、工业系统	任意非线性优化用户