Edit computer vision life

Zach, a mobile robot practitioner, loves the mobile robot industry and is determined to help a better life with science and technology.

The first step in the design idea of logo-beam framework is to extract and separate the ground. This article will explain in detail how logo-beam performs ground extraction.

Idea of ground extraction

As shown in the above figure, the two connected scanning lines hit two points A() and B() in the same column on the ground. Calculate the height difference and plane distance between points A and B, and calculate and construct the triangular angle; If, two points are considered as ground points.

The above method of detecting the ground is relatively simple. If some flat large mesas are encountered, they will also be misidentified as the ground. We can use the installation position of lidar as a priori information (such as altitude information) to further judge the ground point.

Implementation of ground extraction source code

Before looking at the ground extraction source code, let's take a look at several key parameters. In the file logo-logo / logo-logo / include / utility. H:

// VLP-16
extern const int N_SCAN = 16; // Number of lidar lines
extern const int Horizon_SCAN = 1800; // VLP-16 one harness 1800 points, 360 degrees 
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 will sweep to the ground. Here, take 7 harnesses to sweep to the ground

The above parameters define several attribute values of Velodyne-16 line lidar, which will be used in later codes. Except extern const int groundScanInd = 7;, Other attribute values may be easier to understand.

When detecting the ground point cloud, logo-beam does not traverse all scans (scan lines), because the radar is placed horizontally, some scans (scan lines) shoot into the sky, and only seven scans (scan lines) close to the ground are taken in the frame

In the file logo-logo / logo-logo / SRC / imageprojection.cpp, several important variables are introduced first:

cv::Mat rangeMat; // range matrix for range image (range Map)
cv::Mat labelMat; // label matrix for segmentaiton marking
cv::Mat groundMat; // ground matrix for ground cloud marking

The above variable cv::Mat rangeMat; Corresponding to the range Map in this paper, all scanning points of linear lidar are formed into a range Map in this paper; Corresponding to the label in the paper, the label classifies and clusters the non ground points; cv::Mat groundMat; Ground points are marked in the code.

The ground extraction function is implemented in the Imageretrieval / void groundremoval() function. The code in this function is divided into three parts:

• Part I: traverse all points, detect ground points, and mark ground points in groundMat;

// groundMat: mark the identified ground points in the groundMat
// -1. No valid info to check if ground of not
//  0, initial value, after validation, means not ground
//  1. Ground
for (size_t j = 0; j < Horizon_SCAN; ++j) { // Traverse the column, a column of 1800 points
    for (size_t i = 0; i < groundScanInd; ++i) { // Traverse line, 7 ground harness lines
        // Point cloud ID of two rows connected in the same column
        lowerInd = j + ( i )*Horizon_SCAN;
        upperInd = j + (i+1)*Horizon_SCAN;
        // If the selected two points have invalid points, the point lowerInd or (i,j) is also invalid in the point cloud map groundMat
        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;
        }
                
        // Obtain the difference between two points lowerInd and upperInd in the x/y/z 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 the angle between the vertical height diffZ and the horizontal distance of two points lowerInd and upperInd
        angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
        // If the above included angle is less than 10, (i, j) and (i+1, j) are set as ground mark 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 extract two points in two rows connected to the same column:

// Point cloud ID of two rows connected in the same column
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, mark it as and take the point again;

// If the selected two points have invalid points, the point lowerInd or (i,j) is also invalid in the point cloud map groundMat
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;
}

Finally, calculate the angle between the height difference between the two points lowerInd and upperInd and the plane distance. If it is less than, set it at the position corresponding to the groundMap.

// Obtain the difference between two points lowerInd and upperInd in the x/y/z 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 the angle between the vertical height diffZ and the horizontal distance of two points lowerInd and upperInd
angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
// If the above included angle is less than 10, (i, j) and (i+1, j) are set as ground mark 1
if (abs(angle - sensorMountAngle) <= 10) {
    groundMat.at<int8_t>(i,j) = 1;
    groundMat.at<int8_t>(i+1,j) = 1;
}

• Part II: remove ground points and invalid points in labelMat; labelMat clusters all the points. We need to eliminate the ground points and invalid points. The invalid points here can be understood as the points that have not received the return signal.

// Remove ground points and invalid points (points that do not receive return values) in labelMat
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 the ground cloud;

// Extract 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 and can be displayed through rviz. As shown in the figure below, in order to clearly distinguish the ground point cloud, we adjust its color to.

reference material

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