Simple-LIO-SAM——（六）特征提取模块

发表于 更新于 分类于 ， 阅读次数： Changyan：

点云特征提取

Simple-LIO-SAM总体流程图

LIOSAM沿用LOAM、LeGO-LOAM的做法，从原始点云中提取出边缘点和平面点。边缘点和平面点被下游模块用来做点云匹配和构建地图。 特征提取模块是比较简单的模块，只监听来自ImageProjection发布出来的去畸变后点云，同时将提取出的特征点云发布出去。这里有几点需要注意：

  1. 特征点的提取根据点的平滑度，而平滑度则通过每个点与周围点距离的平均值做近似
  2. 实际上在提取完边缘点和平面点之后，LIOSAM在代码里又把所有非边缘点的点都加入了平面点集合。这是比较奇怪的地方。从这里看出也许特征提取也不是必要的， 因为平面点已经包含了大部分的点云。事实上，在Github的一个仓库liorf中，就将特征提取模块去掉。
1
2
3
4
5
6
7
8
9
// 将该段中除了角点之外的点加入平面点集合
// ！！！ 这点让步骤2感觉是多余的，最终的结果可能只是原始点云降采样，可能特征点提取也是没有必要的！！！
// cloudLabel标识该点的性质，-1为平面点，0为非特征点，1为边缘点。
for (int k = sp; k <= ep; k++)
{
    if (cloudLabel[k] <= 0){
        surfaceCloudScan->push_back(extractedCloud->points[k]);
    }
}

特征点提取原理

关于特征点提取部分LIOSAM的论文基本略过，因为跟LOAM和LeGO-LOAM的做法基本相同。本文对算法中点云特征点提取做详细介绍。

转换到RangeMat

LIOSAM沿用LeGO-LOAM的做法，先把三维点全部投影到二维图片，图像像素保存每个点距离原点的距离，称做RangeMat，RangeMat是规则的，每一行代表一条激光线束扫射360度所产生的点，因此后面用这个RangeMat做特征提取更快速更简单。 PS：这部分是在ImageProjection就已经处理好，并且RangeMat也被展开成一维向量存储。

有一点要注意，在一些激光雷达中，RangeMat是可以直接从雷达驱动提供的接口拿到，但并不是所有雷达都提供这样的API。据笔者的了解，ouster的雷达驱动API就提供直接获取RangeMat的功能。

平滑度估计

特征点提取通过每个点的平滑度来提取，平滑度高于某个阈值的认为是边缘点；平滑度小于某个阈值的认为是平面点。 而平滑度的估计则通过每一个点在该激光线成像的前后几个点的平均距离代替。 我们令时间\(t\)的点云为\(P_t = \{p_i, i \in |P_t|\}\)，同时我们可以根据每个点的坐标算出每个点距离原点的距离\(r_i\)，对于每个点，我们取其同一条激光线上前后5个点构成集合\(S\)，则，点\(p_i\)的平滑度可以这样计算：

\[ c = \frac{1}{|S|\cdot||r_i||}\left|\left|\sum_{j\in S,j\neq i}(r_j-r_i)\right|\right| \]

分段提取及特征点数量数量限制

为了让特征点分布更均匀，采用类似LeGO-LOAM，将整个360的图像在水平方向分成6个子图像，在每个子图像中进行特征点提取。并且，对于每一个子图像中的每一条激光线束，会限制边缘点数量不超过20个，平面点的数量则没有限制。

特殊情况

有3种特殊情况不能将该点设置为边缘点：

  1. 当一个点被选择为特征点，则把该点左右5个点都排除在特征点选取范围
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
// 对该角点左右各5个点，如果两点之间的列索引差距小于10，则抛弃周围的点，避免重复对同一块区域提取角点
for (int l = 1; l <= 5; l++)
{
    int columnDiff = std::abs(int(cloudInfo.point_col_ind[ind + l] - cloudInfo.point_col_ind[ind + l - 1]));
    if (columnDiff > 10)
        break;
    cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
    int columnDiff = std::abs(int(cloudInfo.point_col_ind[ind + l] - cloudInfo.point_col_ind[ind + l + 1]));
    if (columnDiff > 10)
        break;
    cloudNeighborPicked[ind + l] = 1;
}
  1. 当该点可能是由于遮挡导致被选择为边缘点
不合格的特征点(from LOAM)

如上图（b）所示，点A是由于被平面遮挡而产生的边缘点，当视角变化后，点A就不再是边缘点。 这个逻辑的判断也是通过点与其周围点的range差距判断。如果是由于遮挡产生的边缘点，则其与左边或者右边会存在距离的突变。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
// featureExtraction.cpp>markOccludedPoints
void markOccludedPoints()
{
    int cloudSize = extractedCloud->points.size();
    // 标记被遮挡的点和与激光束平行的点
    for (int i = 5; i < cloudSize - 6; ++i)
    {
        // 标记被遮挡的点
        float depth1 = cloudInfo.point_range[i];
        float depth2 = cloudInfo.point_range[i+1];
        int columnDiff = std::abs(int(cloudInfo.point_col_ind[i+1] - cloudInfo.point_col_ind[i]));
        // 两个点的列索引相差10个像素之内，认为是同一块区域
        if (columnDiff < 10){
            // 当前点距离大于右点距离0.3米，认为当前点及左边6个点无效
            if (depth1 - depth2 > 0.3){
                cloudNeighborPicked[i - 5] = 1;
                cloudNeighborPicked[i - 4] = 1;
                cloudNeighborPicked[i - 3] = 1;
                cloudNeighborPicked[i - 2] = 1;
                cloudNeighborPicked[i - 1] = 1;
                cloudNeighborPicked[i] = 1;
            // 当前点距离小于右边点距离0.3米，认为右边6个点无效
            }else if (depth2 - depth1 > 0.3){
                cloudNeighborPicked[i + 1] = 1;
                cloudNeighborPicked[i + 2] = 1;
                cloudNeighborPicked[i + 3] = 1;
                cloudNeighborPicked[i + 4] = 1;
                cloudNeighborPicked[i + 5] = 1;
                cloudNeighborPicked[i + 6] = 1;
            }
        }

        // 标记平行与激光束的点
        float diff1 = std::abs(float(cloudInfo.point_range[i-1] - cloudInfo.point_range[i]));
        float diff2 = std::abs(float(cloudInfo.point_range[i+1] - cloudInfo.point_range[i]));
        // 当前点与左右两点的距离均大于阈值，认为当前点是处于平行面的点
        if (diff1 > 0.02 * cloudInfo.point_range[i] && diff2 > 0.02 * cloudInfo.point_range[i])
            cloudNeighborPicked[i] = 1;
    }
}
  1. 当点处于跟激光线平行的平面上时，也可能被认为时边缘点，也需要排除 如上图中的(a)所示，点B位于一个跟激光线较为平行的面上，激光线扫过去，前后几个点的距离差距会很大，因此在处理时可能会被认为是边缘点。这些点也需要做标记，不要设置为边缘点。

特征点提取流程

由于这部分代码和原理都比较简单，这里只说一下特征提取的流程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
/**
* @brief
* cloudInfo话题的回调函数，这个模块的功能都是顺序进行
* 1. 接收到从imageProjection中发布出的一个去畸变点云信息cloudInfo(自定义格式)
* 2. 对每个点计算曲率。计算时是计算周围点的平均距离用来作为曲率的替代
* 3. 标记遮挡点和与激光平行的点，后续这些点不能采纳为特征点
* 4. 特征提取。分别做角点（曲率大）和平面点（曲率小）特征点提取
* 5. 整合信息，发布完整数据包
* 
* @param msgIn 从去畸变模块接受的数据包
*/
void laserCloudInfoHandler(const spl_lio_sam::msg::CloudInfo::SharedPtr msgIn)
{
// 缓存全局变量，后面的函数可以直接读取cloudInfo和cloudHeader进行处理
cloudInfo = *msgIn;
cloudHeader = msgIn->header;
// 把ros2 PointCloud2转成PCL格式，方便后面处理
pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud);

// 计算点云每一个点曲率
calculateSmoothness();

//标记遮挡点和与激光平行的点，后续这些点不能采纳为特征点
markOccludedPoints();

// 特征提取。分别做角点（曲率大）和平面点（曲率小）特征点提取
extractFeatures();

// 整合信息，发布完整数据包
publishFeatureCloud();
}

本项目所有章节

  1. Simple-LIO-SAM——（一）项目简介
  2. Simple-LIO-SAM——（二）环境搭建与运行
  3. Simple-LIO-SAM——（三）总体流程认识
  4. Simple-LIO-SAM——（四）utility文件解读
  5. Simple-LIO-SAM——（五）点云去畸变模块
  6. Simple-LIO-SAM——（六）特征提取模块
  7. Simple-LIO-SAM——（七）GTSAM快速入门
  8. Simple-LIO-SAM——（八）IMU预积分模块
  9. Simple-LIO-SAM——（九）点云匹配算法详解
  10. Simple-LIO-SAM——（十）后端优化模块详解