LOAM源码解析3——laserMapping

SLAM系统前面大多是先估计相邻两帧之间相对位姿估计，接下来开始建图，然后根据建图结果对现有姿态进行优化，最后把当前这特征点填充进地图，且估计当前帧相对于世界坐标系下的位姿。

在扫描周期末尾激光雷达里程计输出畸变矫正过的点云$\widehat{P_{k+1}}$，同时还有整个周期内每个点的姿态$T_{k+1}^L$。雷达建图过程是为了把$\widehat{P_{k+1}}$在世界坐标系下匹配。在得到相邻帧的姿态变换信息后，需要将其和全局地图进行匹配，并将其加入到全局地图中。

主要任务：

1. 点云$P_{k+1}$去畸变，对齐到世界坐标系，$\widehat{Q_{k+1}}$
2. $\widehat{Q_{k+1}}$和 $Q_k$配准，优化位姿$T_{k+1}^W$

参数定义：

• k次扫描累积的地图点云集 $Q_k$
• 第k次扫描结尾，也是k+1帧$t_{k+1}$地图上雷达的初始位姿 $T_k^W$

已知：累积的地图$Q_k$,根据位姿投影的当前帧点云$\widehat{Q_{k+1}}$，粗略的每个点云位姿$T_{k+1}^W$
输出：优化后精密的位姿$T_{k+1}^W$
方式：通过不断优化雷达位姿$T_{k+1}^W$将$\widehat{Q_{k+1}}$和 $Q_k$匹配上。

利用里程计输出的位姿$T_{k+1}^L$，将$T_k^W$不断补充$t_{k+1}$到$t_{k+2}$时间范围内雷达相对于世界坐标系的位姿，最终获得$T_{k+1}^W$.
将$\widehat{P_{k+1}}$通过粗略估计的位姿$T_{k+1}^W$投影到世界坐标系上，表示为$\widehat{Q_{k+1}}$。

map-to-map通过不断匹配$\widehat{Q_{k+1}}$和 $Q_k$优化位姿$T_{k+1}^W$，如果用上所有地图数据计算量很大，这里使用一个边长为10m的立方体替代全局地图。且$\widehat{Q_{k+1}}$是10帧雷达扫描的数据，存入到KD-tree中。

在 $Q_k$中选取相邻点集合S，计算S的协方差矩阵M、特征向量E、特征值V。选取边缘线和平面块方式为：
边缘线：V中特征值一大两小，E中大特征值代表的特征向量代表边缘线的方向。
平面块：V中一小两大，E中小特征值对应的特征向量代表平面片的方向。
边缘线或平面块的位置通过穿过S的几何中心来确定。

这样就可以快速找到$\widehat{Q_{k+1}}$中的点$i$，和$Q_k$中的边缘点$j,l$以及平面点$j,l,m$然后利用LM算法求解$T_{k+1}^W$。

代码讲解

首先介绍重要的参数。

//1.控制接收到的点云数据，每隔几帧处理一次
const int stackFrameNum = 1;
//控制处理得到的点云map，每隔几次publich给rviz显示
const int mapFrameNum = 5;

// 2.立方体cube变量
int laserCloudCenWidth = 10;// cube宽、高、深度
int laserCloudCenHeight = 5;
int laserCloudCenDepth = 10;
const int laserCloudWidth = 21;// 子cube沿宽、高、深度分割个数
const int laserCloudHeight = 11;
const int laserCloudDepth = 21;
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;//子cube总数4851

//3.lidar视域范围内(FOV)的点云集索引
int laserCloudValidInd[125];
//lidar周围的点云集索引
int laserCloudSurroundInd[125];

// 4.pcl点云数据
//最新接收到的特征点（边缘+平面）
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());
//存放当前收到的特征点 （下采样之后的）(in the local frame)
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());
//存放当前收到的特征点（下采样之前的）
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack2(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack2(new pcl::PointCloud<PointType>());
//原始点云坐标和权重
pcl::PointCloud<PointType>::Ptr laserCloudOri(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr coeffSel(