前言：

对于lego-loam中地面点提取部分的源码进行学习。

地面点提取在src/imageProjection.cpp中的函数groundRemoval()。内容比较少，容易理解。

        size_t lowerInd, upperInd;
        float diffX, diffY, diffZ, angle;

lowerInd表示低线数的点云；

upperInd表示相邻的高线数的点云；

diffX,diffY,diffZ分别表示两个点之间x,y,z之间的绝对值差值；

angle表示根据论文推导出来的两点之间的角度。

如上图所示。

一、地面点提取部分

        // groundMat
        // -1, no valid info to check if ground of not
        //  0, initial value, after validation, means not ground
        //  1, ground

源码部分对于属于不同种类的点云进行了分类：

-1是无效点；

0是非地面点；

1表示地面点。

        for (size_t j = 0; j < Horizon_SCAN; ++j){
            for (size_t i = 0; i < groundScanInd; ++i){

                lowerInd = j + ( i )*Horizon_SCAN;
                upperInd = j + (i+1)*Horizon_SCAN;

                if (fullCloud->points[lowerInd].intensity == -1 ||
                    fullCloud->points[upperInd].intensity == -1){
                    // no info to check, invalid points
                    groundMat.at<int8_t>(i,j) = -1;
                    continue;
                }
                    
                diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
                diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
                diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;

                angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;

                if (abs(angle - sensorMountAngle) <= 10){
                    groundMat.at<int8_t>(i,j) = 1;
                    groundMat.at<int8_t>(i+1,j) = 1;
                }
            }
        }

这里的两个for循环是对于地面点的全部遍历，也是论文中提到的将一般图像处理的矩阵应用于点云。

Horizon_SCAN表示1800，因为水平分辨率是0.5，所以是1800个点；

groundScanInd应该是8(如果是16线激光lidar)，表示射向地面部分的发射器。

具体表示如下图所示：

根据上图，对于lowerInd和upperInd的赋值也就一目了然。

                if (fullCloud->points[lowerInd].intensity == -1 ||
                    fullCloud->points[upperInd].intensity == -1){
                    // no info to check, invalid points
                    groundMat.at<int8_t>(i,j) = -1;
                    continue;
                }

这里如果判断是无效点，就将groundMat矩阵的值赋-1。

                diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
                diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
                diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;

                angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;

                if (abs(angle - sensorMountAngle) <= 10){
                    groundMat.at<int8_t>(i,j) = 1;
                    groundMat.at<int8_t>(i+1,j) = 1;
                }

这部分就是计算angle，如果小于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;
                }
            }
        }

这部分的作用是遍历所有的点，发现是地面点和发射后没有返回的点就将它们标志为-1。

        if (pubGroundCloud.getNumSubscribers() != 0){
            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]);
                }
            }
        }

这里是通过前面的groundMat矩阵来判断是否为地面点，如果是地面点。即标志位为1，那么就将该点push_back进groundCloud容器中。