Point cloud noise reduction method based on three-dimensional distance transformation field

Abstract

The invention provides a point cloud noise reduction method based on a three-dimensional distance transformation field, and belongs to the technical field of mobile robot autonomous environment perception. According to the method, noise points are adaptively removed through dynamic resolution voxel division and a three-dimensional field intensity model, and meanwhile, a key point cloud structure is reserved. According to the method, the noisy points in the three-dimensional laser ranging data can be removed in a relatively low computing resource mode without depending on the type of a three-dimensional laser sensor and the spatial distribution characteristics and structural characteristics of the point cloud, meanwhile, the detail characteristics of the point cloud can be stored, the data accuracy is effectively improved, and the accuracy of the three-dimensional laser ranging data is improved. And the requirements of real-time positioning, scene reconstruction, subsequent map construction, scene understanding, path planning and other tasks of the mobile robot in an unknown dynamic environment on accurate three-dimensional laser data are met.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous environmental perception of mobile robots, and particularly relates to a method for reducing noise of point clouds based on a three-dimensional distance transformation field. The method uses a mobile robot as a moving carrier to perform dynamic compensation on the three-dimensional laser ranging data obtained by the mobile robot during movement, and is mainly used for real-time positioning and scene reconstruction of mobile robots in unknown dynamic environments. Background Art

[0002] Laser sensors are one of the most important sensing devices in the field of mobile robots, with advantages such as high measurement accuracy and little environmental interference. However, with the continuous development of mobile robot technology, the measurement methods based on single points or two-dimensional planes can no longer meet the application requirements of mobile robots. How to obtain more accurate three-dimensional laser ranging data with fewer noise points has become a research hotspot in this field.

[0003] In the field of mobile robot technology, three-dimensional laser ranging sensors (such as LiDAR) are widely used to obtain three-dimensional information of the surrounding environment. However, in practical applications, due to the limitations of the sensor itself, external environmental factors (such as changes in reflectivity, weather conditions, etc.) and the influence of the robot's own movement, the collected point cloud data often contains different degrees of noise. If these noises are not processed, they will directly affect the environmental modeling accuracy, navigation performance and task execution effect of the robot.

[0004] At present, there are mainly three methods for 3D point cloud filtering. The first is the denoising method based on deep learning. The deep learning-based method can learn the change patterns of different textures, edges, etc. under noise interference in complex point cloud scenarios, and better distinguish noise from real points. The literature (Qin, P., Zhang, C., & Dang, M. (2022). GVnet: Gaussian model with voxel-based 3D detection network for autonomous driving. Neural Computation & Applications, 34, 6637–6645.) applied the deep learning method to point cloud denoising and proposed the Gaussian model voxel network GVnet. It designed a voxel-based 3D detection network GVnet, which combines the Gaussian model with voxel representation for 3D object detection tasks in autonomous driving, and can effectively process 3D point cloud data and extract features for object detection. It can utilize low-level detailed features and high-level semantic features, further improving the performance of object detection, especially having better adaptability to objects of different sizes and shapes, and obtaining better point cloud information. The parameter estimation of the Gaussian model may be affected by data distribution and noise, and it may be difficult to accurately estimate the optimal parameters in some cases, thus affecting the performance and stability of the model.

[0005] The second denoising method for 3D point cloud is the denoising method based on point cloud density. Density-based denoising algorithms include statistical filtering, radius filtering, etc. This denoising strategy can remove most of the noise points, but it is difficult to remove those noise points with point cloud density similar to the target. The literature (Zhou, S. T., Liu, X. L., Wang, C. Y., & et al. (2020). Non-iterative denoising algorithm based on a dual threshold for a 3D point cloud. Optics and Lasers in Engineering, 126, 105921.) proposed a non-iterative dual-threshold denoising method based on Canny's work in image edge detection, which divides 3D point cloud denoising into two stages: a large threshold and a small threshold. In each stage, the concept of divide and conquer is used to hierarchically process the 3D point cloud. This method is more stable and has a faster response for 3D point cloud models under high noise levels compared to the improved K-means clustering denoising method based on the K-d tree. However, the removal error of the target point cloud is relatively large.

[0006] The third point cloud denoising method is based on smoothing processing to achieve point cloud denoising. This method is represented by the bilateral filtering algorithm. By changing the positions of noise points, it achieves a visually smooth effect of the point cloud model. However, this may also lead to changes in the volume of the point cloud model and overly smooth surfaces, resulting in poor preservation of the detailed features on the point cloud surface. The literature (Zhou, Y.Y., Chen, R., Zhao, Y.Q., & et al. (2021). Point cloud denoising using non-local collaborative projections. Pattern Recognition, 120, 108128.) designed a structure-aware descriptor called the projection height vector. This descriptor captures local height changes through normal height projection and groups the most similar non-local projection height vectors into a height matrix, enhancing the structural representation. This non-local feature extraction method can better utilize the global geometric information of the point cloud and overcome the limitation of traditional methods that only focus on local geometric information. However, due to complex operations such as the calculation of adaptive curvature thresholds, the search for non-local similarities, and the optimization of the height matrix, the computational complexity of this method may be relatively high.

[0007] For mobile robots, obtaining accurate and detailed environmental information is a prerequisite for subsequent tasks such as map construction, scene understanding, and path planning. The accuracy of data is often a key factor determining the success or failure of tasks. For some three-dimensional laser data acquisition devices with a large number of noise points, it is difficult to meet the various application requirements of mobile robots. Based on the above situation, the present invention proposes a point cloud noise reduction method based on a three-dimensional distance transformation map, which can obtain accurate three-dimensional laser data in a relatively low-computing-resource manner without relying on the type of three-dimensional laser sensor, the spatial distribution characteristics, and the structural characteristics of the point cloud. Summary of the Invention

[0008] The objective of the present invention is to propose a point cloud noise reduction method based on a three-dimensional distance transformation field, which adaptively removes noise points through dynamic resolution voxel division and a three-dimensional field strength model while retaining the key point cloud structure.

[0009] The technical solution of the present invention is as follows:

[0010] A point cloud noise reduction method based on a three-dimensional distance transformation field, comprising the following steps:

[0011] S1. Divide the point cloud into regions according to its boundary to obtain a number of voxel units;

[0012] S2. Further divide each voxel unit into dynamic resolution voxels based on the relative density of the point cloud in each voxel unit;

[0013] S3. For any dynamic resolution voxel within each voxel unit, select its neighboring dynamic resolution voxels as the template region, and calculate the field strength value of the dynamic resolution voxel at the center within the template region using a three-dimensional distance field strength model;

[0014] S4. For all the dynamic resolution voxels generated for each voxel unit, assign intensity values to the dynamic resolution voxels according to the three-dimensional distance field strength model described in S3; and filter the laser points within the dynamic resolution voxels based on the intensity values;

[0015] S5. Further screen the points in the dynamic resolution voxels that are retained after filtering to obtain continuous and smooth laser points.

[0016] Furthermore, the specific process of step S1 is as follows:

[0017] S1.1. Take the coordinate center of the three-dimensional laser ranging sensor as the coordinate origin, determine the positive x-axis direction in front of the sensor, and determine the y-axis and z-axis directions according to the right-hand system, thereby establishing a three-dimensional coordinate system;

[0018] S1.2. Initialize the mobile robot, and use the three-dimensional laser ranging sensor to perform full-range environmental perception to collect three-dimensional laser ranging data reflecting the current environmental geometric characteristics;

[0019] S1.3. Analyze the coordinate values of the three-dimensional laser ranging data in the three-dimensional coordinate system, determine its maximum boundary value and minimum boundary value, and calculate the side lengths of the minimum bounding box of the point cloud in the directions of each coordinate axis based on the boundary values:

[0020]

[0021] where, x max , y max , z max are the maximum boundary values of the point cloud, x min , y min , z min are the minimum boundary values of the point cloud; l x , l y and l z are the side lengths of the minimum bounding box of the point cloud in the x, y, and z coordinate axis directions respectively;

[0022] S1.4. Evenly divide the minimum bounding box of the point cloud into M, N, and L parts along the x, y, and z coordinate axes respectively, then the minimum bounding box of the point cloud is divided into M * N * L voxel units:

[0023] SUM = M * N * L (2)

[0024]

[0025] Wherein, SUM is the total number of voxel units, and cell is the side length of the voxel unit;

[0026] S1.5. Use an unordered map container voxel_map to store the number of each voxel unit and the set of laser points corresponding to it, so as to efficiently manage and process these voxel units and their corresponding laser point data; for each voxel unit number (A, B, C), the formula is as follows:

[0027]

[0028] Wherein, x i , y i , z i represent the coordinate values of the i-th laser point data;

[0029] During the storage process, for each laser point, quickly determine the voxel unit it belongs to through its coordinate value, and add it to the corresponding set; quickly query and process each voxel unit and the laser point data it contains through voxel_map.

[0030] Furthermore, in step S2, the specific process of further subdividing each voxel unit into voxel units with dynamic resolution is as follows:

[0031] S2.1. Traverse the set of laser points stored in each voxel unit in voxel_map, calculate the number of laser points contained in each voxel unit, so as to obtain a set of the number of laser points in the voxel unit, denoted as NUM = (N 1 , N 2 , N 3 , L, N j , L, N SUM ), where N j represents the number of laser points in the j-th voxel unit;

[0032] S2.2. Since there may be large differences in the distribution of the number of laser points in different voxel units, in order to achieve more accurate subsequent processing, a mapping function is introduced to map the number of laser points in each voxel unit to a specific range. Specifically, first map min(NUM) and max(NUM) to 0 and 1 respectively. In each of the remaining voxel units, according to the number of laser points, linearly project it into the range (0, 1), and calculate the mapping value corresponding to each voxel unit:

[0033]

[0034] Wherein, α j is the projection value of the j-th voxel unit in the range (0, 1), which can reflect the relative density of each voxel unit;

[0035] S2.3. Further subdivide each voxel unit obtained in S1 based on the relative density of the point cloud of each voxel unit reflected by the projection value to generate smaller dynamic resolution voxels; at the same time, use an octree structure to manage the dynamic voxels to accelerate neighborhood queries; the side length of the dynamic resolution voxel is:

[0036]

[0037] wherein, is the side length of the dynamic resolution voxel generated for the j-th voxel unit, cell is the side length of the voxel unit, and cell min is the side length of the set minimum dynamic resolution voxel.

[0038] Furthermore, the specific method of step S3 is as follows:

[0039] S3.1. First, obtain the information of all laser points in the voxel unit through the voxel_map container, and then determine the dynamic resolution voxel index (a, b, c) where any laser point p is located through the side length of the dynamic resolution voxel, so as to obtain the dynamic resolution voxels with spatial adjacency relationship and the laser point information they contain;

[0040]

[0041] wherein, (x, y, z) are the three-dimensional coordinates of the laser point p;

[0042] In the dynamic resolution voxels generated in a voxel unit, randomly select a dynamic resolution voxel; on this basis, respectively select the dynamic resolution voxel containing the laser point and its two adjacent dynamic resolution voxels along the three coordinate axes (x-axis, y-axis, and z-axis) to form a three-dimensional space region composed of 3×3×3 = 27 dynamic resolution voxels, which is defined as the template region;

[0043] S3.2. Under the three-dimensional distance transformation field model, the more laser points there are in the 26 adjacent dynamic resolution voxels in the template region, and the closer the laser points are to the central dynamic resolution voxel, the relatively higher the intensity value obtained by the central dynamic resolution voxel. Through the three-dimensional distance field intensity model, the intensity value of the dynamic resolution voxel located in the center of the template region can be calculated. For a dynamic resolution voxel and its 26 adjacent dynamic resolution voxels, the formula for calculating the intensity I of the dynamic resolution voxel located in the center of the template region is:

[0044]

[0045] wherein, c dis the central coordinate of the dynamic resolution voxel located at the center within the template region; σ is the variance of the three-dimensional distance field intensity model, set to Neighbors is the set of all laser points of the 26 dynamic resolution voxels of the neighbors;

[0046] The central coordinate c of the dynamic resolution voxel located at the center within the template region d The calculation formula is:

[0047]

[0048] where p m is the coordinate of the m-th laser point in this dynamic resolution voxel, m = 1, 2,..., total.

[0049] Furthermore, the specific process of step S4 is as follows:

[0050] S4.1. Calculate the field intensities {I s , I 1 ,..., I 2} of all dynamic resolution voxels in the voxel unit V S through the three-dimensional distance field intensity model in S3; calculate the average value I s of the field intensity values of all dynamic resolution voxels in the voxel unit V avg and the variance for subsequent noise point filtering;

[0051]

[0052] where I d is the field intensity value of the d-th dynamic resolution voxel in the voxel unit V s ;

[0053] S4.2. Set the dynamic threshold T based on the average value I avg of the field intensity of the dynamic resolution voxel and the variance :

[0054] T = I avg - β·σ I (12)

[0055] where I avg is the average value of the field intensities of all dynamic resolution voxels in this voxel unit, σ I is the standard deviation, and β is an empirical coefficient, usually taking 1.5 - 3.0, used to adjust the filtering intensity;

[0056] S4.3. For the d-th dynamic resolution voxel in the voxel unit V s , if I d<T indicates that the point cloud density around it is low, so the laser points in the d-th dynamic resolution voxel are filtered out.

[0057] Furthermore, the specific process of step S5 is as follows:

[0058] S5.1. After the preliminary filtering of the dynamic resolution voxels, although the dynamic resolution voxels mainly composed of isolated points can be removed, there may still be some isolated points remaining, so further screening is required. For the laser points of these remaining dynamic resolution voxels, first use the least squares method for plane fitting. Assume that the laser point set of the remaining dynamic resolution voxels is X = {x 1 ,..., x h}, where the coordinates of each point are taken as three-dimensional Euclidean coordinates Find the plane parameters l n and l d , such that

[0059]

[0060] where l n is the normal vector of the fitting plane, and l d is the intercept of the fitting plane;

[0061] In order to obtain the most ideal fitting plane, the error error needs to be reduced to the lowest level; the calculation formula of error is:

[0062]

[0063] To simplify the problem, first represent the coordinates in homogeneous coordinate form At the same time, represent the plane parameters as Next, an optimal plane parameter needs to be found such that the sum of the squares of the distances from all laser points to this plane is minimized; the goal is to minimize the error Finally, use singular value decomposition to solve the final plane parameters

[0064] S5.2. Calculate the distance D from each point in the point set within the dynamic resolution voxel to the best fitting plane k and the mean μ of all D k within this dynamic resolution voxel:

[0065]

[0066] Regard the points with D k >β·μ as noise points and remove them; where β is the mean multiple threshold, set to 3.

[0067] Advantages of the present invention: Without relying on the type of 3D laser sensor, the spatial distribution characteristics, and the structural characteristics of the point cloud, the method of the present invention can remove noise points in 3D laser ranging data in a relatively low-computation-resource manner. At the same time, it can preserve the detailed features of the point cloud, effectively improve data accuracy, and meet the requirements of mobile robots for accurate 3D laser data in tasks such as real-time positioning, scene reconstruction, and subsequent map building, scene understanding, and path planning in unknown dynamic environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 It is a schematic diagram of the dynamic resolution voxel of the point cloud; among them, (a) is the result of regional division. According to the different point cloud densities in the voxel units, those with higher densities are divided into smaller dynamic resolution voxels as shown in (b), and those with higher densities are divided into larger dynamic resolution voxels as shown in (c);

[0069] Figure 2 It is a schematic diagram of the field strength of a single point in the point cloud, and the darker the color, the greater the intensity.

[0070] Figure 3 It is the original point cloud containing noise points collected by the lidar sensor.

[0071] Figure 4 It is the point cloud after removing noise points by the method of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0072] The following further describes the specific embodiments of the present invention in conjunction with the drawings and technical solutions.

[0073] In this experiment, the laser ranging data collected by a mobile robot during movement is used for point cloud denoising. The effectiveness of the method of the present invention is verified by comparing the noisy 3D point cloud collected only in the actual scene with the point cloud reconstructed by using the denoising method proposed in the present invention. The ST-240 solid-state Flash lidar is used to scan the indoor scene. Due to the working characteristics of the Flash solid-state lidar, a certain number of noise points will appear during normal use. A point cloud denoising method based on the 3D distance transformation field proposed in the present invention is adopted, and the specific steps are as follows:

[0074] S1. Divide the point cloud into regions according to its boundary to obtain a number of voxel units. The specific process is as follows:

[0075] S1.1. First, establish a point cloud coordinate system. Take the coordinate center of the 3D laser ranging sensor as the coordinate origin, determine the positive x-axis direction in front of the sensor, and determine the y-axis and z-axis directions according to the right-hand system to establish a 3D coordinate system;

[0076] S1.2. Initialize the mobile robot, and use the 3D laser range finder to perform full-range environmental perception to collect 3D laser range data reflecting the geometric characteristics of the current environment;

[0077] S1.3. Analyze the coordinate values of the 3D laser range data in the 3D coordinate system, traverse all points, record the maximum and minimum values of each axis, and calculate the side lengths of the minimum bounding box of the point cloud in the directions of each coordinate axis based on the boundary values:

[0078]

[0079] Among them, x max 、y max 、z max are the maximum boundary values of the point cloud, and x min 、y min 、z min are the minimum boundary values of the point cloud; l x ,l y and l z are the side lengths of the minimum bounding box of the point cloud in the x, y, and z coordinate axes respectively;

[0080] S1.4. Evenly divide the minimum bounding box of the point cloud into M, N, and L parts along the x, y, and z coordinate axes respectively, then the minimum bounding box of the point cloud is divided into M*N*L voxel units; by setting the number of voxel units, it is possible to control the voxel units to approximate cube voxels. Compared with dividing into cuboid voxels, its advantage is that it can provide a more uniform space division. For point cloud data, it can avoid the distortion of local data and make the subsequent dynamic resolution voxel division easier to process.

[0081] To overcome the information loss problem of the traditional block method, this embodiment uses the local height for blocking:

[0082] SUM = M*N*L (17)

[0083]

[0084] Among them, SUM is the total number of voxel units, cell is the side length of the voxel unit, and the initial cell is set to 1.5 times the average unit density of the point cloud;

[0085] S1.5. Use an unordered map container voxel_map to store the number of each voxel unit and the set of laser points corresponding to it, so as to efficiently manage and process these voxel units and their corresponding laser point data; for each voxel unit number (A, B, C), the formula is as follows:

[0086]

[0087] Among them, x i, y i , z i represent the coordinate values of the i-th laser point data;

[0088] During the storage process, for each laser point, quickly determine the voxel unit it belongs to through its coordinate values, and add it to the corresponding set; quickly query and process each voxel unit and the laser point data it contains through voxel_map.

[0089] S2. Further generate dynamic resolution voxels for each voxel unit based on the relative density of the point cloud data. In voxel division, if simply divided into voxels of regular size, although the processing process is relatively simple, there may be information loss, especially when facing point clouds containing complex geometric structures; therefore, when determining the voxel block size, a trade-off needs to be made between computational efficiency and preserving local information. Based on the density difference of the point cloud data, ensure that each voxel unit can be processed at an appropriate resolution to better capture the detailed features of the point cloud, and further subdivide the voxel units divided in S1; the specific process is as follows:

[0090] S2.1. Traverse the set of laser points stored in each voxel unit in voxel_map, calculate the number of laser points contained in each voxel unit, so as to obtain a set of the number of laser points in the voxel units, denoted as NUM = (N 1 , N 2 , N 3 , … N j , …, N SUM ), where N j represents the number of laser points in the j-th voxel unit;

[0091] S2.2. Since the distribution of the number of laser points in different voxel units may vary greatly, in order to achieve more accurate subsequent processing, introduce a mapping function to map the number of laser points in each voxel unit to a specific range. Specifically, first map min(NUM) and max(NUM) to 0 and 1 respectively, and in each of the remaining voxel units, according to the number of laser points, linearly project it into the range (0, 1), and calculate the mapping value corresponding to each voxel unit:

[0092]

[0093] where, α j is the projection value of the j-th voxel unit in the range (0, 1), which can reflect the relative density of each voxel unit;

[0094] S2.3. Based on the relative density of each voxel unit reflected by the projection value, for each voxel unit obtained in S1, according to its side length Perform secondary partitioning to generate smaller voxels with dynamic resolution; at the same time, use an octree structure to manage the dynamic voxels to accelerate neighborhood queries. The side length of the dynamic resolution voxel is:

[0095]

[0096] Where is the side length of the dynamically resolved voxel generated for the j-th voxel unit, cell is the side length of the voxel unit, and cell min is the side length of the smallest dynamically resolved voxel set.

[0097] S3. For each dynamically resolved voxel within each voxel unit, select its neighboring dynamically resolved voxels as the template region, and calculate the field strength parameters of the dynamically resolved voxel within this template region using a three-dimensional distance field strength model; the specific process is as follows:

[0098] S3.1. First, obtain the information of all laser points in the voxel unit through the voxel_map container, and then determine the index (a, b, c) of the dynamically resolved voxel where any laser point p is located through the side length of the dynamically resolved voxel, so as to obtain the dynamically resolved voxels with spatially adjacent relationships and the laser point information they contain;

[0099]

[0100] where (x, y, z) are the three-dimensional coordinates of the laser point p;

[0101] Among the dynamically resolved voxels generated in a voxel unit, randomly select a dynamically resolved voxel; on this basis, select the dynamically resolved voxel containing the laser point and its two adjacent dynamically resolved voxels along the three axis directions (x-axis, y-axis, and z-axis) respectively, to form a three-dimensional space region composed of 3×3×3 = 27 dynamically resolved voxels, which is defined as the template region;

[0102] S3.2. Under the three-dimensional distance transformation field model, the more laser points there are in the 26 adjacent dynamically resolved voxels in the template region, and the closer the laser points are to the central dynamically resolved voxel, the higher the intensity value obtained by the central dynamically resolved voxel. Through the three-dimensional distance field strength model, the field strength value of the dynamically resolved voxel located at the center of the template region can be calculated. For a dynamically resolved voxel and its 26 neighboring dynamically resolved voxels, the formula for calculating the field strength I of the dynamically resolved voxel located at the center of the template region is:

[0103]

[0104] where c dis the central coordinate of the dynamic resolution voxel located at the center within the template area; σ is the variance of the three-dimensional distance field strength model, set to Neighbors is the set of all laser points of the 26 dynamic resolution voxels of the neighbors;

[0105] The central coordinate c of the dynamic resolution voxel located at the center within the template area d The calculation formula is:

[0106]

[0107] where p m is the coordinate of the m-th laser point in this dynamic resolution voxel, m = 1, 2,..., total.

[0108] S4. For all the dynamic resolution voxels generated by each voxel unit, assign intensity values to the dynamic resolution voxels according to the three-dimensional distance field strength model described in S3; count the point cloud distribution of all the dynamic resolution voxels within the same voxel unit, and filter the laser points within the dynamic resolution voxels based on the statistical results and intensity values; the specific process is as follows:

[0109] S4.1. Calculate the field strengths {I s , I 1 , …, I 2 , …, I S} of all the dynamic resolution voxels in the voxel unit V s through the three-dimensional distance field strength model in S3.2; calculate the average value I avg and variance of the field strength values of all the dynamic resolution voxels in the voxel unit V for subsequent noise point filtering;

[0110]

[0111] where I d is the field strength value of the d-th dynamic resolution voxel in the voxel unit V s ;

[0112] S4.2. Set the dynamic threshold T based on the average field strength I avg of the dynamic resolution voxels and the variance :

[0113] T = I avg - β · σ I (27)

[0114] where I avg is the average field strength of all the dynamic resolution voxels in this voxel unit, σ I is the standard deviation, and β is an empirical coefficient, set to 2.5;

[0115] S4.3. For the d-th dynamic resolution voxel in the voxel unit V s if I d < T, it indicates that the point cloud density around it is low, and then the laser points in the d-th dynamic resolution voxel are filtered out.

[0116] S5. After completing the noise filtering, the points in the remaining voxels are further processed to improve the quality of the point cloud, making it smoother, more continuous, and reducing the boundary mutation problem caused by filtering. The specific process is as follows:

[0117] S5.1. After the preliminary filtering of the dynamic resolution voxels, although the dynamic resolution voxels mainly composed of isolated points can be removed, some isolated points may still remain, so further screening is needed. For the laser points of these remaining dynamic resolution voxels, the least squares method is first used for plane fitting. Assume that the laser point set of the remaining dynamic resolution voxels is X = {x 1 ,..., x h}, where the coordinates of each point are taken as three-dimensional Euclidean coordinates Find the plane parameters l n and l d such that

[0118]

[0119] where l n is the normal vector of the fitting plane, and l d is the intercept of the fitting plane;

[0120] To obtain the most ideal fitting plane, the error error needs to be minimized; the calculation formula of error is:

[0121]

[0122] To simplify the problem, the coordinates are first represented in homogeneous coordinate form At the same time, the plane parameters are represented as Next, an optimal plane parameter needs to be found such that the sum of the squares of the distances from all laser points to this plane is minimized. The goal is to minimize the error Finally, the final plane parameters are solved using singular value decomposition

[0123] S5.2. Calculate the distance D of each point in the point set within the dynamic resolution voxel to the best fitting plane k and the mean value μ of all D k within this dynamic resolution voxel:

[0124]

[0125] Regard the points where D k > β·μ as noise points and remove them; where β is the mean multiple threshold, set to 3.

[0126] In a specific embodiment, first perform dynamic resolution sampling on point cloud scenes with different densities, such as Figure 1 shown. In areas with denser point clouds, smaller three-dimensional voxels will be used for subdivision. After that, as Figure 2 shown, create a point cloud neighborhood template. Each laser point will give certain intensity value information to the surrounding voxels. The intensity value of the voxels closer to the laser point in the distance distribution will be higher. After algorithm processing, the three-dimensional point cloud scenes before and after noise filtering are respectively as Figure 3 and Figure 4 shown, and the three-dimensional laser point cloud can represent the real scene more accurately.

[0127] The above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that the technical solutions of the present invention can be modified or equivalently replaced without departing from the spirit and scope of the technical solutions of the present invention.

Claims

1. A point cloud denoising method based on a three-dimensional distance transform field, characterized in that: The following steps are involved: S1, dividing the point cloud into regions according to its boundaries to obtain a number of voxel units; S2, further subdividing each voxel unit into dynamic resolution voxels based on the relative density of the point cloud in each voxel unit; S3. For any dynamic resolution voxel in each voxel unit, select its neighboring dynamic resolution voxels as the template area, and use the three-dimensional distance field strength model to calculate the field strength value of the dynamic resolution voxel located in the center within the template area; S4. For all dynamic resolution voxels generated by each voxel unit, assign intensity values ​​to the dynamic resolution voxels according to the three-dimensional distance field strength model described in S3; and filtering the laser points within each dynamic resolution voxel based on the intensity value; S5. Further screen the points in the dynamic resolution voxels retained by filtering to obtain continuous and smooth laser points.

2. The point cloud denoising method based on three-dimensional distance transform field according to claim 1, characterized in that: The specific process of step S1 is as follows: S1.

1. Take the coordinate center of the three-dimensional laser ranging sensor as the coordinate origin, determine the positive direction of the x-axis in front of the sensor, determine the directions of the y-axis and z-axis according to the right-hand system, and then establish a three-dimensional coordinate system; S1.2, initialize the mobile robot, use the three-dimensional laser ranging sensor to perform full-range environmental perception, and collect three-dimensional laser ranging data reflecting the geometric characteristics of the current environment; S1.3, determining the maximum boundary value and the minimum boundary value of the three-dimensional laser ranging data in the three-dimensional coordinate system, and calculating the side length of the minimum bounding box of the point cloud in the direction of each coordinate axis according to the boundary values; S1.

4. Divide the minimum bounding box of the point cloud into M, N, and L parts along the x, y, and z coordinate axes respectively. Then the minimum bounding box of the point cloud is divided into M*N*L voxel units: SUM=M*N*L (1) Among them, SUM is the total number of voxel units, cell is the side length of the voxel unit; l x , l y and l z The lengths of the minimum bounding box of the point cloud in the x, y, and z coordinate axes respectively; S1.

5. Use an unordered map container voxel_map to store the number of each voxel unit and the set of laser points it corresponds to. During the storage process, for each laser point, quickly determine the voxel unit to which it belongs through its coordinate value and add it to the corresponding set. Use voxel_map to quickly query and process each voxel unit and the laser point data it contains.

3. The point cloud denoising method based on three-dimensional distance transform field according to claim 2, characterized in that: The step S1.3 calculates the side length of the minimum bounding box of the point cloud in each coordinate axis direction according to the boundary value, and the formula is as follows: Among them, x max ,y max 、z max is the maximum boundary value of the point cloud, x min ,y min 、z min is the minimum boundary value of the point cloud; l x , l y and l z The lengths of the minimum bounding box of the point cloud in the x, y, and z coordinate axes.

4. A point cloud denoising method based on three-dimensional distance transform field according to claim 2 or 3, characterized in that: In step S1.5, for each voxel unit number (A, B, C), the formula is as follows: Among them, x i ,y i 、z i Represents the coordinate value of the i-th laser point data.

5. The point cloud denoising method based on three-dimensional distance transform field according to claim 4, characterized in that: In step S2, the specific process of further subdividing each voxel unit into dynamic resolution voxels is as follows: S2.1, traverse the laser point set stored in each voxel unit in voxel_map, calculate the number of laser points contained in each voxel unit, and obtain the set of the number of laser points in the voxel unit, which is recorded as NUM = (N1, N2, N3, L, N j ,L,N SUM ), where N j represents the number of laser points in the jth voxel unit; S2.

2. Map min(NUM) and max(NUM) to 0 and 1 respectively. In each remaining voxel unit, according to the number of laser points, linearly project them to the range of (0,1) and calculate the mapping value corresponding to each voxel unit: Among them, α j is the projection value of the j-th voxel unit in the range of (0,1), which can reflect the relative density of each voxel unit; S2.3, further subdivide each voxel unit obtained in S1 to generate smaller dynamic resolution voxels; at the same time, use an octree structure to manage dynamic voxels to accelerate neighborhood queries; the side length of the dynamic resolution voxel is: in, The side length of the dynamic resolution voxel generated for the jth voxel unit, cell is the side length of the voxel unit, cell min The minimum dynamic resolution voxel side length that is set.

6. The point cloud denoising method based on three-dimensional distance transform field according to claim 5, characterized in that: The specific method of step S3 is as follows: S3.

1. First, the voxel_map container is used to obtain the information of all laser points in the voxel unit, and then the side length of the voxel is dynamically resolved. Determine the dynamic resolution voxel index (a, b, c) of any laser point p, and then obtain the dynamic resolution voxels with close spatial relationship and the laser point information contained therein; Where (x, y, z) is the three-dimensional coordinate of the laser point p; Among the dynamic resolution voxels generated in a voxel unit, one dynamic resolution voxel is randomly selected; on this basis, the dynamic resolution voxel containing the laser point and its two adjacent dynamic resolution voxels are respectively selected along the three coordinate axes to form a three-dimensional space area composed of 3×3×3=27 dynamic resolution voxels, which is defined as the template area; S3.

2. For a dynamic resolution voxel and its 26 neighboring dynamic resolution voxels, the field strength value of the dynamic resolution voxel located in the center of the template area is calculated using the three-dimensional distance field strength model. The formula is: Among them, c d is the central coordinate of the dynamic resolution voxel located in the center of the template area; σ is the variance of the three-dimensional distance field intensity model; Neighbors is the set of all laser points of the 26 neighboring dynamic resolution voxels.

7. The point cloud denoising method based on three-dimensional distance transform field according to claim 6, characterized in that: The center coordinate c of the dynamic resolution voxel located in the center of the template area d The calculation formula is: Among them, p m is the coordinate of the mth laser point in the dynamic resolution voxel, m = 1, 2, ..., total.

8. The point cloud denoising method based on three-dimensional distance transform field according to claim 6 or 7, characterized in that: The specific process of step S4 is as follows: S4.

1. Calculate the voxel unit V through the three-dimensional distance field strength model in S3 s The field strength of all dynamic resolution voxels in {I1,I2,…,I S }; Calculate the voxel unit V s The average value of the field strength of all dynamic resolution voxels within I avg and variance S4.

2. Field strength average value based on dynamic resolution voxels I avg and variance Set the dynamic threshold T: T=I avg -b·s I (10) Among them, β is an empirical coefficient, ranging from 1.5 to 3.0, which is used to adjust the filtering strength; S4.

3. For voxel unit V s The d-th dynamic resolution voxel in d <T, the laser point in the dth dynamic resolution voxel is filtered out.

9. The point cloud denoising method based on three-dimensional distance transform field according to claim 8, characterized in that: In step S4.1, the voxel unit V s The average value of the field strength of all dynamic resolution voxels within I avg and variance The calculation formula is: Among them, I d V is the voxel unit V s The field strength value of the dth dynamic resolution voxel in .

10. The point cloud denoising method based on three-dimensional distance transform field according to claim 1, characterized in that: The specific process of step S5 is as follows: S5.

1. For the laser points of the retained dynamic resolution voxels, the least square method is first used to perform plane fitting. The set of laser points of the retained dynamic resolution voxels is X = {x1,…,x h }, where the coordinates of each point are three-dimensional Euclidean coordinates Find the plane parameter l n and l d , so that Among them, l n is the normal vector of the fitted plane, l d is the intercept of the fitting plane; In order to obtain the most ideal fitting plane, the error needs to be reduced to the minimum; the error calculation formula is: To simplify the problem, first express the coordinates in their secondary coordinate form At the same time, the plane parameters are expressed as Next, we need to find an optimal plane parameter Minimize the sum of the squares of the distances from all laser points to the plane; the goal is to minimize the error Finally, the final plane parameters are solved using singular value decomposition S5.

2. Calculate the distance D from each point in the point set of the dynamic resolution voxel to the best fit plane k And all D k The mean μ of : D k Points with >β·μ are considered as noise points and removed; β is the mean multiple threshold.

Images