LOAM论文与代码阅读（二）建图
上一篇学习了LOAM的里程计。里程计输入原始点云，得到了去畸变的点云和帧间位姿。但帧间位姿不断递推会导致误差不断积累，因此我们需要将帧（单帧或多帧）与地图进行匹配来优化位姿，同时建立全局地图。本篇就来学习LOAM中后端优化建图的过程。
整体流程
建图的整体流程如下：
建图的算法对每个sweep运行一次。
输入：在第k+1个sweep结束时，我们得到了去畸变的点云 \(\overline{P}_{k+1}\), 和这一帧相对于上一帧的变换 \(T^L_{k+1}\)。同时我们有一个地图\(Q_k\)，是由前k帧建立的全局地图点云，和上一帧建图时估计的位姿\(T_k^W\)，即\(t_k\)时刻lidar相对于世界坐标系的位姿。（世界坐标系即为\(Q_k\)建立的坐标系，常使用第一帧的坐标系作为世界坐标系）
输出：\(T_{k+1}^W\)
方法：根据\(T_k^W\)和\(T^L_{k+1}\)，我们可以得到\(T_{k+1}^W\)的一个估计值。用这个值我们将\(\overline{P}_{k+1}\)投影到世界坐标系，用\(\overline{Q}_{k+1}\)表示。把\(\overline{Q}_{k+1}\)与地图\(Q_k\)匹配，就可以优化位姿\(T_{k+1}^W\)了。
具体细节
- 提取特征点的方式与里程计相同，但是提取更多的特征点（10倍）。
由于地图更稠密，而且建图运行频率低，所以可以提取更多特征点？而且建图需要得到一个更准确的位姿估计。
对于地图中大量的点云如果全部存储成kd-tree，树的深度很大，不易存储。地图首先被划分成10m的体素格，再将与当前\(\overline{Q}_{k+1}\)有重叠区域的体素格中的点存成kd-tree.
对于\(\overline{Q}_{k+1}\)中的一个特征点i，我们先找在\(Q_k\)中在i附近一定区域内的点，设这些点构成集合S'。如果i是边缘点，我们只保留S'中的边缘点。如果i是平面点，我们只保留S中的平面点。
然后我们计算S'的协方差矩阵为M。计算M的特征值和特征向量得到V和E。如果i是边缘点，S'应对应一条边缘线，它的特征值应该有1个显著大于另外两个，对应的特征向量代表了这条边缘线的方向。如果i是平面点，S'应对应一个平面，它的特征值应该有2个显著大于另外1个，对应的两个特征向量代表了这个平面的方向。
然后我们取边缘线上的两个点，平面上的三个点，用和里程计里同样的方式计算距离。得到有关外参\(T^W_{k+1}\)的损失函数。同样用鲁棒拟合（bisquare weight）和LM方法来优化。
优化完成后，我们把第k+1帧点云加入地图。为了使得地图上的点均匀分布，对地图做5cm的体素降采样。
LOAM的不足之处
- LOAM整体流程比较繁琐，计算消耗大。每一帧都存入地图，在后期内存需求很大
- 没有后端优化，没有回环检测