LOAM详解（一）特征点提取

---

参考链接

LOAM 论文
LOAM 算法论文核心关键点总结
LOAM 异常点剔除机制代码解析
系列内容
LOAM详解（二）激光里程计
LOAM详解（三）激光建图
LOAM详解（四）补充

特征点提取模块

一、对获得的点云数据进行预处理

1. 将ROS类型的雷达数据转化为PCL类型的点云(获得的点云中的每个点的位姿都是相对于当前雷达的)；
2. 对点云进行处理，分别是去除无效点和近点;
3. 异常点剔除：
   论文中还考虑了在匹配时会出问题的两种情况。一种避免选择周围点已经被选择的点，或局部平面上与激光束大致平行的点，下图的a。另一种避免位于遮挡区域边界上的点，例如下图b,因为连接表面被另外一个对象遮挡，激光运动过程中旋转了视角，有可能就看不到A了。
   

1. 剔除遮挡点：计算当前点与同线束下一点的距离差，当距离差大于0.3时，剔除较远点及该点另一边相邻的5个点。
2. 剔除平行点：当与前一点、后一点的距离差大于当前点自身距离的0.02倍时，认为平行点，直接剔除当前点。（没看懂）

二、计算每个点的角度，用于时间插值来去畸变

1. 计算起始点和终止点的角度（起始点的角度也代表，起始点对应列的所有角度，终止点同理）;
   
   $startOri\in [-\pi ,\pi ]\to -startOri\in [-\pi ,\pi ]$
   $endOri\in [\pi ,3\pi ]$
   $\therefore endOri-startOri\in [0,4\pi ]$
   $\because endOri-startOri\in [\pi ,3\pi ]$即我们只允许激光雷达终止角相比于起始角多转半圈或者少转半圈，所以要进行上图代码中的处理

• 加上负号的原因是其顺时针旋转，而坐标系是上逆时针取正，顺时针取负
• 在KITTI数据集中Y方向朝左X方向朝前，如上图右下图所示。
  因为在去除畸变后点云会出现如右下图所示的断层，而这个断层发生在起始点也就是Y轴方向，所以将Y朝左（认为前方物体相比于两侧更重要）
  

2. 通过计算每个点垂直方向角度将其归算到对应线上
   
3. 计算其他点的角度
   前半圈：按理说前半圈的角度和起始角的角度差在[0,π]以内，超过了就要对角度+π或-π处理，显然这会改变角度大小，而不改变角度的大小的方法是+2π和-2π，所以将角度差向左右两边各扩大π/2，即控制在[-π/2，3π/2]以内。
   后半圈：当按照前半圈的方式调整后发现角度差大于π，则认为进入后半圈，将角度+2π处理后于终止角进行比较。
   

• 注意：loam的强度值存的是scanID+其在一圈中的时间占比
• 这里有一个是否过半的判断，因为终止角我们可以通过+2π来计算其角度，但其他角我们无法这样操作。
  
  补：点云去畸变
  我们知道激光雷达的⼀帧数据是过去⼀段时间⽽⾮某个时刻的数据，因此在这⼀帧时间内的激光雷达或者其载体通常会发⽣运动，因此，这⼀帧数据的原点都不⼀致，会导致⼀些问题，即运动畸变。
  
  

三、计算每个点的曲率

1. 论文中计算曲率的方法
   曲率 = （当前点到其附近点的距离差 / 当前点的值）的总和再求平均 = 平均的距离差
   

2. 代码中的方法

• 去除前5个和后5个点
• 取当前点的左右两边各5个点计算曲率
  计算x5的曲率

        //有 x0，x1,x2,x3,x4,x5,x6,x7,x8,x9,x10
        //diffX = (x0+x1+x2+x3+x4+x6+x7+x8+x9+x10)-(10*x5)
        //同理得diffY，diffZ
        //曲率cloudCurvature = diffX * diffX + diffY * diffY + diffZ * diffZ

3. 改进思路
   点云随着扫描的范围增大，远处的点会很稀疏，这时再找附近的10个点能否还可以表示其局部特征呢？

四、提取角点和平面点

根据c(曲率)值进行排序，然后选择曲率大的为角点，曲率小的为平面点。
论文中为了在环境中均匀分布特征点，将扫描分为四个相同的子区域，如果是360度，也就是80度一个区域，每个子区域最多提取2个角点，4个面点。仅当点的曲率大于或小于阈值且所选点的数量不超过最大值，才将该点选为特征点。

选择特征点时需要满足条件：

1. 特征点数量满足子区域最大值，每个子区域的特征点数目是一定的。
2. 特征点周围点不能再次被选择，本来每个子区域的特征点数量就比较少了，进一步防止特征点集中分布。
3. 特征点非平行激光束的平面点，非被遮挡的平面点，非离群点。这些都是不可靠的点云数据，不能在连续两帧点云数据中得到，不能作为特征点。

代码中的方案

1. 将每条线分割成6段
2. 每段提取最多2个最角点和20个次角点（包含两个最角点）
   获取一个角点将其标记，并计算该角点和其前后五个点相邻两点间的相似度（距离），如果足够相似，则标记不再作为角点。
   
3. 每段提取最多四个平面点，并且对提取的平面点进行滤波