前言
LIO-SAM的全称是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。
LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。
实现了高精度、实时的移动机器人的轨迹估计和建图。
其中特征提取节点在整体框架中如下：
功能就是订阅前节点发布的畸变校正后的点云，进行角点和面点的提取，然后再发布处理后的点云。
特征点提取
lio-sam框架 的 特征点提取部分的代码在 featureExtraction.cpp 中。
int main(int argc, char** argv)
{
ros::init(argc, argv, "lio_sam");
FeatureExtraction FE;
ROS_INFO("\033[1;32m----> Feature Extraction Started.\033[0m");
ros::spin();
return 0;
}
main函数中做的就是
- 节点初始化
- 实例化
FeatureExtraction类
其中特征点提取的功能实现也是在 这个FeatureExtraction类中进行
来看该类的构造函数
FeatureExtraction()
{
在里面主要是进行
- 消息的订阅
- 消息的发布 声明
- 初始化变量
subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
订阅 imageProjection 发布的 点云畸变校正后的 点云信息
pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/feature/cloud_info", 1);
pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_corner", 1);
pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/feature/cloud_surface", 1);
声明要发布的消息
initializationValue();
初始化各变量
下面则可以看 点云的回调函数laserCloudInfoHandler
// 订阅上一个节点的信息
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
cloudInfo = *msgIn;
cloudHeader = msgIn->header;
pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud);
把提取出来的有效点 转成pcl格式
calculateSmoothness(); // 计算每个点的曲率
markOccludedPoints();// 标记遮挡和平行点
extractFeatures();// 提取surface和corner特征
publishFeatureCloud();// 发布特征点云
之后则进行了4个函数的调用，功能分别是
- 计算曲率
- 标记遮挡和平行点
- 提取surface和corner特征
- 发布特征点云
首先来看第一个函数 calculateSmoothness()计算曲率
其中计算的方法和ALOAM是一致的
void calculateSmoothness()
{
int cloudSize = extractedCloud->points.size();
统计点的总个数
for (int i = 5; i < cloudSize - 5; i++)
{
前5个点不要，遍历每一个点，因为要周围10点的距离
float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]
+ cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]
+ cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10
+ cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]
+ cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]
+ cloudInfo.pointRange[i+5];
计算当前点和周围10个点的距离差
cloudCurvature[i] = diffRange*diffRange;
曲率值就是距离差的平方
这里的计算曲率就是这个公式
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
这两个值附成初始值
一个是这个点被选择了，另一个是点的标签
cloudSmoothness[i].value = cloudCurvature[i];
cloudSmoothness[i].ind = i;
将曲率与点的索引保存到点云曲率的队列中，用来进行曲率排序
下面来看第二个函数 markOccludedPoints() 标记一下遮挡的、平行的点
void markOccludedPoints()
{
//获取点云中点的个数
int cloudSize = extractedCloud->points.size();
统计点的总个数
for (int i = 5; i < cloudSize - 6; ++i)
{
循环处理每个点,判断是否是遮挡点还是平行点
float depth1 = cloudInfo.pointRange[i];
float depth2 = cloudInfo.pointRange[i+1];
int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));
取出相邻两个点距离信息
计算两个有效点之间的列id差
if (columnDiff < 10){
只有靠近才比较有意义
如图上的情况，因为去了NAN点，那么i和i+1点距离偏差过大，也不是遮挡点，要排除此种情况
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;
这样的depth1 容易被遮挡，因此其之前的5个点都设置为无效点
}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;
}
这样的depth2 容易被遮挡，因此其之后的5个点都设置为无效点
如果相邻两点距离大于0.3，选出6个点
这时候要分两种情况
一个是i点的距离大,即i为被遮挡点,那么i点也不能要,并且i-1至i-5不要,因为i-1至i-5也不稳定
另外一种情况是i在前面,那么i+1是被遮挡的点,那么i+1至i+6不要
float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));
if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
cloudNeighborPicked[i] = 1;
如果两点距离比较大，就很可能是平行的点，也很可能失去观测
平行点指的就是 图中的B点
就是激光的射线几乎和物体的平面平行了
剔除这种点的原因有两个:
- 激光的数据会不准,射线被拉长
- 这种点被视为特征点后会非常不稳定,下一帧可能就没了,无法做配对了
例如图片中的lidar向左移一点,那么B点就消失了,形成对比的就是A点,极短时间内不会消失
遮挡点就是 图中的A点
lidar一条sacn上,相邻的两个或几个点的距离偏差很大,被视为遮挡点
剔除这种点的原因是:
这种点被视为特征点后会非常不稳定,下一帧可能就没了,无法做配对了
例如图片中的lidar向右移一点,那么A点就消失了,形成对比的就是B点,极短时间内不会消失
此时最后把A点的左边几个点,都剔除掉
下面来看第三个函数 extractFeatures() 提取角点和面点
void extractFeatures()
{
cornerCloud->clear();
surfaceCloud->clear();
清空角点和面点的点云
for (int i = 0; i < N_SCAN; i++)
{
遍历每个scan
for (int j = 0; j < 6; j++)
{
int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;
int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;
if (sp >= ep)
continue;
std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());
把每一根 scan 等分成6份，每份分别提取特征点
安曲率排序
for (int k = ep; k >= sp; k--)
{
开始提取角点
int ind = cloudSmoothness[k].ind;
找到这个点对应的原先的 idx
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
{
如果没有被认为是遮挡点和平行点 并且曲率大于阈值
largestPickedNum++;
if (largestPickedNum <= 20){
cloudLabel[ind] = 1;
cornerCloud->push_back(extractedCloud->points[ind]);
} else {
break;
}
每一段最多只取20个角点
标签置1 表示是角点
这个点收集进存储角点的点云中
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++)
{
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
将这个点周围的几个点设置为遮挡点，避免选取太集中
列idx距离太远就算了，空间上也不会太集中
for (int k = sp; k <= ep; k++)
{
提取面点
int ind = cloudSmoothness[k].ind;
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold)
{
不是遮挡点和平行点 且曲率小于阈值
cloudLabel[ind] = -1;
-1 表示是面点
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
if (columnDiff > 10)
break;
cloudNeighborPicked[ind + l] = 1;
}
把周围点设置为遮挡点
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0){
surfaceCloudScan->push_back(extractedCloud->points[k]);
}
}
注意这里是小于等于0 也就是说 不是角点的都认为是面点了
surfaceCloudScanDS->clear();
downSizeFilter.setInputCloud(surfaceCloudScan);
downSizeFilter.filter(*surfaceCloudScanDS);
*surfaceCloud += *surfaceCloudScanDS;
因为面点太多了，所以做个下采样
最后来看最后一个函数 publishFeatureCloud() 发布特征点云
void publishFeatureCloud()
{
freeCloudInfoMemory();
cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame);
cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);
pubLaserCloudInfo.publish(cloudInfo);
}
发布角点和面点的点云
并且发布 cloudInfo ，此时 cloudInfo 内存了角点和面点
Result
其中粉色的是面点，绿色的是角点。
评论（0）
您还未登录，请登录后发表或查看评论