从零开始搭二维激光SLAM --- 使用单线雷达实现LIO-SAM中的特征点提取

我们这篇文章将对LIO-SAM中特征点提取的部分进行二维激光雷达下的实现。

实现的目的是为了熟悉激光雷达数据的处理方式，体验如何进行激光雷达数据处理，如何进行特征点提取。

LIO-SAM的项目地址为: https://github.com/TixiaoShan/LIO-SAM

LIO-SAM的特征点提取部分与LOAM基本相同,只不过在算曲率值时的具体计算方式稍有不同。

1 特征点提取后的效果

我们首先看一下激光原始数据的样子,如下图所示:

接下来,再看一下经过我们进行特征点提取之后的数据点的样子,如下图所示:

可以看到,经过特征点提取之后的数据点呈现出不太直观的样子,但是分布还是挺均匀的.

接下来,我们通过代码的角度来看一看如何进行特征点提取.

2 代码下载

从本节开始我将不再一步一步地进行代码的编写, 在文章里将只讲解重要部分代码的意义, 完整的项目请在github中下载.

github的地址, 请在我的公众号: 从零开始搭激光SLAM 中回复 开源地址 获得.

推荐使用 git clone 的方式进行下载, 因为代码是正处于更新状态的, git clone 下载的代码可以使用 git pull 很方便地进行更新.

3 运行环境

3.1 工程的目录

请将下载下来的代码文件夹 Creating-2D-laser-slam-from-scratch 放在 ~/catkin_ws/src/ 文件夹下.

3.2 bag文件的目录

并将bag数据放在 ~/bagfiles/ 文件夹下.

本篇文章对应的数据包, 请在我的公众号中回复 lesson1 获得.

3.3 运行环境

请按照上一篇文章中的 环境设置 进行设置.

3.3 代码运行

在终端中输入这行命令即可运行，launch我已经在工程里写好了，并配置好了rviz的显示模块

roslaunch lesson1 feature_detection.launch

3.4 launch文件讲解

bag文件的位置需要根据自己 电脑的用户名 来进行配置

首先，要将参数 use_sim_time 设置为true，代表当前ros的时间为bag文件中的时间。

同时，需要在播放bag时加上 –clock 选项，以表示使用bag文件中的时间为ros的当前时间。

同时启动了rviz，并加载了一个已经配置好的rviz配置文件。

<launch>

    <!-- bag的地址与名称 -->
    <arg name="bag_filename" default="/home/lx/bagfiles/lesson1.bag"/>

    <!-- 使用bag的时间戳 -->
    <param name="use_sim_time" value="true" />

    <!-- 启动节点 -->
    <node name="lesson1_laser_scan_node" pkg="lesson1" type="lesson1_feature_detection_node" output="screen" />
    
    <!-- launch rviz -->
    <node name="rviz" pkg="rviz" type="rviz" required="false"
        args="-d $(find lesson1)/launch/feature.rviz" />

    <!-- play bagfile -->
    <node name="playbag" pkg="rosbag" type="play"
        args="--clock $(arg bag_filename)" />

</launch>

4 代码讲解

代码中的注释很大程度上已经解释的很清楚了。

4.1 main

int main(int argc, char **argv)
{
    ros::init(argc, argv, "lesson1_feature_detection_node"); // 节点的名字
    LaserScan laser_scan;

    ros::spin(); // 程序执行到此处时开始进行等待，每次订阅的消息到来都会执行一次ScanCallback()
    return 0;
}

4.2 数据类型

smoothness_t 为自己定义了一个键值对的数据类型

by_value 为排序函数 sort 所用到的排序方法，表示从小到大进行排序，依赖 smoothness_t 中的value的大小进行从小到大的排序。

#define max_scan_count 1500 // 雷达数据个数的最大值

struct smoothness_t
{
    float value;
    size_t index;
};

// 排序的规则,从小到大进行排序
struct by_value
{
    bool operator()(smoothness_t const &left, smoothness_t const &right)
    {
        return left.value < right.value;
    }
};

4.3 LaserScan类

// 声明一个类
class LaserScan
{
private:
    ros::NodeHandle node_handle_;           // ros中的句柄
    ros::NodeHandle private_node_;          // ros中的私有句柄
    ros::Subscriber laser_scan_subscriber_; // 声明一个Subscriber
    ros::Publisher feature_scan_publisher_; // 声明一个Publisher

    float edge_threshold_; // 提取角点的阈值

public:
    LaserScan();
    ~LaserScan();
    void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
};

4.4 构造函数

构造函数中设置了订阅 laser+scan 的回调函数为： ScanCallback()

并初始化了一个publisher，发布提取到的特征点，数据类型为 sensor_msgs::LaserScan，发布到feature_scan 这个topic。

// 构造函数
LaserScan::LaserScan() : private_node_("~")
{
    // \033[1;32m，\033[0m 终端显示成绿色
    ROS_INFO_STREAM("\033[1;32m----> Feature Extraction Started.\033[0m");

    // 将雷达的回调函数与订阅的topic进行绑定
    laser_scan_subscriber_ = node_handle_.subscribe("laser_scan", 1, &LaserScan::ScanCallback, this);
    // 将提取后的点发布到 feature_scan 这个topic
    feature_scan_publisher_ = node_handle_.advertise<sensor_msgs::LaserScan>("feature_scan", 1, this);

    // 将提取角点的阈值设置为1.0
    edge_threshold_ = 1.0;
}

4.5 回调函数

接下来进行回调函数中代码的讲解：

void LaserScan::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)

4.5.1 临时变量声明

首先，在回调函数的代码的开始部分，声明了一些临时变量，并进行初始化。

（声明太多临时变量的习惯其实不好，每次声明新的变量都会有个 调用构造函数 的时间，浪费时间，只不过这里为了代码更清晰暂时放在这里了。）

    std::vector<smoothness_t> scan_smoothness_(max_scan_count); // 存储每个点的曲率与索引
    float *scan_curvature_ = new float[max_scan_count];         // 存储每个点的曲率

    std::map<int, int> map_index;   // 有效点的索引 对应的 scan实际的索引
    int count = 0;                  // 有效点的索引
    float new_scan[max_scan_count]; // 存储scan数据的距离值

    // 通过ranges中数据的个数进行雷达数据的遍历
    int scan_count = scan_msg->ranges.size();

4.5.2 去除无效点

这个for循环实现的功能就是将无效点去掉了。

因为雷达数据中会存在大量的 inf 或者 nan 的值，这种值在进行数学运算时会报错，所以要在使用range数据前将这种值去掉。

不知道这两个值的可以百度一下，这种数据产生的原因有很多，如墙面与雷达射线的夹角太大，雷达一条线打到桌子的边缘，导致返回强度很低等等等等。这些情况下，雷达的驱动包里会将这个值赋值成inf或者nan。

std::isfinite 在输入无效值时将返回false，所以进行取反。

将有效点的距离值，放入 new_scan 以备后来使用。

我们用count来当做 new_scan 的索引，count是连续的。

    // 去处inf或者nan点,保存有效点
    for (int i = 0; i < scan_count; i++)
    {
        // std::isfinite ：输入的值是有效值，返回true
        if (!std::isfinite(scan_msg->ranges[i]))
        {
            // std::cout << " " << i << " " << scan_msg->ranges[i];
            continue;
        }

        // 这点在原始数据中的索引为i，在new_scan中的索引为count
        map_index[count] = i;
        // new_scan中保存了有效点的距离值
        new_scan[count] = scan_msg->ranges[i];
        count++;
    }

4.5.3 计算曲率值

计算new_scan中每个点的曲率值，这里说的曲率并不是严格按照曲率的计算公式进行计算的，而是通过 当前点前后各5个点的距离值的和 与 当前点距离值的10倍 做差，用这个差值近似代码曲率值。

就是当前点与其前后各5个点的平均偏离程度，作为曲率值。

并将曲率值的平方 存入scan_curvature_ 与 scan_smoothness_ 中备用。

    // 计算曲率值, 通过当前点前后5个点距离值的偏差程度来代表曲率
    // 如果是球面, 则当前点周围的10个点的距离之和 减去 当前点距离的10倍 应该等于0
    for (int i = 5; i < count - 5; i++)
    {
        float diff_range = new_scan[i - 5] + new_scan[i - 4] +
                           new_scan[i - 3] + new_scan[i - 2] +
                           new_scan[i - 1] - new_scan[i] * 10 +
                           new_scan[i + 1] + new_scan[i + 2] +
                           new_scan[i + 3] + new_scan[i + 4] +
                           new_scan[i + 5];
        // diffX * diffX + diffY * diffY
        scan_curvature_[i] = diff_range * diff_range;
        scan_smoothness_[i].value = scan_curvature_[i];
        scan_smoothness_[i].index = i;
    }

注意，上面2个for循环其实可以合并成一个，以减少运行时间，大致思路为

声明一个队列
对前11个有效值，进行入队的操作；
对最后11个有效值，进行出队的操作；
对在这期间的值，先计算这11个值的曲率，再弹出队列头部的第一个值，再在队列尾插入新一个值，即可完成第二个for循环的操作，达到减少运算量的目的。

4.5.4 声明一个sensor_msgs::LaserScan

声明一个临时的sensor_msgs::LaserScan变量，用于存储进行特征提取后的scan特征点，并发布出去，在rviz中进行显示。

    // 声明一个临时的sensor_msgs::LaserScan变量,用于存储特征提取后的scan数据,并发布出去,在rviz中进行显示
    sensor_msgs::LaserScan corner_scan;
    corner_scan.header = scan_msg->header;
    corner_scan.angle_min = scan_msg->angle_min;
    corner_scan.angle_max = scan_msg->angle_max;
    corner_scan.angle_increment = scan_msg->angle_increment;
    corner_scan.range_min = scan_msg->range_min;
    corner_scan.range_max = scan_msg->range_max;

    // 对float[] 进行初始化
    corner_scan.ranges.resize(max_scan_count);

4.5.5 特征点提取

首先，为了保证特征点分布的更均匀，将scan数据分成6部分，每个部分取最多20个特征点。

先计算一下这 六分之一数据段 的起始点和终止点的索引start_index 与 end_index ，再将这段数据根据曲率值由小到大进行排序 sort（）。

这样曲率值大的点处在后面的部分，我们通过从后往前的遍历，选取最多20个点，并将这些点填充进新声明的corner_scan中。

    // 进行角点的提取,将完整的scan分成6部分,每部分提取20个角点
    for (int j = 0; j < 6; j++)
    {
        int start_index = (0 * (6 - j) + count * j) / 6;
        int end_index = (0 * (5 - j) + count * (j + 1)) / 6 - 1;
        // std::cout << "start_index: " << start_index << " end_index: " << end_index << std::endl;

        if (start_index >= end_index)
            continue;

        // 将这段点云按照曲率从小到大进行排序
        std::sort(scan_smoothness_.begin() + start_index,
                  scan_smoothness_.begin() + end_index, by_value());

        int largestPickedNum = 0;
        // 最后的点 的曲率最大，如果满足条件，就是角点
        for (int k = end_index; k >= start_index; k--)
        {
            int index = scan_smoothness_[k].index;
            if (scan_smoothness_[k].value > edge_threshold_)
            {
                // 每一段最多只取20个角点
                largestPickedNum++;
                if (largestPickedNum <= 20)
                {
                    corner_scan.ranges[map_index[index]] = scan_msg->ranges[map_index[index]];
                }
                else
                {
                    break;
                }
            }
        }
    }

4.5.6 数据的发布

    // 将提取后的scan数据发布出去
    feature_scan_publisher_.publish(corner_scan);

5 总结

我们借鉴LIO-SAM中特征提取的部分，实现了一个简单的使用单线雷达的特征点提取功能，体验了如何对激光雷达数据进行处理的过程。

在LIO-SAM中还有一些额外的操作，如 标记遮挡点、平行点、每提取一个特征点后，就将这个特征点前后5个数据进行标记，不再参与特征点提取等操作。

我们展示了角点特征的提取，LIO-SAM中还有平面点特征的提取，由于这部分使用多线的数据点，所以在单线雷达中没有对这块进行实现。

单线雷达还可以进行直线、线段等特征的提取，这里就不再进行实现了。

更多的LIO-SAM的特征点提取的代码阅读，请参看这篇文章
https://blog.youkuaiyun.com/tiancailx/article/details/109513692

下篇文章暂定为：使用PCL中的ICP，进行前端里程计的实现。

---

文章将在 公众号: 从零开始搭SLAM 进行同步更新，欢迎大家关注，以在文章更新的第一时间通知您。

同时，也希望您将这个公众号推荐给您身边做激光SLAM的人们，大家共同进步。

如果您对我写的文章有什么建议，或者想要看哪方面功能如何实现的，请直接在公众号中回复，我可以收到，并将认真考虑您的建议。