Foreword:

Learn the source code of the ground point extraction part in lego-loam.

Ground point extraction function groundRemoval() in src/imageProjection.cpp. The content is relatively small and easy to understand.

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

lowerInd indicates a point cloud with a low number of lines;

upperInd represents the point cloud of the adjacent high line number;

diffX, diffY, and diffZ respectively represent the absolute value difference between x, y, and z between two points;

angle represents the angle between two points derived from the paper.

As shown in FIG.

1. Ground point extraction part

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

The source code part classifies point clouds belonging to different types:

-1 is an invalid point;

0 is a non-ground point;

1 indicates the ground point.

        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;
                }
            }
        }

The two for loops here are all traversals of the ground points, and are also mentioned in the paper to apply the general image processing matrix to the point cloud.

Horizon_SCAN means 1800, because the horizontal resolution is 0.5, so it is 1800 points;

groundScanInd should be 8 (if it is a 16-line laser lidar), indicating the emitter that shoots to the ground.

The specific representation is shown in the following figure:

According to the above figure, the assignment of lowerInd and upperInd is also clear at a glance.

                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;
                }

Here, if it is judged to be an invalid point, the value of the groundMat matrix is ​​assigned -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;
                }

This part is to calculate the angle, if it is less than 10 degrees. Indicates that it is a ground point, and both of these two points are assigned a value of 1.

2. Remove ground points

        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;
                }
            }
        }

The function of this part is to traverse all the points, and mark them as -1 if they are found to be ground points and points that have not returned after launch.

        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]);
                }
            }
        }

Here is to judge whether it is a ground point through the previous groundMat matrix, and if it is a ground point. That is, if the flag bit is 1, then push_back the point into the groundCloud container.

3. Summary

The ground point extraction part in lego-loam is relatively easy to understand, and then use the ground point extraction, you can refer to this source code.