A multi-stage point cloud registration method and system based on semantic feature decoupling

Abstract

This invention relates to a multi-stage point cloud registration method and system based on semantic feature decoupling. The method includes: S1. Semantic feature extraction, extracting three types of semantic features from the keyframe point cloud and the global map point cloud to be registered; S2. First registration stage—Yaw angle estimation, estimating the Yaw angle deviation using the vertical plane features of the keyframe to be registered and the global map; S3. Second registration stage—XY translation estimation, estimating the XY plane translation amount given the known Yaw deviation; S4. Third registration stage—ICP fine registration, performing ICP fine registration based on the Yaw and XY estimated in the first two stages to correct the Z-axis translation and pitch / roll angles. This invention not only reduces the dimensionality of the optimization problem and significantly reduces the probability of getting trapped in local optima, but also avoids the computational burden of traditional 6-DoF full-space search.

Description

Technical Field

[0001] This invention relates to the field of point cloud registration technology, and in particular to a multi-stage point cloud registration method and system based on semantic feature decoupling. Background Technology

[0002] In large outdoor scenarios such as automated ports and smart parks, LiDAR is widely used for environmental perception, mapping, and localization. Point cloud registration is a key technology for unifying point cloud data collected at different times into the same coordinate system. Its core is to solve the rigid body transformation (including rotation and translation, a total of 6 degrees of freedom) between two frames of point cloud data.

[0003] The existing technology has the following shortcomings: Common registration methods such as ICP / NICP / NDT use 6-DoF as a whole for iterative solution, which is sensitive to the initial pose and is prone to getting trapped in local optima. When the initial deviation is large, the registration failure rate is high.

[0004] Feature-point-based registration methods (such as FPFH and SHOT) are computationally intensive in large-scale scenes and rely on textured geometric structures, resulting in poor performance in feature-sparse regions such as ports.

[0005] Existing layered / coarse-to-fine methods typically only change the downsampling resolution, without fundamentally utilizing the geometric constraints of different types of semantic features (light poles, ground, walls) in the scene.

[0006] In a port scenario, the contributions of three types of features—light poles, ground, and building walls—to 6-DoF are different: light poles mainly constrain XY translation, ground constrains Z translation and pitch / roll angle, and vertical walls constrain Yaw angle and XY translation—existing methods have failed to take advantage of these differences in a targeted manner. Summary of the Invention

[0007] In view of the shortcomings of the prior art, the present invention provides a multi-stage point cloud registration method and system based on semantic feature decoupling, which not only reduces the dimensionality of the optimization problem and greatly reduces the probability of getting trapped in local optima, but also avoids the computational cost of traditional 6-DoF full-space search.

[0008] To achieve the above and other related objectives, the present invention provides the following technical solution: A multi-stage point cloud registration method based on semantic feature decoupling, the method comprising: S1. Semantic feature extraction: Three types of semantic features are extracted from the keyframe point cloud and the global map point cloud to be registered. S2. First registration stage – Yaw angle estimation: Yaw angle deviation is estimated using the vertical plane features of the keyframe to be registered and the global map. S3. Second registration stage – XY translation estimation: Estimating the XY plane translation amount based on the known Yaw deviation; S4. Third registration stage - ICP fine registration: Based on the Yaw and XY estimated in the first two stages, ICP fine registration is performed to correct the Z-axis translation and pitch / roll angles. S5. Multi-source initial pose fusion: When there are multiple initial pose sources, adaptive selection is performed based on the quality of the initial pose.

[0009] Furthermore, in step S1, the extraction of three types of semantic features from the keyframe point cloud and the global map point cloud to be registered includes: S11. Light pole feature extraction: Using an enhanced light pole detection algorithm, light pole clusters are extracted from the point cloud. S12. Ground feature extraction: Ground point cloud is extracted using the constrained plane RANSAC method. S13. Vertical plane feature extraction: After removing lamppost points and ground points, voxel downsampling is performed on the remaining point cloud, and then vertical plane features are extracted by RANSAC plane segmentation + Euclidean clustering.

[0010] Furthermore, the enhanced light pole detection algorithm includes: S111. Separate non-ground points; S112. Select points with a height greater than the minimum height threshold (e.g., 5.0m) as seed points for the top of the light pole; S113. Check the spatial isolation of each seed point through KD tree radius search and eliminate interfering points at the edge of buildings; S114. Using the isolated point as the center, collect coaxial points upwards and downwards to construct candidate light poles; S115. Verify whether the vertical span of the candidate light pole meets the minimum light pole height requirement; S116. Merge and remove duplicate candidate light poles that are too close together in space.

[0011] Furthermore, the key parameters of the constrained plane RANSAC method include setting the plane model as a vertical constrained plane, the constrained axis as the Z-axis, the maximum normal angle tolerance as 10-12 degrees, and the RANSAC distance threshold as 0.2-0.3m.

[0012] Furthermore, in step S2, the estimation of the yaw angle using the vertical plane features of the keyframe to be registered and the global map includes: S21. Project the vertical plane normal vectors of the two frames onto the XY plane to construct a 2D normal vector set; S22. Divide the 2D normal vectors into two orthogonal clusters according to their directions; S23. Select a representative plane in each orthogonal cluster and calculate the angle difference between corresponding clusters in two frames; S24. Perform weighted voting based on the number of support points to output the optimal Yaw angle deviation estimate; S25. When the yaw angle deviation is less than 0.02 radians, treat it as zero to avoid noise amplification.

[0013] Furthermore, in step S3, the estimation of the XY plane translation under the known Yaw deviation includes: S31. Rotate the vertical plane normal vector of the keyframe around the Z-axis by delta. yaw Eliminate angular deviation; S32. Project a three-dimensional vertical plane into a two-dimensional straight line representation; S33. For keyframe 2D lines and ground... Figure 2 Line D is paired with its nearest neighbor, with the following pairing conditions: the angle between the normal vectors is less than the maximum angle tolerance, and the difference in plane offset is less than 5.0m. S34. Pairing similarity metric: 0.7 * angle weight + 0.3 * offset weight, select the pair with the highest similarity; S35. Solve for m pairs of matching planes using least squares; S36. If the absolute value of the translation exceeds 5.0m, it is considered unreliable, and an alternative method should be used for estimation.

[0014] Furthermore, in step S4, the step of performing ICP fine registration to correct the Z-axis translation and pitch / roll angles based on the estimated Yaw and XY from the first two stages includes: S41. Perform ICP within the local ROI area corresponding to the matched light pole point and the ground; S42.ICP parameter settings: maximum corresponding distance 1.5-2.0m, RANSAC outside point rejection distance 0.5-0.6m, enable mutual nearest neighbor constraints; S43. Output a globally uniform 4x4 rigid body transformation matrix.

[0015] Furthermore, in step S5, the adaptive selection based on the quality of the initial pose when multiple initial pose sources exist includes: S51. First, perform a coarse ICP assessment using the initial pose; S52. If fitness is higher than the threshold, it indicates that the initial pose deviation is large, so start the coarse registration pipeline; S53. If fitness is below the threshold, perform ICP fine registration directly.

[0016] Furthermore, the threshold value ranges from 0.01 to 0.03.

[0017] To achieve the above and other related objectives, the present invention also provides a multi-stage point cloud registration system based on semantic feature decoupling, including a computer device programmed or configured to perform the steps of the multi-stage point cloud registration method based on semantic feature decoupling.

[0018] The present invention has the following positive effects: 1. The convergence and robustness of this invention are significantly improved. By decoupling 6-DoF into three independent stages, Yaw, XY, and Z / pitch / roll, each stage is solved only on the corresponding degree of freedom, which reduces the dimensionality of the optimization problem and greatly reduces the probability of getting trapped in local optima.

[0019] 2. The semantic feature contribution of this invention is maximized. The lamp post feature constrains XY translation, the ground feature constrains Z translation and pitch / roll, and the vertical wall surface is constrained in both Yaw and XY. All kinds of semantic features play their role in the most suitable degrees of freedom.

[0020] 3. This invention is highly optimized for specific scenarios such as ports. In port scenarios, light poles are sparse but reliable, the ground is flat, and the building walls are abundant. This invention specifically designed algorithms and optimized parameters for these feature distributions.

[0021] 4. The present invention has high computational efficiency. By using semantic feature filtering and phased solution, it avoids the computational burden of traditional 6-DoF full-space search. At the same time, each stage has a voxel downsampling acceleration strategy. Attached Figure Description

[0022] Figure 1 This is a schematic diagram of the method flow of the present invention; Figure 2 The flowchart shows the three types of semantic feature extraction for the keyframe point cloud and global map point cloud to be registered in this invention. Figure 3 This is a flowchart of the enhanced light pole detection algorithm of the present invention. Detailed Implementation

[0023] The exemplary embodiments of this disclosure are described below with reference to the accompanying drawings, including various details of the embodiments to aid understanding, and should be considered merely exemplary. Therefore, those skilled in the art will recognize that various changes and modifications can be made to the embodiments described herein without departing from the scope and spirit of this disclosure. Similarly, for clarity and brevity, descriptions of well-known functions and structures are omitted in the following description.

[0024] Example: Figure 1 As shown, a multi-stage point cloud registration method based on semantic feature decoupling is proposed, the method comprising: S1. Semantic feature extraction: Three types of semantic features are extracted from the keyframe point cloud and the global map point cloud to be registered. S2. First registration stage – Yaw angle estimation: Yaw angle deviation is estimated using the vertical plane features of the keyframe to be registered and the global map. S3. Second registration stage – XY translation estimation: Estimating the XY plane translation amount based on the known Yaw deviation; S4. Third registration stage - ICP fine registration: Based on the Yaw and XY estimated in the first two stages, ICP fine registration is performed to correct the Z-axis translation and pitch / roll angles. S5. Multi-source initial pose fusion: When there are multiple initial pose sources, adaptive selection is performed based on the quality of the initial pose.

[0025] In this embodiment, as Figure 2 As shown, in step S1, the extraction of three types of semantic features from the keyframe point cloud and the global map point cloud to be registered includes: S11. Light pole feature extraction: Using an enhanced light pole detection algorithm, light pole clusters are extracted from the point cloud. S12. Ground feature extraction: Ground point cloud is extracted using the constrained plane RANSAC method. S13. Vertical plane feature extraction: After removing lamppost points and ground points, voxel downsampling is performed on the remaining point cloud, and then vertical plane features are extracted by RANSAC plane segmentation + Euclidean clustering.

[0026] In this embodiment, as Figure 3 As shown, the enhanced light pole detection algorithm includes: S111. Separate non-ground points; S112. Select points with a height greater than the minimum height threshold (e.g., 5.0m) as seed points for the top of the light pole; S113. Check the spatial isolation of each seed point through KD tree radius search and eliminate interfering points at the edge of buildings; S114. Using the isolated point as the center, collect coaxial points upwards and downwards to construct candidate light poles; S115. Verify whether the vertical span of the candidate light pole meets the minimum light pole height requirement; S116. Merge and remove duplicate candidate light poles that are too close together in space.

[0027] In this embodiment, the key parameters of the constrained plane RANSAC method include setting the plane model as a vertical constrained plane, the constrained axis as the Z-axis, the maximum normal angle tolerance as 10-12 degrees, and the RANSAC distance threshold as 0.2-0.3m.

[0028] In this embodiment, step S2, which involves estimating the yaw angle bias using the vertical plane features of the keyframe to be registered and the global map, includes: S21. Project the vertical plane normal vectors of the two frames onto the XY plane to construct a 2D normal vector set; S22. Divide the 2D normal vectors into two orthogonal clusters according to their directions; S23. Select a representative plane in each orthogonal cluster and calculate the angle difference between corresponding clusters in two frames; S24. Perform weighted voting based on the number of support points to output the optimal Yaw angle deviation estimate; S25. When the yaw angle deviation is less than 0.02 radians, treat it as zero to avoid noise amplification.

[0029] In this embodiment, step S3, which involves estimating the XY plane translation based on the known Yaw deviation, includes: S31. Rotate the vertical plane normal vector of the keyframe around the Z-axis by delta. yaw Eliminate angular deviation; S32. Project a three-dimensional vertical plane into a two-dimensional straight line representation; S33. For keyframe 2D lines and ground... Figure 2 Line D is paired with its nearest neighbor, with the following pairing conditions: the angle between the normal vectors is less than the maximum angle tolerance, and the difference in plane offset is less than 5.0m. S34. Pairing similarity metric: 0.7 * angle weight + 0.3 * offset weight, select the pair with the highest similarity; S35. Solve for m pairs of matching planes using least squares; S36. If the absolute value of the translation exceeds 5.0m, it is considered unreliable, and an alternative method should be used for estimation.

[0030] In this embodiment, step S4, which involves performing ICP fine registration based on the estimated Yaw and XY values ​​from the first two stages to correct the Z-axis translation and pitch / roll angles, includes: S41. Perform ICP within the local ROI area corresponding to the matched light pole point and the ground; S42.ICP parameter settings: maximum corresponding distance 1.5-2.0m, RANSAC outside point rejection distance 0.5-0.6m, enable mutual nearest neighbor constraints; S43. Output a globally uniform 4x4 rigid body transformation matrix.

[0031] In this embodiment, in step S5, the adaptive selection based on the quality of the initial pose when multiple initial pose sources exist includes: S51. First, perform a coarse ICP assessment using the initial pose; S52. If fitness is higher than the threshold, it indicates that the initial pose deviation is large, so start the coarse registration pipeline; S53. If fitness is below the threshold, perform ICP fine registration directly.

[0032] In this embodiment, the threshold value ranges from 0.01 to 0.03.

[0033] In this embodiment, the present invention also provides a multi-stage point cloud registration system based on semantic feature decoupling, including a computer device that is programmed or configured to perform the steps of the multi-stage point cloud registration method based on semantic feature decoupling.

[0034] In this embodiment, the present invention provides a computer-readable storage medium storing a computer program programmed or configured to perform the multi-stage point cloud registration method based on semantic feature decoupling.

[0035] Any references to memory, storage, database, or other media used in the embodiments provided in this application may include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM), etc.

[0036] In summary, this invention not only reduces the dimensionality of the optimization problem and significantly reduces the probability of getting trapped in local optima, but also avoids the computational burden of traditional 6-DoF full-space search.

[0037] The specific embodiments described above do not constitute a limitation on the scope of protection of this disclosure. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this disclosure should be included within the scope of protection of this disclosure.

Claims

1. A multi-stage point cloud registration method based on semantic feature decoupling, characterized in that, The method includes: S1. Semantic feature extraction: Three types of semantic features are extracted from the keyframe point cloud and the global map point cloud to be registered. S2. First registration stage – Yaw angle estimation: Yaw angle deviation is estimated using the vertical plane features of the keyframe to be registered and the global map. S3. Second registration stage – XY translation estimation: Estimating the XY plane translation amount based on the known Yaw deviation; S4. Third registration stage - ICP fine registration: Based on the Yaw and XY estimated in the first two stages, ICP fine registration is performed to correct the Z-axis translation and pitch / roll angles. S5. Multi-source initial pose fusion: When there are multiple initial pose sources, adaptive selection is performed based on the quality of the initial pose.

2. The multi-stage point cloud registration method based on semantic feature decoupling according to claim 1, characterized in that, In step S1, the extraction of three types of semantic features from the keyframe point cloud and the global map point cloud to be registered includes: S11. Light pole feature extraction: Using an enhanced light pole detection algorithm, light pole clusters are extracted from the point cloud. S12. Ground feature extraction: Ground point cloud is extracted using the constrained plane RANSAC method. S13. Vertical plane feature extraction: After removing lamppost points and ground points, voxel downsampling is performed on the remaining point cloud, and then vertical plane features are extracted by RANSAC plane segmentation + Euclidean clustering.

3. The multi-stage point cloud registration method based on semantic feature decoupling according to claim 2, characterized in that, The enhanced light pole detection algorithm includes: S111. Separate non-ground points; S112. Select points with a height greater than the minimum height threshold (e.g., 5.0m) as seed points for the top of the light pole; S113. Check the spatial isolation of each seed point through KD tree radius search and eliminate interfering points at the edge of buildings; S114. Using the isolated point as the center, collect coaxial points upwards and downwards to construct candidate light poles; S115. Verify whether the vertical span of the candidate light pole meets the minimum light pole height requirement; S116. Merge and remove duplicate candidate light poles that are too close together in space.

4. The multi-stage point cloud registration method based on semantic feature decoupling according to claim 2, characterized in that, The key parameters of the constrained plane RANSAC method include setting the plane model as a vertical constrained plane, the constrained axis as the Z-axis, the maximum normal angle tolerance as 10-12 degrees, and the RANSAC distance threshold as 0.2-0.3m.

5. The multi-stage point cloud registration method based on semantic feature decoupling according to claim 1, characterized in that, In step S2, the estimation of the yaw angle using the vertical plane features of the keyframe to be registered and the global map includes: S21. Project the vertical plane normal vectors of the two frames onto the XY plane to construct a 2D normal vector set; S22. Divide the 2D normal vectors into two orthogonal clusters according to their directions; S23. Select a representative plane in each orthogonal cluster and calculate the angle difference between corresponding clusters in two frames; S24. Perform weighted voting based on the number of support points to output the optimal Yaw angle deviation estimate; S25. When the yaw angle deviation is less than 0.02 radians, treat it as zero to avoid noise amplification.

6. The multi-stage point cloud registration method based on semantic feature decoupling according to claim 1, characterized in that, In step S3, estimating the XY plane translation under the known Yaw deviation includes: S31. Rotate the vertical plane normal vector of the keyframe around the Z-axis by delta. yaw Eliminate angular deviation; S32. Project a three-dimensional vertical plane into a two-dimensional straight line representation; S33. Perform nearest neighbor pairing on the keyframe 2D lines and map 2D lines. The pairing conditions are: the angle between the normal vectors < the maximum angle tolerance, and the difference in plane offset < 5.0m. S34. Pairing similarity metric: 0.7 * angle weight + 0.3 * offset weight, select the pair with the highest similarity; S35. Solve for m pairs of matching planes using least squares; S36. If the absolute value of the translation exceeds 5.0m, it is considered unreliable, and an alternative method should be used for estimation.

7. The multi-stage point cloud registration method based on semantic feature decoupling according to claim 1, characterized in that, In step S4, the step of performing ICP fine registration to correct the Z-axis translation and pitch / roll angles based on the estimated Yaw and XY values ​​from the first two stages includes: S41. Perform ICP within the local ROI area corresponding to the matched light pole point and the ground; S42.ICP parameter settings: maximum corresponding distance 1.5-2.0m, RANSAC outside point rejection distance 0.5-0.6m, enable mutual nearest neighbor constraints; S43. Output a globally uniform 4x4 rigid body transformation matrix.

8. The multi-stage point cloud registration method based on semantic feature decoupling according to claim 1, characterized in that, In step S5, the adaptive selection based on the quality of the initial pose when multiple initial pose sources exist includes: S51. First, perform a coarse ICP assessment using the initial pose; S52. If fitness is higher than the threshold, it indicates that the initial pose deviation is large, so start the coarse registration pipeline; S53. If fitness is below the threshold, perform ICP fine registration directly.

9. The multi-stage point cloud registration method based on semantic feature decoupling according to claim 8, characterized in that: The threshold value ranges from 0.01 to 0.

03.

10. A multi-stage point cloud registration system based on semantic feature decoupling, comprising computer equipment, characterized in that, The computer device is programmed or configured to perform the steps of the multi-stage point cloud registration method based on semantic feature decoupling as described in any one of claims 1 to 9.

Images