由四个部分组成。特征点提取、高频低精度odom(laserOdometry)、低频高精度odom(laserMapping)、双频odom融合功能。
归结为已知一段点云序列Pk，计算在前k个时期内的雷达位姿以及构建全局地图。
- 订阅雷达话题/rslidar_points和惯导话题/imu/data
- 通过激光角的角度，确认他们所属的scan。
- 通过imu的信息去除重力加速度影响的各个方向的线加速度，并转换到局部地球坐标系，得到假设的匀速运动的点
- 进行点的曲率计算（与周围五个点）（分成六段计算，升序排列）（分为是否less）（去除遮挡和重复的点
- scanRegistration主要完成点云的按线分类，当有imu数据时完成对应激光点的由加速度造成的匀速运动假设参数补偿，接着完成激光点曲率计算并提取特征点，最后发布处理结果。
- 订阅上一节点的话题，把按线分类后的点云分别存入不同的容器中（光滑和角点以及是否less，共四个），初始化后将分类后的点用于target匹配，并构建kdtree，初始化工作将第一帧初始点imu姿态角作为全局坐标系姿态角。
- 初始化后接收一帧新数据，配准source点云线特征点，进行一次LM优化（最多迭代25次），再通过TransformToStart()函数，得到该帧初始点坐标系相对当前激光点坐标系位姿关系，并将当前激光点转换至初始点坐标系。对于每一个线特征点调用TransformToStart()函数，将其转换至初始点位姿坐标系。每迭代五次，重新配准当前特征点，从上下三条线搜索次邻近点。
- 线特征点（两个点）构建距离函数，并配置权重s，保存满足距离条件的匹配结果。
- 特征面上的点配准与特征线类似，差异在于线特征点需要target点云中与之匹配的2个线特征点，而面特征点需要找3个匹配点。
- 面特征点配准，公式主要思想为利用四面体的边在高度方向的投影来计算。
- 构建代价函数并求解（对ex和x等求偏导），更新变换矩阵transform，后续通过transform计算全局位姿用到AccumulateRotation()函数（计算当前帧全局姿态角rx/ry/rz，将相邻帧位姿向量中的平移分量去除imu重力加速度产生的位置偏差，再转换到全局坐标系）
- 特征点云和总点云都经过TransformToEnd()函数处理，该函数主要功能为将当前帧激光点转换至当前帧末尾时刻并去除imu偏差。函数首先根据当前激光点的相对时刻插值位姿矩阵函数首先根据当前激光点的相对时刻，将激光点转换至当前帧初始点坐标系，然后再转换至末尾点坐标系。接着再补偿imu位置误差，由于imu位置误差是当前帧终止点在初始时刻坐标系，因此先旋转到地球坐标系，再旋转至终止时刻imu坐标系。
- 最后，发布当前帧的位姿tf，并每隔skipFrameNum发布一次用于下一章构建地图，同时发布总点云。
- 我们想要得到的是pk和pk+1之间的变换关系，也就是ek和ek+1（边沿点）以及hk和hk+1（平面点）之间的关系。 由于雷达自身在k和k+1时刻过程中是运动的，所以，我们每个点对应的姿态变换矩阵都应该得到一定的修正。为了方便处理，我们将所有的点重投影到每一帧的初始时刻，这样在这一帧中的所有点都可以得到对应的姿态变换信息。
- 通过LiDAR在map坐标系中的位姿T,将LiDAR坐标系下的特征点转到map坐标系下.针对map坐标系下的每个特征点,寻找与之接近的线或者面(corner对应线,surface对应面),然后计算点与线/面的距离,这个距离就是该点对应的loss.然后loss对位姿T求雅可比,使用高斯牛顿的方法,迭代优化T减小loss直到收敛。 *
- Paper与代码的差别:
paper中使用LM方法进行优化,而代码中使用的是高斯牛顿法则.
paper中使用的是angle-axis进行优化,而代码中使用的是旋转矩阵直接对欧拉角求导,最终优化的是欧拉角. - 建图环节将整个空间三维区域划分为laserCloudNum个子cube,通过一个while循环把时间对其，通过地图匹配前后的姿态以及初始姿态求出匹配的转换矩阵
- 通过上面求得的转换把面特征点和线特征点进行转换到全局坐标系并保存点云
- 确认一个点（0，10，0）并转换到全局坐标系，并查找所属的cube索引（分成了5x5x5个cube），进行cube的边界判断，得到当前帧全局位姿在cube中经过处理的索引，再通过中心点坐标以及八个顶点坐标计算与改点距离判断索引是否有效
- 将有效的cube索引所在的点云拼接到对应的map中再转换到lidar坐标系并滤波减少数据量，然后在进行线和面的匹配
- 对于面特征点，上一章帧间匹配通过kd树查找3个最邻近点构成平面，然后通过论文中公式计算点到面的距离。本章地图匹配则仍然是先搜索5个最近点，由于这5个点在一个平面上，代码中直接通过矩阵运算求解5个点构成平面的法向量。
- 匹配后在通过迭代更新位姿变换矩阵，在通过imu进行稍微补偿，最后将订阅的帧间匹配位姿赋值给transformBefMapped，将融合后位姿赋给最终的transformAftMapped
- 完成当前帧特征点与与地图的匹配优化后，将当前帧特征点云加入地图中（通过与之前cube索引的方法添加），特征点加入地图后再进行降采样和滤波，再发送处理后消息，首先发布当前帧附近laserCloudSurroundNum个cube中的特征点云，同样需要滤波处理。接着发布当前帧点云，需通过优化后的位姿转换到全局坐标系。最后发布优化后的lidar位姿并tf广播。
- 地图匹配章节主要是在前一章帧间匹配基础上优化lidar位姿，达到更好的里程计效果。
loss优化部分没看懂，arx等和代码上的没对应上