在上一篇中对 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 总结

  去除掉如下的点：  

1. 距离过大和过小的点
2. 接近边缘的点
3. 强度过大的点和过小的点
4. 入射角接近π或者0
5. 藏在物体后面的点

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 算法 代码研究