A two-stage sparse lidar point cloud feature extraction method based on cells

Abstract

The present invention provides a two-stage sparse laser radar point cloud feature extraction method based on cells, which comprises: step 1: reading original three-dimensional point cloud data; step 2: dividing a frame of point cloud into N*S cells; step 3: marking the cells corresponding to each point cloud; step 4, traversing each cell, and entering step 5 when the number of points in the cell is greater than a set value; step 5, calculating the normalized mean coordinates of the cell; step 6, constructing the covariance matrix of the cell and solving it to obtain the eigenvalue; step 7, judging the planarity or linearity of the cell according to the eigenvalue; step 81, the cell is judged to be planar, the covariance of each point in the cell is calculated, the eigenvalue is obtained, and step 91 is entered; step 82, the cell is judged to be linear, the covariance of each point in the cell is calculated, the eigenvalue is obtained, and step 92 is entered; step 91, according to the planarity of the point cloud, the plane feature points are obtained; step 92, according to the linearity of the point cloud, the line feature points are obtained.

Description

Technical Field

[0001] The present invention relates to the field of laser radar, and in particular to a cell-based two-stage sparse laser radar point cloud feature extraction method. Background Art

[0002] With the development of technology in industries such as robotics and autonomous driving, LiDAR is widely used as one of the sensors. The original point cloud obtained by the robot through LiDAR contains a large amount of point cloud data. It is very time-consuming to process a large amount of original point cloud, so one of the steps to speed up the robot method is to extract effective feature points from the original point cloud.

[0003] The existing three-dimensional laser radar point cloud feature extraction methods mainly include point cloud curvature feature extraction methods and PCA (Principal Components Analysis) feature extraction methods. The point cloud curvature feature extraction method is to use the horizontal resolution and vertical resolution of the laser radar to calculate the laser radar beam to which each point cloud belongs, traverse each point cloud on each beam, select the nearest points before and after the same beam to calculate the curvature of each point cloud, and select the corner point features and plane features according to the calculated curvature value. However, in the scene of geometric structure degradation (such as an open outdoor scene), the curvature of the points scanned by the laser radar is basically the same, which will cause the feature extraction method to fail. The PCA feature extraction method is to search for several nearest neighbor points from a global frame of point cloud for the current point, fit the current point and these neighboring points into a plane, calculate the covariance matrix of the plane to obtain the eigenvalues ​​and eigenvectors, and judge whether the current point is a linear point or a planar point through the relationship between the eigenvalues, thereby extracting feature points. The PCA-based extraction method needs to search for the nearest points from a frame of global point cloud, and needs to fit each point into a local plane to calculate the covariance matrix, which makes the method very time-consuming. Therefore, a new feature extraction method needs to be proposed. Summary of the invention

[0004] The purpose of the present invention is to provide a fast and accurate laser radar sparse point cloud feature extraction method, which divides a frame of point cloud into multiple cells according to spatial position. Through a two-stage fast and robust point cloud feature extraction method for cells, the problem that the existing method relies on resolution to calculate the line beam is not robust enough for multiple types of radars and the method has high time complexity in extracting features is solved.

[0005] The present invention provides a two-stage sparse laser radar point cloud feature extraction method based on cells, which comprises: step 1: reading original three-dimensional point cloud data; step 2: dividing a frame of point cloud into N*S cells; step 3: marking the cells corresponding to each point cloud; step 4, traversing each cell, and entering step 5 when the number of points in the cell is greater than a set value; step 5, calculating the normalized mean coordinates of the cell; step 6, constructing the covariance matrix of the cell and solving it to obtain the eigenvalue; step 7, judging the planarity or linearity of the cell according to the eigenvalue; step 81, the cell is judged to be planar, the covariance of each point in the cell is calculated, the eigenvalue is obtained, and step 91 is entered; step 82, the cell is judged to be linear, the covariance of each point in the cell is calculated, the eigenvalue is obtained, and step 92 is entered; step 91, according to the planarity of the point cloud, the plane feature points are obtained; step 92, according to the linearity of the point cloud, the line feature points are obtained.

[0006] Preferably, step 2 is specifically as follows: according to the radius of the point cloud, every x meters is divided into a circular ring, a total of N rings are divided; according to the azimuth, every y degrees is divided into a sector, a total of S sectors are divided; each frame of point cloud is divided into N*S different cells, marked as Mat[Ni,Si].

[0007] Preferably, step 3 is specifically as follows: calculating the radius distance RangId of each point cloud in a frame and calculating the azimuth angle AngleId of each point cloud, storing them in the corresponding cell Mat[RangId,AngleId], assigning all points to the corresponding cells and recording them.

[0008] Preferably, step 4 is specifically as follows: traverse each cell in each frame of point cloud, and when the number of point clouds in a cell is less than a set threshold thre_num, skip the cell to the next cell; when the number of point clouds is greater than or equal to the set threshold thre_num, proceed to step 5.

[0009] Preferably, step 5 is specifically as follows: calculate the sum Sum of the three-dimensional coordinates of all points in the cell, divide the Sum by the number of point clouds in the cell to obtain the arithmetic mean of the coordinates, subtract the arithmetic mean of the coordinates from the coordinates of each point in the cell and add them up to obtain the average sum Sum_mean, divide the Sum_mean by the total number of points in the cell to obtain the normalized mean coordinates of the cell.

[0010] Preferably, step 6 is specifically as follows: fit all points in the cell into a local plane and construct a covariance matrix with normalized mean coordinates, so as to obtain a 3x3 covariance matrix, solve the covariance matrix, obtain eigenvalues ​​and eigenvectors, and sort the eigenvalues ​​so that λ1>λ2>λ3>0.

[0011] Preferably, step 7 is specifically as follows: when (λ2-λ3) / λ1>planarity threshold plane_threshold1, the cell is considered to be a planar cell; when (λ1-λ2) / λ1>linearity threshold line_threshold, the cell is considered to be a linear cell.

[0012] Preferably, step 81 and step 91 are specifically as follows: Step 81: Each point in the cell forms a new covariance matrix with other points in the same cell, and the new covariance is solved to obtain the eigenvalue λ11>λ22>λ33>0, and the planarity of each point cloud is determined; Step 91: When the point cloud satisfies (λ22-λ33) / λ11>plane_threshold2, the point is selected as a plane feature point.

[0013] Preferably, step 82 and step 92 are specifically as follows: Step 82: Each point in the cell forms a new covariance matrix with other points in the same cell, and the new covariance is solved to obtain the eigenvalue λ111>λ222>λ333>0; Step 92: Determine the curvature of each point cloud (λ111 / (λ111+λ222+λ333)), and when the point cloud curvature satisfies (λ111 / (λ111+λ222+λ333))>Curvature_threshold, select the point as a linear feature point.

[0014] The two-stage sparse LiDAR point cloud feature extraction method based on cells and the two-stage sparse LiDAR point cloud feature extraction method based on cells provided by the present invention have the following beneficial effects:

[0015] The present invention does not need to calculate the line beam according to the horizontal and vertical resolution of the laser radar, but divides the point cloud into multiple cells according to the spatial position, which is applicable to all mechanical radars. The points in the same cell are taken as adjacent points, the time of searching for adjacent points from a frame of global point cloud is subtracted, the points in the same cell are fitted into a plane, the covariance matrix of the plane is calculated, and the eigenvalues ​​solved by the covariance matrix are used to determine whether the cell satisfies the plane characteristic or the linear characteristic; if the cell satisfies these two characteristics, the plane characteristics and linear characteristics of each point cloud in the cell are specifically calculated. If the number of point clouds in the cell is too small or the cell does not meet the two characteristics of the judgment, the cell is eliminated. We only extract features from the point cloud in the cell that meets the conditions, which reduces the amount of point cloud data and reduces the time complexity of the method. We also extract features through two-stage screening, which makes the extracted feature points more accurate and the method more efficient. Moreover, the proposed method for extracting features can adapt to the environment of geometric structure degradation, so the feature extraction method has good robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] Figure 1 This is a step diagram of a two-stage sparse lidar point cloud feature extraction method based on cells provided in the first embodiment of the present invention. DETAILED DESCRIPTION

[0017] The cell-based two-stage sparse lidar point cloud feature extraction method provided by the present invention is further described below in conjunction with the accompanying drawings. It should be pointed out that the technical solution and design principle of the present invention are described in detail below only as an optimized technical solution.

[0018] In the entire description of the present invention, it should be noted that directional words, such as the terms "center", "transverse", "longitudinal", "lateral", "length", "width", "thickness", "up", "down", "front", "back", "left", "right", "vertical", "perpendicular", "horizontal", "top", "bottom", "inside", "outside", "clockwise", "counterclockwise" and the like, indicating directions and positional relationships are directions or positional relationships based on the directions shown in the drawings or the common names used by those skilled in the art, and are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific direction, be constructed and operated in a specific direction, and should not be understood as limiting the specific protection scope of the present invention.

[0019] First, see Figure 1 The present invention provides a two-stage sparse laser radar point cloud feature extraction method based on cells, which includes: step 1: reading the original three-dimensional point cloud data; step 2: dividing a frame of point cloud into N*S cells; step 3: marking the cell corresponding to each point cloud; step 4, traversing each cell, and entering step 5 when the number of points in the cell is greater than the set value; step 5, calculating the normalized mean coordinates of the cell; step 6, constructing the covariance matrix of the cell and solving it to obtain the eigenvalue; step 7, judging the planarity or linearity of the cell according to the eigenvalue; step 81, the cell is judged to be planar, the covariance of each point in the cell is calculated, the eigenvalue is obtained, and the step 91 is entered; step 82, the cell is judged to be linear, the covariance of each point in the cell is calculated, the eigenvalue is obtained, and the step 92 is entered; step 91, according to the planarity of the point cloud, the plane feature point is obtained; step 92, according to the linearity of the point cloud, the line feature point is obtained.

[0020] For details, please refer to Figure 1 ,

[0021] Step 2 is as follows: according to the radius of the point cloud (from the center point to the maximum sensor range), every x meters is divided into a ring, and a total of N rings are divided; according to the azimuth (0 to 2π), every y degrees is divided into a sector, and a total of S sectors are divided; each frame of the point cloud is divided into N*S different cells, marked as Mat[Ni,Si];

[0022] Step 3 is as follows: calculate the radius distance RangId and the azimuth angle AngleId of each point cloud in a frame, store them in the corresponding cell Mat[RangId,AngleId], assign all points to the corresponding cells and record them;

[0023] Step 4 is as follows: traverse each cell in each frame of the point cloud. When the number of point clouds in a cell is less than the set threshold thre_num, skip the cell and go to the next cell; when the number of point clouds is greater than or equal to the set threshold thre_num, go to step 5;

[0024] Step 5 is as follows: calculate the sum of the three-dimensional coordinates of all points in the cell, divide the sum by the number of point clouds in the cell to get the coordinate arithmetic mean (i.e., expectation), subtract the coordinate arithmetic mean from the coordinates of each point in the cell and add them up to get the average sum Sum_mean, divide Sum_mean by the total number of points in the cell to get the normalized mean coordinates of the cell;

[0025] Step 6 is as follows: fit all points in the cell into a local plane and construct a covariance matrix using normalized mean coordinates, so as to obtain a 3x3 covariance matrix, solve the covariance matrix, obtain eigenvalues ​​and eigenvectors, and sort the eigenvalues ​​so that λ1>λ2>λ3>0;

[0026] Step 7 is specifically, when (λ2-λ3) / λ1>planarity threshold plane_threshold1 (artificially set planarity threshold), the cell is considered to be a planar cell; when (λ1-λ2) / λ1>linearity threshold line_threshold (artificially set linearity threshold), the cell is considered to be a linear cell. When the cell does not meet the planarity or linearity requirements, it returns to step 4 and enters the next cell until all cells are traversed to complete the extraction of plane feature points and linear feature points of a frame of point cloud;

[0027] Step 81 is specifically as follows: each point in a cell forms a new covariance matrix with other points in the same cell, solves the new covariance to obtain eigenvalues ​​λ11>λ22>λ33>0, and determines the planarity of each point cloud;

[0028] Step 82 is specifically as follows: each point in a cell forms a new covariance matrix with other points in the same cell, and the new covariance is solved to obtain the eigenvalue λ111>λ222>λ333>0;

[0029] Step 91 is specifically as follows: when the point cloud satisfies (λ22-λ33) / λ11>plane_threshold2, the point is selected as a plane feature point;

[0030] Step 92 is specifically as follows: determining the curvature of each point cloud (λ111 / (λ111+λ222+λ333)), and selecting the point as a linear feature point when the point cloud curvature satisfies (λ111 / (λ111+λ222+λ333))>Curvature_threshold.

[0031] The present invention adopts a more efficient two-stage feature extraction method based on cells, divides a frame of point cloud into multiple small cells, and fits the points in the same cell as adjacent points into a local plane. The covariance matrix of the local plane is calculated to determine the planarity and linearity of the cell, and a rough screening of the point cloud is performed. Then, the features of each point in the cells that meet the conditions are accurately extracted; the time complexity is reduced, the accuracy is improved, and the feature extraction using the proposed method can adapt to the environment of geometric structure degradation, so that the feature extraction method has good robustness.

[0032] The above are only preferred embodiments of the present invention. It should be noted that the above preferred embodiments should not be regarded as limiting the present invention, and the protection scope of the present invention should be based on the scope defined by the claims. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the spirit and scope of the present invention, and these improvements and modifications should also be regarded as the protection scope of the present invention.

Claims

1. A two-stage sparse lidar point cloud feature extraction method based on cells, characterized in that: It includes: Step 1: Read the original 3D point cloud data; Step 2: Divide a frame of point cloud into N*S cells; Step 3: Mark the cells corresponding to each point cloud; Step 4, traverse each cell, and when the number of points in the cell is greater than the set value, go to step 5; Step 5, calculate the normalized mean coordinates of the cell; Step 6, construct the covariance matrix of the cell and solve it to obtain the eigenvalue; Step 7, judging the planarity or linearity of the cell according to the eigenvalue; Step 81, the cell is judged to be planar, the covariance of each point in the cell is calculated, the eigenvalue is obtained, and the process proceeds to step 91; Step 81: Each point in a cell forms a new covariance matrix with other points in the same cell, solves the new covariance to obtain eigenvalues ​​λ11>λ22>λ33>0, and determines the planarity of each point cloud; Step 82, the cell is determined to be linear, the covariance of each point in the cell is calculated, the eigenvalue is obtained, and the process proceeds to step 92; The step 82: each point in the cell forms a new covariance matrix with other points in the same cell, and the new covariance is solved to obtain the eigenvalue λ111>λ222>λ333>0; Step 91, determine the planarity of each point cloud and obtain the plane feature points; Step 91: when the point cloud satisfies (λ22-λ33) / λ11>planarity threshold plane_threshold2, the point is selected as a plane feature point; Step 92, determine the linearity of each point cloud and obtain line feature points; The step 92: determine the curvature of each point cloud (λ111 / (λ111+λ222+λ333)), and select the point as a linear feature point when the point cloud curvature satisfies (λ111 / (λ111+λ222+λ333))>linear threshold Curvature_threshold.

2. The two-stage sparse laser radar point cloud feature extraction method based on cells according to claim 1 is characterized in that: Step 2 is as follows: according to the radius of the point cloud, every x meters is divided into a ring, a total of N rings, according to the azimuth angle, every y degrees is divided into a sector, a total of S sectors, and each frame of point cloud is divided into N*S different cells, marked as Mat[Ni,Si].

3. The two-stage sparse laser radar point cloud feature extraction method based on cells according to claim 2 is characterized in that: Step 3 is as follows: calculate the radius distance RangId and the azimuth angle AngleId of each point cloud in a frame, store them in the corresponding cell Mat[RangId,AngleId], assign all points to the corresponding cells and record them.

4. The two-stage sparse LiDAR point cloud feature extraction method based on cells according to claim 1, characterized in that: Step 4 is as follows: traverse each cell in each frame of the point cloud. When the number of point clouds in a cell is less than the set threshold thre_num, skip the cell and go to the next cell; when the number of point clouds is greater than or equal to the set threshold thre_num, go to step 5.

5. The two-stage sparse laser radar point cloud feature extraction method based on cells according to claim 4 is characterized in that: Step 5 is as follows: calculate the sum of the three-dimensional coordinates of all points in the cell, divide the sum by the number of point clouds in the cell to get the arithmetic mean of the coordinates, subtract the arithmetic mean of the coordinates from the coordinates of each point in the cell and add them up to get the average sum Sum_mean, divide Sum_mean by the total number of points in the cell to get the normalized mean coordinates of the cell.

6. The two-stage sparse LiDAR point cloud feature extraction method based on cells according to claim 5, characterized in that: Step 6 is specifically as follows: fit all points in the cell into a local plane and construct a covariance matrix with normalized mean coordinates to obtain a 3*3 covariance matrix, solve the covariance matrix, obtain eigenvalues ​​and eigenvectors, and sort the eigenvalues ​​so that λ1>λ2>λ3>0.

7. The cell-based two-stage sparse LiDAR point cloud feature extraction method according to claim 6, characterized in that: Specifically, step 7 is that when (λ2-λ3) / λ1>planarity threshold plane_threshold1, the cell is considered to be a planar cell; when (λ1-λ2) / λ1>linearity threshold line_threshold, the cell is considered to be a linear cell.

Images