Code practice | ground extraction with logo-loam

Edit computer vision life

The authors introduce ：Zach, Mobile robot practitioners , Love the mobile robot industry , Aspire to science and technology to help a better life .

LeGO-LOAM The first step of the framework design idea is to extract and separate the ground . This article will explain in detail LeGO-LOAM How to extract the ground .

The idea of ground extraction

As shown in the figure above , The two connected scan lines are marked at two points in the same column on the ground A() and B(). Calculation A,B Height difference between two points , Plane distance , Calculation and The triangle angle of the construction ; If , I think there are two points , Belongs to the ground point .

The above ground detection method is relatively simple , If you encounter some flat large countertops , It can also be mistaken for the ground . We can use the installation position of the lidar as a priori information ( For example, height information ) To further judge the ground point .

Ground extraction source code implementation

Before looking at the ground extraction source code , Let's take a look at some key parameters . In the file LEGO-LOAM/LeGO-LOAM/include/utility.h in ：

// VLP-16
extern const int N_SCAN = 16; //  Number of laser radar lines 
extern const int Horizon_SCAN = 1800; // VLP-16  A harness 1800 A little bit , 360 degree . 
extern const float ang_res_x = 0.2; //  Horizontal resolution of a single harness 
extern const float ang_res_y = 2.0; //  Vertical resolution between harnesses 
extern const float ang_bottom = 15.0+0.1; // 
extern const int groundScanInd = 7; //  Ground harness ID. Because the lidar is placed horizontally ,  Not all harnesses sweep to the ground , Here take 7 The harness sweeps to the ground 

The above parameters define Velodyne-16 Several attribute values of linear lidar , In the following code, we will use . except extern const int groundScanInd = 7;, Other attribute values may be easier to understand .

LeGO-LOAM When detecting ground point clouds , It's not all traversal scan( Scan line ) Of , Because the radar is placed horizontally , Part of it scan( Scan line ) It was shot into the sky , Only seven bars close to the ground are taken from the frame scan( Scan line )

In the file LEGO-LOAM/LeGO-LOAM/src/imageProjection.cpp in , Let's start with a few important variables ：

cv::Mat rangeMat; // range matrix for range image (range Map)
cv::Mat labelMat; // label matrix for segmentaiton marking ( For point cloud segmentation  label  matrix )
cv::Mat groundMat; // ground matrix for ground cloud marking ( Used to mark ground point clouds )

These variables cv::Mat rangeMat; Corresponding to range Map, In this paper, all scanning points of linear lidar Make up a range Map; Corresponding to label,label The non ground points are classified and clustered ;cv::Mat groundMat; The ground points are marked in the code .

The ground extraction function is implemented in imageProhection/void groundRemoval() Function . The code in this function is divided into three parts ：

• The first part ： Traverse all points , Detect ground points , stay groundMat Mark ground points in ;

// groundMat:  The identified ground points ,  Marked on groundMat in 
// -1, no valid info to check if ground of not( Invalid point )
//  0, initial value, after validation, means not ground ( Non ground point )
//  1, ground( Indicates the ground point )
for (size_t j = 0; j < Horizon_SCAN; ++j) { //  Ergodic column ,  A column of 1800 A little bit 
    for (size_t i = 0; i < groundScanInd; ++i) { //  Traversal line , Ground harness row 7 individual 
        //  Point clouds connected by two rows in the same column ID
        lowerInd = j + ( i )*Horizon_SCAN;
        upperInd = j + (i+1)*Horizon_SCAN;
        //  If the two points taken have invalid points ,  This point lowerInd perhaps (i,j) On the point cloud map groundMat Is also an invalid point in 
        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;
        }
                
        //  Get two points lowerInd and upperInd stay x/y/z Difference in direction 
        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;
        //  Calculate two points lowerInd and upperInd Vertical height diffZ Angle with horizontal distance 
        angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
        //  If the above included angle is less than 10,  be (i, j)  and  (i+1, j) Set as ground sign 1
        if (abs(angle - sensorMountAngle) <= 10) {
            groundMat.at<int8_t>(i,j) = 1;
            groundMat.at<int8_t>(i+1,j) = 1;
        }
    }
}

In the above code , First, two points of two rows connected in the same column are extracted in turn ：

//  Point clouds connected by two rows in the same column ID
lowerInd = j + ( i )*Horizon_SCAN;
upperInd = j + (i+1)*Horizon_SCAN;

then , Judge whether the two extracted points are invalid points according to the intensity value ; If it is an invalid point , Then it is marked with , Re point ;

//  If the two points taken have invalid points ,  This point lowerInd perhaps (i,j) On the point cloud map groundMat Is also an invalid point in 
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;
}

Last , Calculate two points lowerInd and upperInd The angle between the height difference and the plane distance , If it is less than , It's in groundMap The corresponding position is set to .

//  Get two points lowerInd and upperInd stay x/y/z Difference in direction 
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;
//  Calculate two points lowerInd and upperInd Vertical height diffZ Angle with horizontal distance 
angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
//  If the above included angle is less than 10,  be (i, j)  and  (i+1, j) Set as ground sign 1
if (abs(angle - sensorMountAngle) <= 10) {
    groundMat.at<int8_t>(i,j) = 1;
    groundMat.at<int8_t>(i+1,j) = 1;
}

• The second part ： stay labelMat Remove ground points and invalid points ;labelMat All the points are clustered and marked , We need to eliminate Ground point and Invalid point , there Invalid point It can be understood as the point where the return signal is not received .

//  stay labelMat Remove ground points and invalid points ( Point that did not receive the return value )
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 third part ： Extract the ground point cloud and store it in groundCloud;

//  Extract the ground point cloud and store it in  groundCloud
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]);
        }
    }
}

The extracted point cloud is published through the theme , Can pass rviz Show it . As shown in the figure below , In order to distinguish clearly Ground point cloud , Adjust its color to .

Reference material

1. https://github.com/RobustFieldAutonomyLab/LeGO-LOAM