3D激光slam:LeGO-LOAM---地面点提取方法及代码分析

举报
月照银海似蛟龙 发表于 2022/08/02 08:56:30 2022/08/02
【摘要】 LeGO-LOAM中前端改进中很重要的一点就是充分利用地面点,本片博客主要讲解 如何进行地面点提取

前言

地面点提取方法

LeGO-LOAM中前端改进中很重要的一点就是充分利用地面点,本片博客主要讲解 如何进行地面点提取

如下图所示,相邻的两个scan的同一列,打在地面上,形成两个点A和B。
在这里插入图片描述

它们的垂直高度差为h,这个值在理想情况(雷达水平安装,地面是水平的)接近于0
在这里插入图片描述
水平距离差d
在这里插入图片描述
和水平面的夹角为
在这里插入图片描述
如果为地面点,在理想情况下,这个角点接近0.

但是雷达的安装不会完全水平,并且地面也不是平的,因此这个角度会大于0,LeGO-LOAM设置的是10°。
即小于10°被判断为地面点

这种地面点的提取算法有些过于简单,还可以结合激光雷达安装高度,等其它信息进行判断。例如下面这种情况,也会被判断为地面点:
在这里插入图片描述

代码分析

LeGO-LOAM的地面提取的代码在 imageProjection.cppgroundRemoval 函数

    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测试

在这里插入图片描述
在这里插入图片描述
效果还是挺好得,没有出现异常点

但是由于没有加入高度得判断,在前面里说得这种情况则会出现问题
在这里插入图片描述
在这里插入图片描述

【版权声明】本文为华为云社区用户原创内容,转载时必须标注文章的来源(华为云社区)、文章链接、文章作者等基本信息, 否则作者和本社区有权追究责任。如果您发现本社区中有涉嫌抄袭的内容,欢迎发送邮件进行举报,并提供相关证据,一经查实,本社区将立刻删除涉嫌侵权内容,举报邮箱: cloudbbs@huaweicloud.com
  • 点赞
  • 收藏
  • 关注作者

评论(0

0/1000
抱歉,系统识别当前为高风险访问,暂不支持该操作

全部回复

上滑加载中

设置昵称

在此一键设置昵称,即可参与社区互动!

*长度不超过10个汉字或20个英文字符,设置后3个月内不可修改。

*长度不超过10个汉字或20个英文字符,设置后3个月内不可修改。