《LIO-SAM阅读笔记》2.ImageProjection&featureExtraction

前言：

LIO-SAM中ImageProjection类和featureExtraction类的内容比较简单，这里就放在一起记录，这两部分属于点云的预处理前端，ImageProjection用来点云的运动去畸变和为每帧点云位姿进行粗略初始化，featureExtraction类用来提取特征点，包括角点和面点。

一：ImageProjection类

功能简介:

1、利用当前激光帧起止时刻间的imu数据计算旋转增量，IMU里程计数据（来自ImuPreintegration）计算平移增量，进而对该帧激光每一时刻的激光点进行运动畸变校正（利用相对于激光帧起始时刻的位姿增量，变换当前激光点到起始时刻激光点的坐标系下，实现校正）；

2、同时用IMU数据的姿态角（RPY，roll、pitch、yaw）、IMU里程计数据的的位姿，对当前帧激光位姿进行粗略初始化。

订阅：

1、订阅原始IMU数据；

2、订阅IMU里程计数据，来自ImuPreintegration，表示每一时刻对应的位姿； 

3、订阅原始激光点云数据。

发布：

1、发布当前帧激光运动畸变校正之后的有效点云，用于rviz展示；

2、发布当前帧激光运动畸变校正之后的点云信息，包括点云数据、初始位姿、姿态角、有效点云数据等，发布给FeatureExtraction进行特征提取。

位姿初始化

在deskewInfo函数中有两个子函数：

// 当前帧对应imu数据处理
// 1、遍历当前激光帧起止时刻之间的imu数据，初始时刻对应imu的姿态角RPY设为当前帧的初始姿态角  //！：IMU_RPY
// 2、用角速度、时间积分，计算每一时刻相对于初始时刻的旋转量，初始时刻旋转设为0
// 注：imu数据都已经转换到lidar系下了
imuDeskewInfo();

// 当前帧对应imu里程计处理
// 1、遍历当前激光帧起止时刻之间的imu里程计数据，初始时刻对应imu里程计设为当前帧的初始位姿
// 2、用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量
// 注：imu数据都已经转换到lidar系下了
odomDeskewInfo();

下面两个功能函数，一个将当前时刻imu的原始PRY当做cloudInfo.imuPitchInit....，一个将imu里程计的PRY当做cloudInfo.initialGuessPitch.....，有什么区别？

• imu当前时刻的原始RPY在两个地方进行了使用：
  1）作为 第一帧 雷达的初始姿态角
  2）用于scan-to-map之后的姿态角融合。
• imu预积分的RPY用于第一帧以后得每帧雷达的初始姿态角

二：featureExtraction类

功能简介:

对经过运动畸变校正之后的当前帧激光点云，计算每个点的曲率，进而提取角点、平面点（用曲率的大小进行判定）。

订阅：

1、订阅当前激光帧运动畸变校正后的点云信息，来自ImageProjection。

发布：

1、发布当前激光帧提取特征之后的点云信息，包括的历史数据有：运动畸变校正，点云数据，初始位姿，姿态角，有效点云数据，角点点云，平面点点云等，发布给MapOptimization；

2、发布当前激光帧提取的角点点云，用于rviz展示；

3、发布当前激光帧提取的平面点点云，用于rviz展示。

计算曲率：

        用当前激光点前后5个点计算当前点的曲率，平坦位置处曲率较小，角点处曲率较大；这个方法很简单但有效。

判断遮挡和平行：

1.两个点在同一扫描线上，且距离相差大于0.3，认为存在遮挡关系（也就是这两个点不在同一平面上，如果在同一平面上，距离相差不会太大）。远处的点会被遮挡，标记一下该点以及相邻的5个点，后面不再进行特征提取。

 2.用前后相邻点判断当前点所在平面是否与激光束方向平行。

点云角点、平面点特征提取

曲率大于一定的阈值则认为是角点，否则是平面点，同一条扫描线上角点和面点前后5个点标记一下，不再处理，避免特征聚集。

1、遍历扫描线，每根扫描线扫描一周的点云划分为6段，针对每段提取20个角点、不限数量的平面点，加入角点集合、平面点集合。

2、认为非角点的点都是平面点，加入平面点云集合，最后降采样。