前言
地面点提取方法
LeGO-LOAM中前端改进中很重要的一点就是充分利用地面点,本片博客主要讲解 如何进行地面点提取
如下图所示,相邻的两个scan的同一列,打在地面上,形成两个点A和B。
它们的垂直高度差为h,这个值在理想情况(雷达水平安装,地面是水平的)接近于0
水平距离差d
和水平面的夹角为
如果为地面点,在理想情况下,这个角点接近0.
但是雷达的安装不会完全水平,并且地面也不是平的,因此这个角度会大于0,LeGO-LOAM设置的是10°。
即小于10°被判断为地面点
这种地面点的提取算法有些过于简单,还可以结合激光雷达安装高度,等其它信息进行判断。例如下面这种情况,也会被判断为地面点:
代码分析
LeGO-LOAM的地面提取的代码在 imageProjection.cpp 中 groundRemoval 函数
void groundRemoval(){
size_t lowerInd, upperInd;
float diffX, diffY, diffZ, angle;
lowerInd, upperInd 是相邻scan上点的索引值
diffX, diffY, diffZ, angle 是 dx dy dz 水平角
for (size_t j = 0; j < Horizon_SCAN; ++j){//遍历水平方向的点 360/0.2 1800个点
for (size_t i = 0; i < groundScanInd; ++i){//groundScanInd 为8 地面点不能在上面
嵌套两个for循环, 列要在前面,因为要计算同一列的值
第一行,遍历水平方向的点 360/0.2 1800个点
第二行,groundScanInd 为8 地面点不能在上面
lowerInd = j + ( i )*Horizon_SCAN;//下面的点
upperInd = j + (i+1)*Horizon_SCAN;//上面的点
计算的两个点的索引
Horizon_SCAN为1800,
if (fullCloud->points[lowerInd].intensity == -1 ||
fullCloud->points[upperInd].intensity == -1){
// no info to check, invalid points
groundMat.at<int8_t>(i,j) = -1;//标志位 至-1
continue;
}
判断两个点是否有效,点无效的话intensity为-1
有一个点无效的话 标志位 至-1
diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;//dx
diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;//dy
diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;//dz
angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;//计算水平角度
计算 dx dy dz 和水平角,就是这个公式
if (abs(angle - sensorMountAngle) <= 10){
groundMat.at<int8_t>(i,j) = 1;
groundMat.at<int8_t>(i+1,j) = 1;
}
}
}
sensorMountAngle 是 liadr 是和水平面的倾斜角
这里就是把那两个点的水平角,和10°做比较,判断是不是地面点
如何使把标志位 至 1
for (size_t i = 0; i < N_SCAN; ++i){
for (size_t j = 0; j < Horizon_SCAN; ++j){
if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){
labelMat.at<int>(i,j) = -1;//labelMat 至为 -1 ,不参与后续线特征和面特征的提取
}
}
}
判断完地面点后,再遍历每个点,
如过该点是 地面点或者无效点,则把 labelMat 上的该点标志位至-1 .
labelMat 至为 -1 ,不参与后续线特征和面特征的提取
//地面点可视化
if (pubGroundCloud.getNumSubscribers() != 0){//如果有节点要订阅这个地面点的topic 再进行发布
for (size_t i = 0; i <= groundScanInd; ++i){
for (size_t j = 0; j < Horizon_SCAN; ++j){
if (groundMat.at<int8_t>(i,j) == 1)
groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);//把点加在 地面点点云中 之后会发布出去
}
}
}
}
最后进行地面点得可视化
如果有节点要订阅这个地面点的topic 再进行发布
遍历0-groundScanInd 上得每个点,判断如果是地面点,则添加该点到 groundCloud 中
之后会被发布出去
发布得在这个地方
// original dense ground cloud
if (pubGroundCloud.getNumSubscribers() != 0){
pcl::toROSMsg(*groundCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubGroundCloud.publish(laserCloudTemp);
}
topic得名称是/ground_cloud
pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2> ("/ground_cloud", 1);
gazebo测试
效果还是挺好得,没有出现异常点文章来源:https://www.toymoban.com/news/detail-408470.html
但是由于没有加入高度得判断,在前面里说得这种情况则会出现问题文章来源地址https://www.toymoban.com/news/detail-408470.html
到了这里,关于3D激光slam:LeGO-LOAM---地面点提取方法及代码分析的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!