LIO-SAM 学习总结

[1 LIO-SAM 介绍](#1 LIO-SAM 介绍)
[2 核心代码](#2 核心代码)
- [2.1 imageProjection 部分](#2.1 imageProjection 部分)
- [2.2 featureExtraction 部分](#2.2 featureExtraction 部分)
- - [(1) 处理流程](#(1) 处理流程)
- [2.3 imuPreintegration 部分](#2.3 imuPreintegration 部分)
- [2.4 mapOptmization 部分](#2.4 mapOptmization 部分)
- - [（1）核心函数](#（1）核心函数)
  - （2）核心算法流程

1 LIO-SAM 介绍

2 核心代码

featureExtraction.cpp 特征提取角点和面点（易）

imageProjection.cpp 点云畸变矫正（易）

imuPreintegration.cpp imu预积分和位姿优化（较难）

mapOptmization.cpp 地图匹配、位姿优化和回环检测（难）

代码阅读顺序：

imageProjection >> featureExtraction >> imuPreintegration >> mapOptmization

2.1 imageProjection 部分

作为LIO-SAM的前端预处理模块，它的主要任务是将一帧无序、带有因传感器自身运动而产生畸变的原始点云，转换成一帧经过运动补偿的、有序的、滤除无效点的点云，并整理出用于后续特征提取（如角点、平面点）的辅助信息。

（1）主要订阅与发布主题

订阅:

原始点云 (pointCloudTopic)：来自激光雷达的原始数据。
IMU数据 (imuTopic)：提供高频的姿态角速度信息，用于旋转去畸变。
增量里程计 (odomTopic)：提供来自IMU预积分或其他来源的初步位姿估计，为后续图优化提供初始猜测，其增量信息理论上可用于位置去畸变（但代码中已注释）。

发布:

去畸变后的点云 (lio_sam/deskew/cloud_deskewed)：供后续模块使用的核心点云。
点云信息 (lio_sam/deskew/cloud_info)：一个自定义消息，包含处理后的点云、以及每个点的行列索引、距离、所属扫描线（ring）的起止索引等关键信息，极大方便了后续的特征提取。

（2）关键数据结构与变量

VelodynePointXYZIRT, OusterPointXYZIRT: 定义了不同品牌雷达的点数据格式，最终统一为 PointXYZIRT使用。
rangeMat(cv::Mat): 一个大小为 N_SCAN x Horizon_SCAN的矩阵，存储每个像素位置（对应一个点）的距离值。初始为 FLT_MAX，用于判断该位置是否已被填充，实现最近点保留。（N_SCAN 表示激光雷达的线束数量，Horizon_SCAN 表示横向扫描分辨率）
fullCloud: 一个"图像化"的点云，fullCloud->points[columnIdn + rowIdn * Horizon_SCAN] 对应 rangeMat中 (rowIdn, columnIdn) 位置的点。它是一个"虚容器"，很多位置是空的。
extractedCloud: 从 fullCloud中提取出的所有有效点（即rangeMat值不为FLT_MAX的点）集合，是最终发布的点云。 imuTime, imuRotX/Y/Z 数组: 缓存当前帧点云时间段内IMU的时间戳和积分后的旋转角，用于对每个点进行精确的旋转插值。
cloudInfo(自定义消息): 承载所有输出信息的核心结构。

（3）处理流程 (cloudHandler)

当收到一帧点云时，按顺序执行以下步骤：

cachePointCloud(缓存与转换):

将点云消息放入队列，确保处理时有缓冲。从队列中取出一帧，根据雷达类型（Velodyne, Ouster, Livox）转换为统一的PointXYZIRT格式。
获取该帧点云的起始时间戳 (timeScanCur) 和结束时间戳 (timeScanCur + points.back().time)。
检查点云是否dense（无NaN点）以及是否包含ring和time通道。

deskewInfo(获取去畸变信息):

同步检查: 确保有覆盖本帧点云时间段的IMU数据可用。

imuDeskewInfo(IMU去畸变信息处理函数--角度积分)

清空早于本帧开始时间的IMU数据。
对IMU角速度进行数值积分，计算出本帧时间段内一系列时间点 (imuTime) 对应的累积旋转角 (imuRotX/Y/Z)，存入环形数组。这等效于得到了一个关于时间的旋转函数。
记录本帧起始时刻的姿态 (imu_roll/pitch/yaw_init) 作为初始猜测。

odomDeskewInfo

找到本帧扫描开始和结束时刻对应的里程计位姿。用开始的位姿 (startOdomMsg) 初始化 cloudInfo.initial_guess_，供后端优化使用。

projectPointCloud(点云投影与去畸变):

这是最核心的函数，逐点处理：

点筛选: 根据配置的最近和最远距离、扫描线号 (ring) 进行过滤。downsampleRate可对扫描线进行降采样。
投影到图像: 根据点的 (x,y)坐标计算水平角 horizonAngle，再根据分辨率 ang_res_x 换算成列索引 columnIdn。行索引 rowIdn 就是 ring。这样每个点都被分配到一个 (row, column) 像素位置。
距离图像滤波: 如果同一个 (row, column) 位置已有更近的点 (rangeMat值更小)，则忽略当前点（最近点保留）。
运动去畸变: 对于通过筛选的点，调用 deskewPoint 函数（将其余点对齐到初始时刻）。

findRotation(基于IMU数据的时间插值功能)

根据该点的时间戳 pointTime，在预积分的IMU旋转数组 (imuRotX/Y/Z) 中进行线性插值，得到该点精确时刻相对于激光雷达初始时刻的旋转 (rotXCur, rotYCur, rotZCur)。

cloudExtraction(点云提取):

遍历 rangeMat，将所有有效点（距离不为FLT_MAX）按顺序取出，放入 extractedCloud。
同时，在 cloudInfo中记录：
point_col_ind: 每个有效点在其原始扫描线中的列索引。
point_range: 每个有效点的距离。
start_ring_index和 end_ring_index: 每条扫描线上有效点的起止索引（在extractedCloud中的索引），这对于按线束提取平面特征至关重要。

publishClouds(发布结果):

发布去畸变、有序化后的点云 extractedCloud。
发布包含丰富结构化信息的 cloudInfo消息，供后续的 featureExtraction节点使用。

2.2 featureExtraction 部分

角点（边缘点） → 用于线特征匹配

匹配原理：点到线的距离最小化

适用场景：墙角、桌沿、物体边缘
平面点（面点） → 用于面特征匹配

匹配原理：点到面的距离最小化

适用场景：墙面、地面、桌面

(1) 处理流程

calculateSmoothness(计算点云中每个点的平滑度或曲率)

输入: 有序点云 extractedCloud 和对应的深度数组 cloudInfo.point_range[]

计算步骤：

遍历点云（跳过边界5个点保证窗口完整）
对每个点i，取前后5个点（共11个点）的深度值
计算加权差分：前5点+后5点 - 当前点×10
平方得曲率：cloudCurvature[i] = diffRange²
初始化标记数组
存入平滑度结构体供排序

markOccludedPoints(判断前后点)

标记两类不适合作为特征点的点：
(1) 被其他物体遮挡的点（Occluded Points）
(2) 激光束与表面几乎平行的点（Parallel Beam Points）
将这些点标记为cloudNeighborPicked[i]=1，后续不会选为特征点

遮挡检测原理

bash 复制代码

视觉示意图：


情况1（前点遮挡后点）：          情况2（后点遮挡前点）：
    ○  (depth1=5m)                  ○  (depth1=3m)
     \                             /
      \                           /
       ●  (depth2=2m)            ●  (depth2=6m)
     前点遮挡了后点               后点被前点遮挡
     
条件判断：
1. columnDiff < 10：确保两点在同一扫描线附近
2. depth1-depth2 > 0.3：前点比后点远0.3m以上 → 前点被遮挡
3. depth2-depth1 > 0.3：后点比前点远0.3m以上 → 后点被遮挡

平行光束检测原理

bash 复制代码

物理原理：
激光束
   ↓
   |\ θ (入射角很小，接近平行)
   | \
   |  \ 物体表面
   |   \
   ○----○
   
当激光束与物体表面夹角θ很小时：
1. 激光点可能落在物体边缘
2. 深度测量误差增大（激光光斑被拉长）
3. 相邻点深度差异大（>2%的当前深度）

数学判断：|range[i-1]-range[i]| > 0.02×range[i] AND
         |range[i+1]-range[i]| > 0.02×range[i]
表示当前点深度与前后点都有显著差异

extractFeatures(提取角点和面点)

复制代码

// 输入：已计算曲率和标记不可靠点的点云
// 输出：两类特征点云
    cornerCloud    // 边缘点/角点（高曲率，用于线特征匹配）
    surfaceCloud   // 平面点/面点（低曲率，用于面特征匹配）

// 处理流程：
    1. 将每条扫描线分成6个子区域
    2. 在每个子区域中独立提取特征
    3. 对平面点进行下采样以减少数据量
    4. 发布提取的特征点

分块原理：

bash 复制代码

每条扫描线索引范围：start_ring_index[i] ~ end_ring_index[i]
分成6等分：sp是子区域起点，ep是终点
示例：第i条线有300个点
  j=0: sp=0,  ep=49   (0~49)
  j=1: sp=50, ep=99   (50~99)
  j=2: sp=100,ep=149  (100~149)
  ...

角点选择策略

从曲率最大的点开始（边缘特征最明显）
每个子区域最多选20个，保证特征点分布均匀
选择后标记周围点，避免特征点聚集

平面点选择策略

从曲率最小的点开始（表面最平坦）
没有数量限制，但会进行下采样减少数据量
同样标记周围点避免聚集

2.3 imuPreintegration 部分

（1）算法介绍

算法数据流程

bash 复制代码

IMU数据流（高频） → imuHandler() → 实时预积分 → 发布高频IMU里程计
                     ↓
                  存储队列
                     ↓
里程计数据流（低频） → odometryHandler() → 图优化 → 更新状态

算法特点与优势

紧耦合优化
将IMU预积分因子和激光里程计因子一起优化
实时估计和校正IMU偏差
增量式优化
使用gtsam::ISAM2进行增量优化
每100个关键帧重置图优化，避免计算量无限增长
鲁棒性设计
退化检测：当激光里程计退化时使用更大的噪声模型
故障检测：检测异常速度和偏差，及时重置系统
时间同步：正确处理IMU和激光雷达的时间戳

（2）核心函数

odometryHandler(关键优化)

bash 复制代码

gtsam::ISAM2 optimizer;          // 增量平滑与建图优化器
gtsam::NonlinearFactorGraph graphFactors;  // 因子图
gtsam::Values graphValues;       // 优化变量值
int key = 0;                     // 关键帧索引

2.4 mapOptmization 部分

（1）核心函数

cornerOptimization(角点优化)

点线距离计算：

对每个下采样后的角点，在地图角点云中寻找5个最近邻点
通过主成分分析（PCA）判断这5个点是否构成一条直线
如果最大特征值 > 3 * 次大特征值，则认为构成直线
计算当前点到直线的距离，构建点到线的距离残差

并行加速 ：使用OpenMP多线程并行处理
残差加权：根据距离大小对残差进行加权，距离越近权重越高

surfOptimization

点面距离计算：

对每个下采样后的面点，在地图面点云中寻找5个最近邻点
通过最小二乘法拟合平面方程
计算当前点到拟合平面的距离
验证5个点是否都接近拟合平面（最大距离<0.2m）

平面有效性验证 ：确保5个点确实能构成一个平面
残差计算：计算点到平面的距离残差

LMOptimization

Levenberg-Marquardt优化：

构建雅可比矩阵matA和残差向量matB
求解正规方程：matAtA * matX = matAtB

退化处理：

第一次迭代时进行特征值分析
如果系统退化（某些方向不可观），将退化方向的特征向量置零
在退化情况下，通过投影矩阵matP修正优化方向

收敛判断：

计算旋转和平移的增量大小
如果ΔR < 0.05度且ΔT < 0.05cm，认为收敛
坐标系转换：代码中包含激光雷达到相机的坐标系转换注释，这是从LOAM继承的

extractSurroundingKeyFrames(提取局部地图)

从历史关键帧中提取空间相邻的关键帧
构建当前优化所需的局部地图：
通过KD-Tree查找当前位置附近的关键帧
将这些关键帧的特征点云转换到世界坐标系，拼接成局部地图

scan2MapOptimization(优化流程)

检查特征点数是否足够
设置 KD-Tree 输入
迭代优化（最多30次）：
角点优化
面点优化
合并系数
LM优化，如果收敛则提前退出
位姿更新

performLoopClosure(闭环检测)

复制代码

    1. 检测闭环候选 (通过Scan Context等)
    2. 构建局部地图 
    3. ICP匹配 (将当前帧(角点面点)与局部地图对齐)
    4. 计算变换矩阵
    5. 添加闭环约束到因子图
    6. 优化位姿图

loopFindNearKeyframes(关键帧回环)

功能：提取指定关键帧附近多个关键帧的点云，构建局部地图
输入：key - 中心关键帧索引，searchNum - 搜索范围
输出：nearKeyframes - 合并后的局部地图点云
用途：为ICP匹配提供"目标点云"，用于闭环检测的精确对齐

visualizeLoopClosure(回环可视化)

功能：可视化检测到的闭环约束
输入：loopIndexContainer - 存储闭环对（当前帧索引，历史帧索引）
输出：可视化标记（球形节点和连接线）

bash 复制代码

球形节点：表示参与闭环的关键帧位置
连接线：表示检测到的闭环约束
颜色编码：
	青色节点：易识别，不与其他元素冲突
	黄色边：醒目，表示重要的约束关系

用途：在RViz中显示闭环关系，便于调试和分析

（2）核心算法流程

bash 复制代码

接收特征点云
    ↓
更新初始位姿（IMU/里程计）
    ↓
提取局部地图（空间+时间临近关键帧）
    ↓
下采样当前扫描
    ↓
Scan-to-Map优化（迭代LM优化）
    ↓
变换更新与IMU融合
    ↓
判断是否保存关键帧
    ↓
添加因子（里程计/GPS/闭环）
    ↓
iSAM2增量优化
    ↓
更新关键帧位姿和地图
    ↓
发布里程计和TF
    ↓
闭环时全局修正

LIO-SAM 学习总结

目录

1 LIO-SAM 介绍

2 核心代码

2.1 imageProjection 部分

（1）主要订阅与发布主题

（2）关键数据结构与变量

（3）处理流程 (cloudHandler)

cachePointCloud(缓存与转换):

deskewInfo(获取去畸变信息):

imuDeskewInfo(IMU去畸变信息处理函数--角度积分)

odomDeskewInfo

projectPointCloud(点云投影与去畸变):

findRotation(基于IMU数据的时间插值功能)

cloudExtraction(点云提取):

publishClouds(发布结果):

2.2 featureExtraction 部分

(1) 处理流程

calculateSmoothness(计算点云中每个点的平滑度或曲率)

markOccludedPoints(判断前后点)

遮挡检测原理

平行光束检测原理

extractFeatures(提取角点和面点)

角点选择策略

平面点选择策略

2.3 imuPreintegration 部分

（1）算法介绍

算法数据流程

算法特点与优势

（2）核心函数

odometryHandler(关键优化)

2.4 mapOptmization 部分

（1） 核心函数

cornerOptimization(角点优化)

surfOptimization

LMOptimization

extractSurroundingKeyFrames(提取局部地图)

scan2MapOptimization(优化流程)

performLoopClosure(闭环检测)

loopFindNearKeyframes(关键帧回环)

visualizeLoopClosure(回环可视化)

（2）核心算法流程

（1）核心函数