我们这篇文章将对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.csdn.net/tiancailx/article/details/109513692
下篇文章暂定为：使用PCL中的ICP，进行前端里程计的实现。
文章将在 公众号: 从零开始搭SLAM 进行同步更新，欢迎大家关注，以在文章更新的第一时间通知您。
同时，也希望您将这个公众号推荐给您身边做激光SLAM的人们，大家共同进步。
如果您对我写的文章有什么建议，或者想要看哪方面功能如何实现的，请直接在公众号中回复，我可以收到，并将认真考虑您的建议。
评论（1）
您还未登录，请登录后发表或查看评论