3 d laser slam: LeGO - LOAM - ground point extracting method and the analysis of the code

前言

Ground point extraction method

LeGO-LOAMA very important point in the improvement of the mid-front end is to make full use of the ground points,This blog post mainly explains How to do ground point extraction

如下图所示,相邻的两个scan的同一列,hit the ground,form two pointsA和B.

Their vertical height difference is h,This value is ideal（The radar is installed horizontally,The ground is level）接近于0

Poor horizontal distanced

The angle with the horizontal plane is

If it is a ground point,在理想情况下,This corner is close0.

But the radar installation will not be completely horizontal,And the ground is not flat,So this angle will be greater than 0,LeGO-LOAM设置的是10°.
即小于10°is judged to be a ground point

The extraction algorithm of this ground point is a bit too simplistic,It can also be combined with the lidar installation height,Wait for other information to judge.例如下面这种情况,are also judged as ground points：

代码分析

LeGO-LOAMThe code for the ground extraction is in imageProjection.cpp 中 groundRemoval 函数

    void groundRemoval(){        size_t lowerInd, upperInd;        float diffX, diffY, diffZ, angle;

lowerInd, upperInd 是相邻scanThe index value of the upper point
diffX, diffY, diffZ, angle 是 dx dy dz 水平角

        for (size_t j = 0; j < Horizon_SCAN; ++j){//Traverse the points in the horizontal direction 360/0.2 1800个点            for (size_t i = 0; i < groundScanInd; ++i){//groundScanInd 为8 The ground point cannot be on it

嵌套两个for循环, Column to be first,Because you want to calculate the value of the same column

第一行,Traverse the points in the horizontal direction 360/0.2 1800个点
第二行,groundScanInd 为8 The ground point cannot be on it

                lowerInd = j + ( i )*Horizon_SCAN;//下面的点                upperInd = j + (i+1)*Horizon_SCAN;//上面的点

Calculate the indices of the two points
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;                }              

Determine whether two points are valid,Point is invalidintensity为-1
There is one point that is invalid 标志位 至-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 and horizontal angle,就是这个公式

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

sensorMountAngle 是 liadr is the inclination angle from the horizontal plane
Here is the horizontal angle of those two points,和10°做比较,Determine whether it is a ground point
How to make put flag bit 至 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 ,Does not participate in the extraction of subsequent line features and surface features                }            }        }

After judging the ground point,Iterate over each point again,
If past that point is Ground point or invalid point,则把 labelMat The point flag on the -1 .
labelMat 至为 -1 ,Does not participate in the extraction of subsequent line features and surface features

        //Ground point visualization         if (pubGroundCloud.getNumSubscribers() != 0){//If there is a node to subscribe to this ground pointtopic 再进行发布            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]);//add point to Ground point cloud It will be published later                }            }        }    }

Finally, visualize the ground points
If there is a node to subscribe to this ground pointtopic 再进行发布
遍历0-groundScanInd Get every point,Determine if it is a ground point,then add that point to groundCloud 中

It will be published later

Post it here

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

topicGot the name is/ground_cloud

pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2> ("/ground_cloud", 1);

gazebo测试

The effect is still pretty good,No outliers occurred

But because there is no height to be judged,As mentioned above, there will be a problem in this case