LINS代码阅读（2022.8.1）

image_projection_node.cpp

1、话题接收：

/points_raw 雷达原始点云，回调函数为cloudHandler

2、话题发布：

/full_cloud_projected ----转换成图片的点云，原始点云
/full_cloud_info ---- 带有距离信息的点云
/ground_cloud ---- 地面点云
/segmented_cloud ---- 已经分割的点云
/segmented_cloud_pure ----具有几何信息的分割点云
/segmented_cloud_info
/outlier_cloud ---- 过滤掉的点云

3、类：

ImageProjection：
allocateMemory()-- 缓存分配;
resetParameters()-- 设置参数;

4、回调函数cloudHandler：

copyPointCloud ---- 将消息转换为pcl数据格式，去除无限远点。
findStartEndAngle ---- 计算一帧消息起止角度和角度范围。
projectPointCloud ---- 投影到有序的image中，记录点的行列和深度。根据点的竖直方向角度划分竖直方向索引，对于16线激光雷达，-15～15，分辨率为2，索引范围[0，15]；根据x,y方向夹角计算水平方向索引，对于16线激光雷达，分辨率为0.2，索引范围[0，1799]；计算点的距离，将距离放入rangeMat中（rowIdn行，columnIdn列），设置点的intensity = rowIdn + columnIdn / 10000，index = columnIdn + rowIdn * SCAN_NUM。
groundRemoval ---- 移除地面点。groundScanInd 是在 utility.h 文件中声明的线数，groundScanInd=5，这个参数应该是根据雷达安装高度确定，跳过空点；当点与XoY平面夹角和雷达装配角差值小于一定范围时认为该点为地面点，将地面点与无穷远点的label赋为-1，不参与聚类。
cloudSegmentation ---- 非地面点聚类。使用广度优先搜索遍历帧内所有的点，找出较为平整的连续的点，孤立的点舍弃，不参与聚类；将地面点、参与聚类的点和被舍弃的点分别保存转发。
publishCloud ---- 话题发布。
resetParameters ---- 重置参数。

lins_fusion_node.cpp

1、话题接收：

/segmented_cloud ---- 已经分割的点云
/segmented_cloud_info
/outlier_cloud ---- 过滤掉的点
/imu_raw ---- IMU原始消息
/integrated_to_init---- 融合到初始

2、话题发布：

/undistorted_point_cloud
/laser_cloud_sharp
/laser_cloud_less_sharp
/laser_cloud_flat
/laser_cloud_less_flat
/laser_cloud_corner_last
/laser_cloud_surf_last
/outlier_cloud_last
/laser_odom_to_init

3、类：

parameter：这是个命名空间，规定了一些系统的主要参数（相关文件路径：”src/LINS—LiDAR-inertial-SLAM-dev/lins/include/parameters.h”、”src/LINS—LiDAR-inertial-SLAM-dev/lins/config/exp_config/exp_port.yaml”）
LinsFusion：命名空间fusion下的类(相关文件路径：”src/LINS—LiDAR-inertial-SLAM-dev/lins/include/Estimator.h”、”src/LINS—LiDAR-inertial-SLAM-dev/lins/src/lib/Estimator.cpp”)

4、回调函数：

mapOdometryCallback：只进行了坐标转换
imuCallback：提取当前帧六自由度加速度，把测量从IMU坐标系转换到车体坐标系。IMU和车体在yaw上存在偏角，把测量转换到车体坐标系下。
确认接收到第一帧点云数据后对其初始化，首先检测是否初始化，执行processFirstPointCloud函数进行初始化，对第一帧点云进行处理：首先获取最新一帧的点云和IMU信息，导入processPCL函数进行处理，将点云从雷达坐标系转换至车体坐标系，计算当前帧点云的曲率，去除被遮挡点和噪点，并进行特征点提取（线点和面点）；初始化后，第二帧点云开始，与IMU数据进行匹配，策略是利用时间戳信息，找出可以被两个IMU帧包含的点云帧；找到与当前关键帧时间戳相近的IMU时间戳或雷达时间戳，计算上一关键帧到当前关键帧的时间差值，此时需要对扩展卡尔曼滤波器进行设置，具体步骤为：通过预积分计算出相对位姿，对线特征点和面特征点点云数据分别进行匹配，计算出jacobian矩阵（与LOAM不同，这边没有使用ceres进行求解），并对初始状态进行定义，假设车一开始静止，此时加速度应该为g，根据加速度在xy轴上的分量估计roll和pitch，根据点云匹配计算出的相对位置和速度，静止估计出的roll和pitch，给定的bias，进行运动畸变去除；在后续雷达帧，使用卡尔曼滤波进行状态变化估计（文件：src/LINS—LiDAR-inertial-SLAM-dev/lins/include/StateEstimator.hpp）：假设在相邻关键帧之间bias和重力g不变，根据IMU线性模型计算出位移与速度变化量，更新误差状态转移矩阵和误差状态协方差。

lidar_mapping_node.cpp（与LEGO-LOAM的mapOptmization.cpp一样）

1、话题接收：

/laser_cloud_corner_last
/laser_cloud_surf_last
/outlier_cloud_last
/laser_odom_to_init

2、话题发布：

/key_pose_origin
/laser_cloud_surround
/aft_mapped_to_init
/aft_xyz_mapped_to_init
/history_cloud
/corrected_cloud
/recent_cloud

3、类：

MappingHandler：主要的流程写在run函数中，首先根据从上一帧到当前帧的偏移量，计算当前帧在世界坐标系最优位姿估计，transformBefMapped是上一次构图时在世界坐标系下的Lidar里程计信息，即上一时刻的transformSum；第二步，搜索周围关键帧，并拼接成map，对待是否回环存在不同的处理方式，并进行下采样；第三步，scan2map位姿优化，更新transformBefMapped和transformAftMapped；第四步，判断是否需要新增关键帧，如果需要那么利用因子图进行全局位姿优化，并新增关键帧；最后将优化后的信息发布。

4、回调函数：

laserOdometryHandler：保存里程计信息，transformSum前三个信息分别存三轴旋转信息，后三个存三轴平移量。

transform_fusion_node.cpp

1、话题接收：

/laser_odom_to_init ---- 雷达里程计到初始位置
/aft_mapped_to_init

2、话题发布：

/integrated_to_init

3、类：

TransformFusion

4、回调函数：

laserOdometryHandler
odomAftMappedHandler

参考链接

知乎专栏
论文地址
代码地址
大佬LEGO-LOAM代码地址，带详细注释

第一次写关于代码的文章，整体阅读了一周时间，可能有些地方有遗漏或者错误，希望各位大佬能给指出来。有些地方还不是很明白，在这边做一个学习记录，弄明白后进行更新。