LOAM框架后端优化总结

LOAM，即Lidar Odometry and Mapping in Real Time

一、Lidar Mapping 原理

在 Lidar Mapping 部分，通过与多次扫描构成的地图进行匹配，消除Lidar Odometry 中利用两帧之间计算位姿产生的漂移，从而对轨迹进行优化；

输入：

1. $T_{K+1}^L$表示lidar odometry 模块输出的$k+1$时刻相 对于起始帧的位姿变换;
2. $T_K^W$表示截止到k时刻已经别lidar mapping优化后的位姿
3. ${\bar{P}}_{K+1}$表示$k+1$时刻去畸变（对齐到末尾时刻）的单帧点云；
4. $Q_k$表示截止到$k$时刻已经构建好的点云地图；

流程：

1. 通过$T_{K+1}^L$和 $T_K^W$可以求取出优化前的$T_{K+1}^W$
2. 通过$T_{K+1}^W$和${\bar{P}}_{K+1}$可以求出世界坐标系下的点云${\bar{Q}}_{K+1}$
3. 通过${\bar{Q}}_{K+1}$和$Q_k$ 的配准，可以得到优化后的$T_{K+1}^W$，进而得到优化后的$Q_{K+1}$

关键：

1. 按照同样的方式从${\bar{Q}}_{K+1}$中提取特征点，但是比激光里程计中多十倍；
2. 为了寻找特征点的匹配，把地图$Q_k$ 存储在10m 的立方体区域中，同时将其中与${\bar{Q}}_{K+1}$有交集的区域建立成KD-tree;
3. 对于${\bar{Q}}_{K+1}$中的每个特征点在交集（KD-tree）搜索邻域, 得到邻域点并保留其中的对应线和对应面；
4. 通过约束优化角点到线平面点到面的距离，来优化位姿$T_{K+1}^W$

二、 通过$T_{K+1}^L$和 $T_K^W$可以求取出优化前的$T_{K+1}^W$以及${\bar{Q}}_{K+1}$

即可计算出绕yxz的旋转角，并基于优化前的位姿$T_{K+1}^W$和${\bar{P}}_{K+1}$,求取待配准点云${\bar{Q}}_{K+1}$。

三、 Scan-To-SubMap匹配

如果使用全部的地图数据，在计算效率上会大打折扣，所以，建图环节将整个空间三维区域划分为多个子cube(边长为10m的立方体)，并接着计算当前帧位姿所在的cube索引，然后在当前帧位姿附近$5\ast 5\ast 5$个cube中寻找最有效的cube作为$Q_k$，用以代替全局地图，优化得到最终的姿态变换矩阵 $T_{K+1}^W$。

首先将特征地图存入kd树，设置迭代循环。对于边特征点，先转到全局坐标系，再在KD树中搜索5个邻近点。接着求5个邻域点的均值点，并构建协方差矩阵，进行特征值分解，通过最大特征值对应的特征向量构建直线。

对于面特征点，上一章帧间匹配通过kd树查找3个最邻近点构成平面，然后通过论文中公式计算点到面的距离。本章地图匹配则仍然是先搜索5个最近点，由于这5个点在一个平面上，代码中直接通过矩阵运算求解5个点构成平面的法向量。

得到当前帧线特征点和面特征点与地图配准后，构建loss函数求解。

如果邻域点分布在一条线段上，那么matD1中的一个特征值就会明显大于另外两个，matV1中与较大特征值对应的特征向量就代表线的方向。
如果邻域点分布在一块平面上，那么matD1中的一个特征值就会明显小于另外两个，matV1中与较小特征值对应的特征向量就代表平面的方向。