A lightweight pose estimation method and system based on directional feature selection

Abstract

The present invention provides a lightweight pose estimation method and system based on directional feature selection. The method includes the following steps: performing ground point cloud segmentation on the original laser point cloud to screen out the ground point cloud and obtain the point cloud in space; based on the polar coordinate grid representation method of the concentric region model, dividing the point cloud in space into blocks and then obtaining the planarity index, normal vector, and degradation direction of the point cloud within the block; obtaining the feature score of the current environment, and obtaining the importance score of each unit space according to the feature score, planarity, and normal vector of the current environment; screening the feature set according to the importance score of the unit space, and adding the point cloud set with a high score to the feature subset; inputting the screened features into the laser odometer module for pose estimation. By performing directional selection on the laser point cloud features, the present invention can reduce the algorithm time consumption by about half under the condition of ensuring almost the same accuracy, and improve the real-time performance of the algorithm.

Description

Technical Field

[0001] The present invention belongs to the technical field of vehicle autonomous positioning, and particularly relates to a lightweight pose estimation method and system based on directional feature selection. Background Art

[0002] The odometer based on lidar is an important autonomous positioning technology in the field of driverless. The specific algorithm generally obtains the relative position transformation through a matching algorithm such as the iterative closest point for the point cloud data measured by the lidar, so as to estimate the position change of the intelligent body. However, the commonly used lidar odometer methods generally rely on a huge number of lidar measurement degrees, which contain a large amount of redundant data and noise, and the algorithm efficiency is low. Since the contribution degrees of different feature constraints of lidar measurement to pose solution are different, screening the feature point cloud participating in the lidar odometer matching calculation can improve the algorithm efficiency and stability.

[0003] Currently, the commonly used methods generally perform feature screening through statistical sampling algorithms. We propose a directional feature selection method that can significantly reduce the number of features participating in the estimation to improve the algorithm efficiency while ensuring similar accuracy results. In addition, the uniformity of the constraint selection direction is ensured to maximize the response to the challenges brought by the degradation of the feature environment. Summary of the Invention

[0004] In order to solve the problems existing in the prior art, the present invention provides a directional feature selection method, which can significantly reduce the number of features participating in the estimation while ensuring similar accuracy results, thereby improving the efficiency.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is: a lightweight pose estimation method based on directional feature selection, including the following steps:

[0006] The original lidar point cloud is evenly divided into different blocks centered on the sensor and with the same angle on the horizontal plane by surrounding the lidar in a circle.

[0007] The ground point cloud is removed from the original lidar point cloud to obtain the point cloud in space.

[0008] Based on the polar coordinate grid representation method of the concentric region model, the point cloud in space is divided into blocks.

[0009] For the divided point cloud, the planarity index, normal vector and degradation direction of the point cloud in the block are obtained.

[0010] The feature score of the current environment is obtained, and the importance score of each unit space is obtained according to the feature score, planarity and normal vector of the current environment.

[0011] Filter the feature set according to the importance score of the unit space, and add the point cloud set with a high score to the feature subset;

[0012] Use the filtered features to input into the laser odometer module for pose estimation.

[0013] Obtain the candidate ground point cloud specifically as follows: Starting from the sub-block closest to the center of the lidar, calculate the slope of the lowest point cloud along the horizontal plane for each sub-block and in the direction of the lidar ray, and connect the slopes of the straight lines , where ; If the absolute value of the slope is less than the set value, then mark the lidar points from the origin to the corresponding straight line as the ground point cloud.

[0014] Based on the polar coordinate grid representation method of the concentric region model, the point cloud in space is partitioned specifically as follows: Divide the observation laser space of the lidar, estimate the features of the laser data in the unit space, and for a point in space, according to value to put it into the corresponding block in the polar coordinate grid to obtain the partitioned space point cloud.

[0015] For the partitioned space point cloud, obtain the planarity and normal vector of the point cloud within the block, including:

[0016] Estimate the covariance matrix for the point cloud within the block. For the point cloud set within the block, calculate its spatial mean , let , then the covariance matrix ;

[0017] Perform eigenvalue decomposition on the covariance matrix to obtain three eigenvalues λ 1 、λ 2 and λ 3, and the eigenvectors corresponding to the three eigenvalues are , , where > > , define the planarity index of the block point cloud: , and the normal vector of the point cloud within the corresponding block is directly replaced by the eigenvector λ corresponding to the smallest eigenvalue 3.

[0018] Combine all the estimated point cloud normal vectors into a normal vector set matrix , calculate the information matrix of the normal vector, according to the information matrix Magnitude of the determinant Compare with the set value to evaluate the quality of the current environment characteristics. When the determinant of the information matrix is less than the set value, it indicates that the current environment characteristics are sparse and disorderly;

[0019] Use the ratio of the two largest eigenvalues to evaluate the characteristic distribution of the current environment, , and the ratio The larger it is, the more uneven the current characteristic distribution is, and it is easy to degenerate in the direction of the corresponding eigenvector; Assign a higher score to the point cloud area with a similar orientation to the degeneration direction, and call the eigenvector corresponding to the smaller eigenvalue on the plane the degeneration direction.

[0020] The importance score is:

[0021]

[0022] where represents the normal vector of the feature point cloud of the current block.

[0023] According to the importance score of the unit space, screen the feature set, and add the point cloud set with a high score to the feature subset. Specifically: traverse each unit space feature point cloud in the full feature point cloud obtained by the lidar, and calculate the overall importance score considering flatness and directionality , under the premise that the number of screened feature sets does not exceed the set screening number, sort by the importance score from high to low, and select the point cloud set with a high score to add to the feature subset.

[0024] According to the technical concept of the above method, the present invention provides a lightweight pose estimation system based on directional feature selection, including a segmentation module, a spatial point cloud acquisition module, a spatial segmentation module, a feature acquisition module, an importance score acquisition module, a screening module, and an estimation module;

[0025] The segmentation module is used to evenly divide the original laser point cloud around the lidar in a horizontal plane into different blocks centered on the sensor with the same angle;

[0026] The spatial point cloud acquisition module is used to screen out the ground point cloud from the original laser point cloud to obtain the point cloud in space;

[0027] The spatial segmentation module is used to divide the point cloud in space based on the polar coordinate grid representation method of the concentric region model;

[0028] The feature acquisition module is used to obtain the planarity index, normal vector, and degeneration direction of the point cloud in the block for the segmented point cloud;

[0029] The importance score acquisition module is used to obtain the feature score of the current environment, and obtain the importance score of each unit space according to the feature score, planarity and normal vector of the current environment;

[0030] The screening module is used to screen the feature set according to the importance score of the unit space, and the point cloud set with a high score is added to the feature subset;

[0031] The estimation module inputs the screened features into the laser odometer module for pose estimation.

[0032] In addition, a computer device is provided, including a processor and a memory. The memory is used to store computer-executable programs. The processor reads and executes the computer-executable programs from the memory. When the processor executes the programs, it can implement the lightweight pose estimation method based on directional feature selection of the present invention.

[0033] The present invention also provides a computer-readable storage medium. A computer program is stored in the computer-readable storage medium. When the computer program is executed by a processor, it can implement the lightweight pose estimation method based on directional feature selection of the present invention.

[0034] Compared with the prior art, the present invention has at least the following beneficial effects:

[0035] Compared with open-source / existing laser positioning technologies, by making directional selections on laser point cloud features, the algorithm time consumption can be reduced by about half while ensuring almost the same accuracy, improving the real-time performance of the algorithm. The additional time overhead brought by the feature selection algorithm can be ignored. Compared with existing feature selection algorithms, most feature selection algorithms use statistical methods to select the feature subset that contributes the most to pose estimation. This method usually lacks interpretability and does not consider the degradation direction of the current environment, and is prone to odometer drift failure in some scenarios. However, we use the information provided by the plane feature normal vector, evaluate according to the feature constraints of the current environment, and give higher scores to the plane features that can bring constraints in the degradation direction. Selecting the feature subset according to the scores of the features during the feature selection process can make the feature selection algorithm more stable. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 It is a schematic diagram of the ground segmentation method.

[0037] Figure 2 It is the polar coordinate grid representation based on the concentric region model.

[0038] Figure 3 It is a schematic flowchart of an implementable method.

[0039] Figure 4The flowchart of the method described in the present invention is shown in a diagrammatic form. Detailed implementation manners

[0040] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0041] In the description of the present invention, it should be understood that the terms "include" and "comprise" indicate the presence of the described features, wholes, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, wholes, steps, operations, elements, components and / or their combinations.

[0042] The method described in the present invention includes the following steps: data preprocessing, point cloud geometric estimation, feature screening, and laser odometry pose estimation.

[0043] Data preprocessing includes steps of ground point cloud segmentation, point cloud feature estimation, and space partitioning.

[0044] Due to the mechanical characteristics of lidar, the data in the direction perpendicular to the ground is relatively sparse (usually only 16, 32, or 64 lines of data). The feature selection method of the present invention only selects non-ground features; the ground point cloud segmentation method first evenly divides a circle around the lidar into different blocks centered on the sensor with the same angle, such as Figure 2 As shown, let a point cloud in space be Then each point will belong to a specific block according to its orientation angle The specific block The lidar in a circle is evenly divided into blocks. The number of blocks determines the accuracy of ground segmentation. The size of the blocks determines the accuracy of ground segmentation. For each block Sn, it is divided into different blocks according to the number of lines of the lidar, as shown by the short lines in Figure 1

[0045] Starting from the sub-block closest to the center of the lidar, calculate the slope of the lowest point cloud in each sub-block and along the lidar ray direction in the horizontal plane, and connect the slopes of the straight lines where If the absolute value of the slope is less than the set value, then the lidar points from the origin to the corresponding straight line are marked as ground point clouds.

[0046] For the original laser point cloud, according to the calculation of the smoothness of the point cloud and its adjacent point clouds, the point cloud is classified into plane features and edge features. Typical plane features include walls; edge features include tree branches, utility poles, etc. Since the number of edge features is much smaller than that of plane features and has little impact on the result of pose estimation, the method described in the present invention only selects plane features for processing.

[0047] Since the geometric features of the point cloud need to be estimated when selecting features in the present invention, the point cloud in space is segmented. Traditional segmentation algorithms use methods such as voxel grids to divide the uniform space. The present invention uses a polar coordinate grid representation method based on a concentric region model, which is more in line with the ranging principle of lidar. The size of the segmentation region is increased for sparse point clouds at long distances and dense point clouds at short distances to prevent under-estimation and over-fitting problems respectively, and to retain the geometric features of the segmented point cloud to the greatest extent. For a point in space , according to value to put it into the corresponding block in the polar coordinate grid.

[0048] Estimation of point cloud geometric features

[0049] For the segmented point cloud, the planarity and normal vector of the point cloud within the block are obtained for subsequent feature selection. First, the covariance matrix of the point cloud within the block is estimated. For the set of point clouds within the block , its spatial mean can be calculated, let , then the covariance matrix can be expressed as , where is the deviation-from-mean matrix and n is the number of point clouds within the block; then the covariance matrix is eigen-decomposed to obtain three eigenvalues λ 1 、λ 2 and λ 3, and the eigenvectors corresponding to the three eigenvalues are , , where > > , the planarity index of the block point cloud is defined as: , there are a total of three eigenvalues, and the denominator is the sum of the three eigenvalues, which can also be written as ). The larger the λ , the flatter the point cloud in the space, the smaller the noise in constructing the residual equation for the optimization problem, and the higher the contribution degree of the planarity index to pose estimation. The normal vector of the point cloud within the corresponding block can be directly replaced by the eigenvector corresponding to the smallest eigenvalue

[0050] Feature set screening

[0051] First, combine all the estimated point cloud normal vectors into a normal vector set matrix, and calculate the information matrix of the normal vectors , and according to the magnitude of the determinant of the information matrix , the quality of the features in the current environment can be evaluated; when the determinant of the information matrix is less than the set value, it indicates that the features in the current environment are sparse and messy, and there may be a risk of degradation. In this case, all the feature sets are used for pose estimation.

[0052] If the value of the determinant is greater than the set value, it means that the current number of features is sufficient and the constraint situation is good, and redundant feature points can be screened out. First, perform eigenvalue decomposition on the normal vector matrix N to obtain three eigenvalues λ 1, λ 2, λ 3, and their corresponding eigenvectors are , where > > . The present invention only screens non-ground features. Therefore, the ratio of the two largest eigenvalues is used to evaluate the feature distribution in the current environment. The ratio of the two largest eigenvalues is denoted as , The larger it is, the more uneven the current feature distribution is, and it is easy to generate degradation in the direction of the corresponding eigenvector, and then the degradation direction is obtained. Based on this assumption, a higher score is given to the point cloud region whose orientation is close to the degradation direction. The eigenvector corresponding to the smaller eigenvalue in the xy plane is called the degradation direction.

[0053] In the step of scoring the point cloud, the flatness and directionality of the block point cloud will be considered simultaneously. The overall goal is: if the point cloud set in the unit space is flatter, the score will be higher; if the normal vector of the point cloud set in the unit space is more consistent with the degradation direction, then the importance of the point cloud set in the unit space will also be correspondingly greater. An importance score of the block point cloud is designed. Among them represents the normal vector of the current block feature point cloud.

[0054] Finally, the feature set is screened according to the importance score of the unit space; for the full feature point cloud obtained by the lidar, first calculate the determinant of the information matrix . If the calculated value is greater than the set threshold, then half of the full feature point cloud is selected as the upper limit of the selected feature subset. First, traverse the feature point cloud of each unit space, and calculate the overall importance score considering flatness and directionality , on the premise of ensuring that the number of the screened feature sets does not exceed the set number of screenings, sort them in descending order according to the importance scores, and select the point cloud sets with high scores to be added to the feature subset. When the number of the feature subsets reaches the set upper limit, the algorithm ends. Hand over the screened feature subset to the traditional laser odometer optimization backend for calculation.

[0055] With the same technical concept as the method, the present invention also provides a lightweight pose estimation system based on directional feature selection, including a segmentation module, a spatial point cloud acquisition module, a spatial segmentation module, a feature acquisition module, an importance score acquisition module, a screening module, and an estimation module;

[0056] The segmentation module is used to evenly segment the original laser point cloud around the lidar in a horizontal plane into different blocks centered on the sensor with the same angle;

[0057] The spatial point cloud acquisition module is used to screen out the ground point cloud from the original laser point cloud to obtain the point cloud in space;

[0058] The spatial segmentation module is used to block the point cloud in space based on the polar coordinate grid representation method of the concentric region model;

[0059] The feature acquisition module is used to obtain the planarity index, normal vector, and degradation direction of the point cloud within the block for the segmented point cloud;

[0060] The importance score acquisition module is used to obtain the feature score of the current environment, and obtain the importance score of each unit space according to the feature score, planarity, and normal vector of the current environment;

[0061] The screening module is used to screen the feature set according to the importance score of the unit space, and add the point cloud set with a high score to the feature subset;

[0062] The estimation module inputs the screened features into the laser odometer module for pose estimation.

[0063] The present invention can also provide a computer device, including a processor and a memory. The memory is used to store computer executable programs. The processor reads the computer executable programs from the memory and executes them. When the processor executes the computer executable programs, it can implement the lightweight pose estimation method based on directional feature selection of the present invention.

[0064] On the other hand, the present invention also provides a computer-readable storage medium. A computer program is stored in the computer-readable storage medium. When the computer program is executed by a processor, it can implement the lightweight pose estimation method based on directional feature selection of the present invention.

[0065] The computer device may be a laptop computer, a desktop computer or a workstation.

[0066] For the processor described in the present invention, it may be a central processing unit (CPU), a graphics processing unit (GPU), a digital signal processor (DSP), an application specific integrated circuit (ASIC) or a field programmable gate array (FPGA).

[0067] For the memory described in the present invention, it may be an internal storage unit of a laptop computer, a desktop computer or a workstation, such as a memory or a hard disk; or an external storage unit may be adopted, such as a mobile hard disk or a flash card.

[0068] The computer-readable storage medium may include a computer storage medium and a communication medium. The computer storage medium includes volatile and non-volatile, removable and non-removable media implemented by any method or technology for storing information such as computer-readable instructions, data structures, program modules or other data. The computer-readable storage medium may include: read-only memory (ROM), random access memory (RAM), solid state drives (SSD) or optical discs, etc. Among them, the random access memory may include resistive random access memory (ReRAM) and dynamic random access memory (DRAM).

[0069] Finally, it should be noted that the above description is only for illustrating the specific implementation manners of the present invention, but the protection scope of the present invention is not limited thereto. Those skilled in the art of this technology should understand that any modifications or deformations made based on the technical solutions of the present invention and its inventive concept should be covered within the protection scope of the present invention.

Claims

1. A lightweight pose estimation method based on directional feature selection, characterized in that It includes the following steps: The original lidar point cloud is evenly segmented into different blocks centered on the sensor with the same angle around the lidar in the horizontal plane; The ground point cloud is filtered out from the original lidar point cloud to obtain the point cloud in space; Based on the polar coordinate grid representation method of the concentric region model, the point cloud in space is segmented; For the segmented point cloud, the planarity index, normal vector, and degradation direction of the point cloud within the corresponding block are obtained; The feature score of the current environment is obtained, and the importance score of each unit space is obtained according to the feature score of the current environment, the planarity index, and the normal vector; The feature set is filtered according to the importance score of the unit space, and the point cloud set with a high score is added to the feature subset; The filtered features are used as input to the lidar odometry module for pose estimation; For the segmented point cloud in space, obtaining the planarity index and normal vector of the point cloud within the corresponding block includes: Estimate the covariance matrix for the point cloud within the corresponding block. For the set of point clouds within the corresponding block , calculate its spatial mean . Let , then the covariance matrix , is the deviation-from-mean matrix, and n is the number of point clouds within the block; For the covariance matrix perform eigenvalue decomposition to obtain three eigenvalues λ 1 、λ 2 and λ 3. The eigenvectors corresponding to the three eigenvalues are 、 , where > > . Define the planarity index of the point cloud within the corresponding block: . The normal vector of the point cloud within the corresponding block is directly replaced by the eigenvector λ corresponding to the smallest eigenvalue 3; Obtain the feature score of the current environment. Obtaining the importance score of each unit space according to the feature score of the current environment, the planarity index, and the normal vector specifically includes: combining the normal vectors of all estimated point clouds into a normal vector set matrix , calculate the information matrix of the normal vector , according to the information matrix the magnitude of the determinant compare with the set value to evaluate the quality of the features of the current environment. When the determinant of the information matrix is less than the set value, it indicates that the features of the current environment are sparse and disorderly; Use the ratio of the two largest eigenvalues to evaluate the feature distribution of the current environment. , the ratio The larger the ratio, the more uneven the current feature distribution, and it is easy to produce degeneration in the direction of the corresponding feature vector; assign a higher importance score to the point cloud region whose orientation is close to the degeneration direction, and the eigenvector corresponding to the smaller eigenvalue on the plane is called the degeneration direction. The way of assigning the importance score is: Among them represents the normal vector of the point cloud within the current block; Filter the feature set according to the importance score of the unit space, and add the point cloud set with a high score to the feature subset. Specifically: traverse the importance scores of all unit spaces , under the premise that the number of filtered feature sets does not exceed the set number of filters, sort the importance scores from high to low, and select the point cloud set with a high score to add to the feature subset.

2. The lightweight pose estimation method based on directional feature selection according to claim 1, characterized in that Obtaining the ground point cloud specifically involves: starting from the sub-block closest to the center of the lidar, calculating the slope of the lowest point cloud in each sub-block and along the horizontal plane of the lidar ray direction, and connecting the slopes of the straight lines where ; if the absolute value of the slope is less than the set value, then the lidar point cloud from the origin to the corresponding straight line is marked as the ground point cloud.

3. The lightweight pose estimation method based on directional feature selection according to claim 1, characterized in that, The method for representing a polar coordinate grid based on a concentric region model to partition a point cloud in space is as follows: For a point in the spatial point cloud , according to value, it is placed into the corresponding block in the polar coordinate grid to obtain the partitioned spatial point cloud.

4. A lightweight pose estimation system based on directional feature selection, characterized in that It includes a segmentation module, a spatial point cloud acquisition module, a spatial segmentation module, a feature acquisition module, an importance score acquisition module, a screening module, and an estimation module; The segmentation module is used to evenly segment the original lidar point cloud into different blocks centered on the sensor with the same angle around the lidar in the horizontal plane; The spatial point cloud acquisition module is used to filter out the ground point cloud from the original lidar point cloud to obtain the point cloud in space; The spatial segmentation module is used to segment the point cloud in space based on the polar coordinate grid representation method of the concentric region model; The feature acquisition module is used to obtain the planarity index, normal vector, and degradation direction of the point cloud within the corresponding block for the segmented point cloud; The importance score acquisition module is used to obtain the feature score of the current environment, and obtain the importance score of each unit space according to the feature score of the current environment, the planarity index, and the normal vector; The screening module is used to filter the feature set according to the importance score of the unit space, and add the point cloud set with a high score to the feature subset; The estimation module uses the filtered features as input to the lidar odometry module for pose estimation; Among them, for the segmented point cloud in space, obtaining the planarity index and normal vector of the point cloud within the corresponding block includes: Estimate the covariance matrix for the point cloud within the corresponding block. For the set of point clouds within the corresponding block , calculate its spatial mean . Let , then the covariance matrix , is the deviation-from-mean matrix, and n is the number of point clouds within the block; For the covariance matrix perform eigenvalue decomposition to obtain three eigenvalues λ 1 、λ 2 and λ 3. The eigenvectors corresponding to the three eigenvalues are 、 , where > > . Define the planarity index of the point cloud within the corresponding block: . The normal vector of the point cloud within the corresponding block is directly replaced by the eigenvector λ corresponding to the minimum eigenvalue 3; Obtain the feature score of the current environment. Obtaining the importance score of each unit space based on the feature score of the current environment, the planarity index, and the normal vector specifically includes: combining the normal vectors of all estimated point clouds into a normal vector set matrix , calculate the information matrix of the normal vector , according to the information matrix the magnitude of the determinant compare with a set value to evaluate the quality of the features of the current environment. When the determinant of the information matrix is less than the set value, it indicates that the features of the current environment are sparse and messy; Use the ratio of the two largest eigenvalues to evaluate the feature distribution of the current environment. , the ratio The larger the value, the more uneven the current feature distribution, and it is easy to produce degeneration in the direction of the corresponding feature vector; assign higher importance scores to the point cloud regions with similar directions to the degeneration direction, and call the feature vector corresponding to the smaller eigenvalue on the plane the degeneration direction. The way of assigning the importance score is: Among them represents the normal vector of the point cloud within the current block; Screen the feature set according to the importance score of the unit space, and add the point cloud set with a high score to the feature subset. Specifically: traverse the importance scores of all unit spaces , under the premise that the number of screened feature sets does not exceed the set screening number, sort by the importance score from high to low, and select the point cloud set with a high score to add to the feature subset.

5. A computer device, characterized in that, It includes a processor and a memory. The memory is used to store computer-executable programs. The processor reads and executes the computer-executable programs from the memory. When the processor executes the programs, it can implement the lightweight pose estimation method based on directional feature selection described in any one of claims 1-3.

6. A computer-readable storage medium, characterized in that A computer program is stored in a computer-readable storage medium. When the computer program is executed by a processor, it can implement the lightweight pose estimation method based on directional feature selection described in any one of claims 1-3.

Images