在上一篇中对 LOAM-Livox 算法做了整体的总结。 最后给出了 算法的整体框图 如下： 
本篇主要对LOAM-Livox 算法进行 前端处理 和 迭代位姿优化 的 总结
1 前端处理
LOAM-Livox 算法 的 前端 处理 也就是红框的部分 
主要包含两个环节
- good points 选取
- 特征点提取
1.1 good points 选取
考虑到激光雷达低等级的物理特性（点云光斑大小、信噪比），通过点的等级机制来选取出“优点”
1.1.1 距离
激光雷达直接输出的点的格式可以选择 点云格式 X、Y、Z 分别代表雷达的前、左、上的方向坐标轴，雷达输出的三维坐标点P则可以用(Px,Py,Pz)表示 测量点P到雷达 的 距离计算公式如下 
筛选：去除掉过近或过远的点。
1.1.2 偏转角
雷达的偏转角是雷达射线和X轴的夹角，计算公式如下 
筛选：接近边缘的点，通过偏转角的大小可以判断是否接近边缘 对于 Livox MID40，偏转角大于17，则舍去。
1.1.3 强度
雷达直接返回量 如果雷达直接返回的是反射率，则通过下面公式计算 
R是物体的反射率，通过雷达传感器测量得到（一些雷达比如, Velodyne Puck，直接返回强度而不是反射率。 筛选：去除掉强度过大或过小的点。强度较小意味着测量点离雷达较远，或者物体的反射率很低。强度过大怎距离较近。
1.1.4 入射角
入射角，是雷达射线和测量点周围的平面的夹角 其中 a点和c点是b点相邻的两个，关系如下图所示 
计算公式如下 
公式的含义就是通过向量点乘的方式，计算向量ca和ob的夹角。 
筛选：入射角接近π或者0，去除。 Livox MID40的范围为5°-175°，像图中的f点，因为这样角度入射的激光点会被拉长。
1.1.5 遮挡点
说白了就是藏在物体后面的点，想上图的e点。 通过下面的公式判断 这种点 
筛选：去除这种点，这样的点很容易造成边缘特征的误识别。
1.1.6 总结
去除掉如下的点：
- 距离过大和过小的点
- 接近边缘的点
- 强度过大的点和过小的点
- 入射角接近π或者0
- 藏在物体后面的点
2 后端迭代位姿优化
由于固态雷达为非重复式扫描，提取的特征点不能持续的被跟踪。 即使雷达静止不动，扫描的轨迹和特征点和前一帧比也是不同的。 LOAM-Livox 算法 用了一种迭代式位姿优化方法来计算雷达的位姿。 实现了实时里程计和建图，频率20HZ
2.1 边缘到边缘的残差
εk 表示 当前帧的所有边缘特征点 εm 表示 地图中所有边缘特征点 对于εk中的每一个点，从εm中找5个和这个点最近的点，向下面那样 
为了提高搜索的速度，建立εm（地图所有边缘特征点）的KD-tree， KD-tree由另一个线程维护，由最新的雷达数据帧实时更新 这样当新的帧来时，KD-tree可以立即被使用。 Pl 为当前帧(k帧)中的边缘特征点(εk )中的一个点 注意Pl是在雷达坐标系下的点，然而εm中的点是地图坐标系下的点。 为了在εm中找到和Pl最近的点，需要将Pl转换到地图坐标系下，通过下面的方式 
(Rk, Tk)是 当前帧中最后一个被采集的点 时刻的 雷达的位姿，需用通过位姿优化得到。 在这使用这个雷达位姿作为当做帧所有点的雷达位姿 用这个位姿，当前帧所有点就可以投影到地图坐标系下。 Pi表示第i个离Pw最近的点在εk中， 为了保证Pi在一条直线上，计算m个离Pw最近点的平局值µ和协方差矩阵Σ，设置m为5 如果协方差矩阵Σ的最大特征值比第二大特征值大三倍以上，那么最近的这个几个点就在一条直线上 残差的计算公式如下： 
这个公式的含义： 通过向量叉乘求点到线的距离 假设给出三个点，A，B和C，你想找出点C到点A、B定出的直线间距离。 就是d = (AB x AC)/|AB|
2.2 面到面的残差
和边缘到边缘的残差类似，对于一个点在面的特征点计里面，以Pk为当前帧的一个面的特征点，从地图坐标系下找5个最近的点以Pm表示。 假设这5个最近的点在一个平面上，通过计算他们协方差矩阵。 如果最小的特征值要小于第2小的特征值的3倍。那么则认为在一个平面上。 通过下面的公式计算残差 
公式的含义就是计算点到面的距离（法向量法）
2.3 运动畸变校正
像之前提及的， 当雷达一直在移动，三维点将会在不同时刻不同位置被采集。 减小运动畸变的影响，做如下两种方法的处理
2.3.1 分段处理
一种简单但是有效的方法来减小运动畸变的影响就是通过分段处理。 将输入的一个帧分成3个连续的子帧。 然后，这个三个子帧 和 当前建立的地图 分别 做匹配 在子帧扫描匹配的过程中，通过子帧中最后一个点时刻雷达的位姿，子帧里面的所有点转换到地图坐标系下 通过这样做，每一个子帧的时间间隔是原始帧的三分之一 分段处理在算法中通过多线程的方式并行处理子帧匹配，相当于算法加速。
2.3.2 线性差值
经常用的进行运动畸变校正的方法就是通过线性差值的方式。 用(Rk,Tk)代表当前帧的最后一点的雷达位姿 (Rk-1,Tk-1) 是 前一帧的最后一点的雷达位姿 
是前一帧和当前帧的旋转和平移关系 当前帧的最后一点的雷达位姿和 前一帧的最后一点的雷达位姿 则存在如下关系 
设t(k-1)是前一帧雷达里面的最后一个点的采样时刻 对于当前帧的每一个点都存在采样时间t t∈[t(k-1),t(k)] s是时间的百分比 
可以算出在t时刻的线性插值的位姿 先求出和前一帧最后一点的旋转和平移关系 
其中θ 
ω 是 其旋转向量 ω ^是ω 的斜对称矩阵 
有另一种更快速的计算方式 
上面式子仅有 sin(sθ) 和cos(sθ)需要计算，其它的都是固定值，这样计算可以减小计算时间 最终在t时刻雷达的位姿为 
将这个点投影到地图坐标系下 
3 迭代位姿优化算法流程
在执行迭代位姿优化的每次迭代中，重新找每个特征点的最近邻点，并且在目标函数加入边缘残差和平面距离残差。首先迭代2次，用结果计算残差，剔除最大的20％点云，然后做完全的位置优化。 
下面一篇做 LOAM-Livox 算法 代码研究
