A-LOAM(前端-1)的特征提取及均匀化-算法流程+代码注释
文章目录
- laserCloudHandler流程
-
- 曲率计算代码
- 每个scan提取特征
laserCloudHandler流程
该函数是提取特征点的处理函数，接收到雷达硬件发送过来的消息的时候就会运行这个函数，来对点云进行预处理
这个函数是个回调函数，在订阅到点云数据的时候才会运行，ALOAM中的点云接收频率为10HZ
-
先判断初始化，等待几帧来到后才认定为初始化成功
-
进行线束id计算与每个点的时间戳，并将点云存到其对应的第scanID个数组中
-
标记每个线束的最左5与最右5不参与曲率计算
-
开始曲率计算，notic：这里实际处理是将全部点云都参与曲率计算了，计算曲率的公式可以这样认为 ，这里是仅对x处理时，对y，z操作同理 d i f f X = x 1 + x 2 + x 3 + x 4 + x 5 − 10 ∗ x 6 + x 7 + x 8 + x 9 + x 10 + x 11 diffX=x1+x2+x3+x4+x5-10*x6+x7+x8+x9+x10+x11 diffX=x1+x2+x3+x4+x5−10∗x6+x7+x8+x9+x10+x11
曲率 σ = d i f f X 2 + d i f f Y 2 + d i f f Z 2 σ = diffX^{2}+ diffY^{2}+ diffZ^{2} σ=diffX2+diffY2+diffZ2 -
开始特征提取，用到第4点的曲率信息
- 提取曲率最大的2个点
- 提取曲率一般大的20-2个点
- 边缘点均匀化操作
- 将选中点的周围5个点都置1，判断条件为，查看相邻点的距离是否差异过大，过大则说明不连续是边缘点会break跳出不置为，差异小则说明是临近点有冗余，则置为1（往左操作5个点云，往右操作5个点云，共两次for循环）
- 提取平坦点，只要曲率小于0.1都认为是平坦点，没有最平坦和一般平坦
- 和上面一样均匀化操作
- 对平坦点进行体素滤波获得体素滤波后的平坦点
-
将原始点云、曲率最大的点、曲率一般大的点、全部平坦点、体素滤波后的平坦点发布出去给odom和mapping进行计算
-
判断点云预处理时间是否超过100ms，因为KITTY数据集是10HZ，数据处理超过100ms可能会出现丢帧的情况进而影响odom和mapping的效果
个人学习总结，有错误的地方欢迎指出一起讨论
曲率计算代码
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++)
{ //每个scan的前后5个点都不参与曲率计算
scanStartInd[i] = laserCloud->size() + 5; //最左5不加入曲率计算，scanStartInd来区分第几个线束的点，判断是否加入曲率计算
*laserCloud += laserCloudScans[i];
scanEndInd[i] = laserCloud->size() - 6; //最右5不加入曲率计算
}
printf("prepare time %f \n", t_prepare.toc());
//开始计算曲率，这里将全部点就计算曲率了
for (int i = 5; i < cloudSize - 5; i++) //只有开始和最后的5个点没计算曲率
{
// 对x维度的曲率进行计算 x1+x2+x3+x4+x5-10*x6+x7+x8+x9+x10+x11
//可以上面这个公式这么理解
float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;
//存储曲率，索引
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
cloudSortInd[i] = i;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
}
每个scan提取特征
TicToc t_pts;
pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> cornerPointsLessSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;
float t_q_sort = 0;
//遍历每个scan
for (int i = 0; i < N_SCANS; i++)
{
if( scanEndInd[i] - scanStartInd[i] < 6) //有效点太少就continue
continue;
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
//将每个scan等分成6等份
for (int j = 0; j < 6; j++)
{
//每个等分的起始和结束点 scanStartInd，scanEndInd 限制了排序的区域，交界点的曲率不会用到
int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
TicToc t_tmp;
//对点云曲率进行排序，小前大后
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
t_q_sort += t_tmp.toc();
int largestPickedNum = 0;
// 挑选曲率比较大的地方，排序后点都乱了
for (int k = ep; k >= sp; k--)
{
//ind 代表挑选出来的点在原始中的id
int ind = cloudSortInd[k];
//看看这个点是否有效，并且曲率大于阈值0.1，大于0.1认为边缘点，小于0.1认为可能的面点
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1)
{
largestPickedNum++; //挑选了多少个曲率大的点
//这里存点是为了帧间里程计中，当前帧的大曲率点和上一帧的小曲率点的约束关系的建立
if (largestPickedNum <= 2) //选2个
{
cloudLabel[ind] = 2; //标记为2是曲率大的点的标记
cornerPointsSharp.push_back(laserCloud->points[ind]); //存储曲率大的点
cornerPointsLessSharp.push_back(laserCloud->points[ind]); //第二大的点放这里
}
else if (largestPickedNum <= 20) //选20个曲率稍微大的点
{
cloudLabel[ind] = 1;
cornerPointsLessSharp.push_back(laserCloud->points[ind]);//第二大的点放这里
}
else //超过20个就算了
{
break;
}
cloudNeighborPicked[ind] = 1;
//避免每个特征点过于集中，将选中的点的周围5个点都置1，避免后续会选到
for (int l = 1; l <= 5; l++)
{
//查看相邻点距离是否差异过大，差异过大说明点云不连续，如果是特征边缘就是新的特征，因此不置位
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
//往右找5个，与上同理
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
//下面开始提取面点，认为不是角点的就是面点
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++)
{
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 && //没被标记+曲率小于0.1
cloudCurvature[ind] < 0.1)
{
//-1认为是平坦的点
cloudLabel[ind] = -1;
surfPointsFlat.push_back(laserCloud->points[ind]);
smallestPickedNum++;
//这里不区分平坦和比较平坦
if (smallestPickedNum >= 4)
{
break;
}
//也是均匀化，与上同理
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0) //将全部平坦点存着准备用于体素滤波
{
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
} //到这里结束了scan的遍历
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
//一般平坦的点比较多，这里做一个体素滤波，下采样，为了节约计算方式
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
surfPointsLessFlat += surfPointsLessFlatScanDS;
}
printf("sort q time %f \n", t_q_sort);
printf("seperate points time %f \n", t_pts.toc());
//分别将当前点云和四种特征的点云发布出去，发出去给odm和mapping进行计算
sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/camera_init";
pubLaserCloud.publish(laserCloudOutMsg);
//曲率很大的点
sensor_msgs::PointCloud2 cornerPointsSharpMsg;
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsSharpMsg.header.frame_id = "/camera_init";
pubCornerPointsSharp.publish(cornerPointsSharpMsg);
//曲率第2的点
sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsLessSharpMsg.header.frame_id = "/camera_init";
pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);
//比较平坦的点
sensor_msgs::PointCloud2 surfPointsFlat2;
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsFlat2.header.frame_id = "/camera_init";
pubSurfPointsFlat.publish(surfPointsFlat2);
//体素滤波后的平坦点
sensor_msgs::PointCloud2 surfPointsLessFlat2;
pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsLessFlat2.header.frame_id = "/camera_init";
pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
// pub each scam 可以按照每个scan发出去，但是这里设置了false
if(PUB_EACH_LINE)
{
for(int i = 0; i< N_SCANS; i++)
{
sensor_msgs::PointCloud2 scanMsg;
pcl::toROSMsg(laserCloudScans[i], scanMsg);
scanMsg.header.stamp = laserCloudMsg->header.stamp;
scanMsg.header.frame_id = "/camera_init";
pubEachScan[i].publish(scanMsg);
}
}
printf("scan registration time %f ms *************\n", t_whole.toc());
//算一下每个配准是否超过100ms，因为是10HZ来一次数据，经历控制上面的预处理在100ms以内，判断是否丢帧
if(t_whole.toc() > 100)
ROS_WARN("scan registration process over 100ms");
A-LOAM系列讲解
A-LOAM(前端-1)的特征提取及均匀化-算法流程+代码注释
A-LOAM(前端-2)异常点的剔除-算法流程+代码注释
A-LOAM(前端-3)的雷达畸变及运动补偿-算法流程+代码注释
A-LOAM(前端-4)的帧间lidar里程计-算法流程+代码注释
A-LOAM(后端1)基于栅格点云地图构建-算法流程+代码注释
A-LOAM(后端2)地图中的线面特征提取及优化问题构建-算法流程+代码注释
A-LOAM总结-(前端+后端)算法流程分析
关于ROS中map、odom、base_link三个坐标系的理解