LIO-SAM代码解析——featureExtraction.cpp
目录
- featureExtraction.cpp
-
- 1. FeatureExtraction类
-
- 1.1. laserCloudInfoHandler
-
- 1.1.1. calculateSmoothness()
- 1.1.2. markOccludedPoints()
- 1.1.3. extractFeatures()
- 1.1.4. publishFeatureCloud()
传感器输入： IMU，Point Cloud， GPS（可选）
输出 ： IMU 频率的odometry
imageProjection.cpp: 接受IMU，PointCloud以及IMU预积分输出的IMU odometry（系统刚初始化时没有IMU odometry）。
- 主要功能：
- 基于IMU odometry得到系统的初始位姿
- 将点云投影到cv::mat中，做相应的预处理
- 对原始点云数据做运动补偿（点云的去畸变补偿在代码中只应用于旋转部分，注释掉了平移部分）
featureExtraction.cpp :接收来自imageProjection处理完的lidar帧数据，对应的点云是去畸变的，也就是在同一个坐标系下。对点云进行面点，角点特征的分类。
- 主要功能：
- 提取点云边缘特征和面特征
mapOptimization.cpp:
- 主要功能：
- 将提取到点云特征与地图中的边缘特征和面特征进行配准
- 配准后得到当前帧在地图中的位姿
- 图优化： 将lidar的帧间约束，回环的约束，（GPS因子）添加到因子图中
imuPreintegration.cpp:一开始并没有工作，只有收到lidar odometry后才会工作
- 主要功能：
- 图优化：lidar odometry和IMU的帧间约束添加到因子图中
- 估计IMU零偏
featureExtraction.cpp
主函数只存在1个类FeatureExtraction FE
1. FeatureExtraction类
FeatureExtraction()
{
// 订阅当前激光帧运动畸变校正后的点云信息
subLaserCloudInfo = nh.subscribe<lio_sam::cloud_info>("lio_sam/deskew/cloud_info", 1, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
// 发布当前激光帧提取特征之后的点云信息
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();
}
1.1. laserCloudInfoHandler
// 点云特征提取
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
cloudInfo = *msgIn; // new cloud info
cloudHeader = msgIn->header; // new cloud header
// 把提取出的有效的点转成pcl格式
pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // new cloud for extraction
// 计算当前激光帧点云中每个点的曲率
calculateSmoothness();
// 标记属于遮挡、平行两种情况的点，不做特征提取，同LOAM论文图4的情况
markOccludedPoints();
// 提取特征，同aloam，没有区分明显和不明显的角点/面点
extractFeatures();
// 发布角点、面点点云，发布带特征点云数据的当前激光帧点云信息
publishFeatureCloud();
}
1.1.1. calculateSmoothness()
// 计算曲率
void calculateSmoothness()
{
// 遍历当前激光帧运动畸变校正后的有效点云
int cloudSize = extractedCloud->points.size();
for (int i = 5; i < cloudSize - 5; i++)
{
// 计算当前点和周围十个点的距离差，平坦位置处曲率较小，角点处曲率较大
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];
// 距离差的平方做曲率
cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ;
// 下面两个值赋成初始值
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
// cloudSmoothness for sorting
// 存储该点曲率值、激光点一维索引
cloudSmoothness[i].value = cloudCurvature[i];
cloudSmoothness[i].ind = i;
}
}
1.1.2. markOccludedPoints()
// 选出(剔除)与激光方向平行或被遮挡的激光点，同LOAM论文图4的情况
void markOccludedPoints()
{
int cloudSize = extractedCloud->points.size();
// mark occluded points and parallel beam points
for (int i = 5; i < cloudSize - 6; ++i)
{
// occluded points
// 取出相邻两个点距离信息
float depth1 = cloudInfo.pointRange[i]; // 存储了点到lidar中心的距离
float depth2 = cloudInfo.pointRange[i+1];
// 计算两个有效点之间的列id差
int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));
// 1. 判断是否存在遮挡
// 只有比较靠近才有意义
if (columnDiff < 10){
// 10 pixel diff in range image
// 前者深度更大，说明后者把前者遮挡了，所以前者标记为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;
// 后者深度更大，说明前者把后者遮挡了，所以后者标记为1后续就不用了
}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;
}
}
// parallel beam
// 2.判断当前点是否在与激光帧平行的平面上
// 用前后相邻点判断当前点所在平面是否与激光束方向平行
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;
}
}
1.1.3. extractFeatures()
// 提取特征
void extractFeatures()
{
cornerCloud->clear();
surfaceCloud->clear();
pcl::PointCloud<PointType>::Ptr surfaceCloudScan(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfaceCloudScanDS(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCAN; i++)
{
surfaceCloudScan->clear();
// 把每一份scan等分成6份，每份分别提取特征点
for (int j = 0; j < 6; j++)
{
// 根据之前得到的每个scan的起始点和结束id来均分
// 每段点云的起始、结束索引；startRingIndex为扫描线起始第5个激光点在一维数组中的索引
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());
int largestPickedNum = 0;
// 按照曲率从大到小遍历
for (int k = ep; k >= sp; k--)
{
// 找到这个点对应的原先的idx
int ind = cloudSmoothness[k].ind;
// 如果没有被认为是遮挡点，且曲率大于边缘点阈值，则认为是角点
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
{
largestPickedNum++;
// 每段最多找20个角点
// 如果单条扫描线扫描一周是1800个点，则划分6段，每段300个点，从中提取20个角点
if (largestPickedNum <= 20){
// 标记为角点
cloudLabel[ind] = 1;
// 加入角点点云
cornerCloud->push_back(extractedCloud->points[ind]);
} else {
break;
}
// 标记已被处理
cloudNeighborPicked[ind] = 1;
// 同一条扫描线上后5个点标记一下，不再处理，避免特征聚集
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;
}
// 同一条扫描线上前5个点标记一下，不再处理，避免特征聚集
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++)
{
// 激光点的索引
int ind = cloudSmoothness[k].ind;
// 当前激光点还未被处理，且曲率小于阈值，则认为是平面点
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold)
{
// 标记为平面点
cloudLabel[ind] = -1;
// 标记已被处理
cloudNeighborPicked[ind] = 1;
// 同一条扫描线上后5个点标记一下，不再处理，避免特征聚集
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;
}
// 同一条扫描线上前5个点标记一下，不再处理，避免特征聚集
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++)
{
// 注意这里小于等于0,也就是说不是角点的都认为是面点了
if (cloudLabel[k] <= 0){
surfaceCloudScan->push_back(extractedCloud->points[k]);
}
}
}
surfaceCloudScanDS->clear();
// 因为面点太多，所以做一个下采样
downSizeFilter.setInputCloud(surfaceCloudScan);
downSizeFilter.filter(*surfaceCloudScanDS);
// 加入平面点云集合
*surfaceCloud += *surfaceCloudScanDS;
}
}
1.1.4. publishFeatureCloud()
void publishFeatureCloud()
{
// free cloud info memory
freeCloudInfoMemory();
// save newly extracted features
// 发布角点、面点点云，用于rviz展示
cloudInfo.cloud_corner = publishCloud(&pubCornerPoints, cornerCloud, cloudHeader.stamp, lidarFrame);
cloudInfo.cloud_surface = publishCloud(&pubSurfacePoints, surfaceCloud, cloudHeader.stamp, lidarFrame);
// publish to mapOptimization
// 发布当前激光帧点云信息，加入了角点、面点点云数据，发布给mapOptimization
pubLaserCloudInfo.publish(cloudInfo);
}