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

前言

地面点提取方法

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

效果还是挺好得，没有出现异常点

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