loam 框架流程描述

前端流程(scanRegistration.cpp)

多线激光雷达即有多个激光发射器同时工作，如常见的 Velodyne16,就是共有 16 个激光发射器，一般这些发射器竖排排列，然后一起水平旋转。
激光雷达在一定的时间内旋转一圈，即一帧的点云数据。值得注意的是，在一帧时间内激光雷达的载体也会运动，因此，一帧数据内会包括不同时间的点云，这也就是激光雷达的运动畸变。
下图是kitti数据集的坐标系表示

loam论文中的框图

1.scanRegistration.cpp

main函数：

1. 建立scanRegistration节点
2. 设置雷达线束(64)，最小范围(5m)
3. 订阅雷达原始点云信息，发布的信息包括有效点云、大曲率角点、稍微大曲率角点、小曲率面点，稍微小曲率面点，主要处理过程在原始点云信息的回调函数

laserCloudHandler回调函数

1. ros格式转pcl点云，移除空点及车体范围内的点
2. 角度补偿， 计算起始角度和结束角度，确保相差2 PAI
3. 遍历点云，根据俯仰角计算对应的扫描线，根据水平角计算当前角度距离起始角度的差值，根据差值计算时间戳，用于后面进行运动补偿，将点云存储进对应的扫描线。这里注意intensity属性整数部分存储的是扫描线的索引，小数部分是相对于起始时刻的时间
4. 提取特征并均匀化。首先计算所有点云曲率，将每个扫描线分为6等分，将角点曲率从小到大排序，每段取最大的两个作为大曲率角点，次大的20个作为稍微大曲率的角点，取曲率最小的4个作为面点，对面点进行下采样滤波
5. 发布点云信息，包括包括有效点云、大曲率角点、稍微大曲率角点、小曲率面点，稍微小曲率面点

2.异常点筛选

这一步其实是需要放在特征提取之前做的
但是这块A-loam中没有，借助lio-sam中featureExtraction.cpp讲解

首先遍历所有点云，取出相邻两点的距离信息，计算两个点的列数id差。由于在筛选时会去除空点，因此防止两个有效点云虽然相邻但是间隔很远。
根据深度信息筛选下图中b的情况，根据相邻点的距离差，筛选下图中a的情况。

3.激光运动畸变及补偿

激光雷达的一帧数据是过去一段时间而非某个时刻的数据，在这一帧时
间内的激光雷达或者其载体通常会发生运动，因此，这一帧数据的原点都不一致，运动补偿的目的就是把所有的点云补偿到某一个时刻，这样就可以把本身在过去100ms 内收集的点云统一到一个时间点上去
$$P_{start}=T_{star{t}_{c}urrent}\ast P_{current}$$
运动补偿需要知道每个点该时刻对应的位姿$ T_{start_current $，通常有几种做法：

1. 如果有高频里程记，可以比较方便的获取每个点相对起始扫描时刻的位姿
2. 如果有 imu，可以方便的求出每个点相对起始点的旋转
3. 如果没有其他传感器，可以使用匀速模型假设，使用上一个帧间里程记的结果作为当前两帧之间的运动，同时假设当前帧也是匀速运动，也可以估计出每个点相对起始时刻的位姿

代码对应laserOdometry.cpp
去畸变
TransformToStart()函数 将输入的点云坐标转到起始时刻

1. 输入点云信息
2. 对于旋转，使用Eigen的四元数插值得到当前时刻的四元数
3. 对于平移，使用根据时间占当前周期的比例直接相乘
4. 由于kitti里面是补偿过的，因此这里比例都设置为1
5. 输出去畸变后的点云

TransformToEnd()函数 将输入的点云坐标转到结束时刻

1. 先调用TransformToStart()将输入点云转到起始时刻
2. 根据帧到起始时刻到结束时刻的位姿信息，将点云转到结束时刻

主函数

1. 定义节点laserOdometry
2. 设置参数
3. 订阅scanRegistration发布的角点、面点、有效点云5个topic，每个回调函数只负责把输入存入数组
4. 发布5个topic，分别是
5. while循环实现激光里程计

while循环

1. 触发一次回调函数（spinOnce）
2. ros消息转为pcl格式
3. 第一帧的点云信息不处理，只加入kd tree。对第二帧开始构建约束求解，对于点线约束，构建当前点和上一帧两点的距离约束，两个点分别是kd tree中最近的点和不同扫描线上较近的点；对于点面约束，找的三个面点分别是kd tree最近的点、相同扫描线较近的点、不同扫描线较近的点。
4. 根据约束构建ceres优化器，迭代4次进行优化
5. 递推更新相对世界系的位姿
6. 将当前帧点云加入kd tree,发布雷达里程计信息、路径、将较平坦的面点、曲率较大的角点、有效角点转为ros发布，用于后端

后端流程

laserMapping.cpp

main()函数

建立laserMapping节点，加载体素滤波参数，订阅前端点云和里程计信息，回调函数就是把信息存进队列。将后端数组复位，建立process线程进行处理

process()函数

1. 里程计、点云、面点队列的首个元素时间戳都和角点队列的首个元素时间戳对齐，然后将首个元素都转为ros格式并弹出，清空角点数组以保证每次处理的实时性
2. 将前端得到的位姿转到后端地图下$$T_{map\_cur}=T_{map\_odom}\ast T_{odom\_cur}$$
   得到当前帧在地图中的初始估计
3. 根据初始估计确定在栅格中的索引，每个栅格是505050的正方体，宽21个(x)、高21个(y)、深11个(z)，一共4851个。如果当前点的栅格太靠左，就将整体右移，将第i-1个栅格点云放到第i个，靠近右边界就左移，其他两个轴也这样操作，这样移动完后会空出一列。经过这个步骤保证当前的位姿步骤栅格地图的边缘。
4. 取出当前栅格附近 250250150的局部小地图做当前帧的配准，取出小地图的栅格id以及点云信息。
5. 对当前帧的角、面特征下采样。下采样后如果还有足够的特征，进行优化。对于点线约束，先将角点点云投影到当前坐标系下，使用kd tree从小地图找到5个最近的角点，最远的必须小于1m，然后计算这个五个点的协方差矩阵，使用eigen进行特征值分解，如果最大特征值大于次大特征值的3倍，就认为是线特征，可以添加到ceres优化器。对于点面约束，同样kd tree搜索5个最近面点，计算其法向量，然后判断每个点到平面的距离，都小于0.2则将当前点和法向量送入优化器，开始优化。
6. 进行位姿更新
   $$T_{map\_odom}=T_{map\_cur}\ast T_{odom\_cur}^{-1}$$
7. 将当前帧优化后的点云加到局部地图，将特征投影到地图坐标系，计算其在栅格中的位置，得到索引后加入一维数组。
8. 对局部地图进行体素滤波，然后发布位姿、轨迹、tf、点云信息。