LOAM详解（三）激光建图

一、Odom到Map坐标变换预处理

将上一帧里程计坐标和地图坐标的相对变换作为这一帧的初值

二、Submap的构建和更新

首先定义子图大小，伪子图为212111，真子图为553，如上图所示:灰色部分为伪子图，黄色部分为真子图。
1. 伪子图的更新
我们要控制current_scan在伪子图中的位置在3～18，3～18，3～8以内，即尽量控制其在中心位置
如果出界则采用以下方案


2. 真子图的构建
中心点的位置怎么确定？
通过先前预测的位置确定
获取中心格子也就是上图蓝色格子周围553范围内的所有点云组合成submap，用于scan-submap的匹配。同时还会对这个存放角点和平面点的submap进行过滤。

三、构建scan-submap点到线和点到面的距离

在后端的当前帧与地图匹配的时候，我们就需要从地图里寻找线特征和面特征的约束对，此时，由于没有了线束信息，我们就需要采取而外的操作来判断其是否符合线特征和面特征的给定约束。

1. 点到线计算方法
   通过KDtree在地图中找到5个最近的线特征，为了判断他们是否符合线特征的特性，我们需要对其进行特征值分解，通常来说，当上述5个点都在一条直线上时，他们只有一个主方向，也就是特征值是一个大特征值和两个小特征值，最大特征值对应的特征向量就对应着直线的方向向量。
   
   
   这里调用了eigen的SelfAdjointEigenSolver计算特征值特征向量

Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);

2. 点到面的计算方法
   同样首先通过KDtree在地图上找到最近的面特征，原则上面特征也可以通过特征分解的方式，选出最小特征值对应的特征向量及平面的法向量，不过代码里选用的是平面拟合的方式：
   我们知道平面方程$Ax+By+Cz+D=0$，考虑到等式的形式，可以进一步写成$Ax+By+Cz+1=0$，也就是三个未知数，五个方程，写成矩阵的形式就是一个5*3大小的矩阵，求出结果后，我们还需要对结果进行校验，来观察其是否符合平面约束，具体就是分别求出5个点到求出平面的距离，如果太远，则说明该平面拟合不成功。
   
   计算 ABC 使用了 QR 分解

四、Lidar Mapping 原理介绍

其中最后整合出一个10HZ的位姿输出方法是：

可以看到，q_w_curr以1HZ的频率进行更新，其他情况下用的都是未更新量。