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、滤波/图优化研究者 | 图优化研究者、工业系统 | 任意非线性优化用户 |