Curvature weighted 3D-SIFT and adaptive radius FPFH-based point cloud registration method and system

Abstract

The invention provides a curvature weighted 3D-SIFT and adaptive radius FPFH-based point cloud registration method and system. The method comprises the steps of obtaining point cloud data and performing preprocessing; carrying out point cloud key point extraction based on curvature weighted 3D-SIFT to obtain corresponding key points; constructing a point cloud feature descriptor, and calculating the feature descriptor by using an improved FPFH fast point feature histogram algorithm; an attitude estimation scheme in an IGSP algorithm is adopted, firstly, an energy function is constructed, then the energy function is optimized through a KM algorithm to determine global optimal correspondence, and transformation is calculated on the basis; and finally, continuously repeating the process of optimal corresponding matching and transformation calculation through an iteration strategy to obtain a globally optimal solution. The method is combined with density characteristics, the description capability of complex scenes (such as vegetation and irregular terrains) is remarkably enhanced, and mismatching is reduced.

Description

Technical Field

[0001] The present invention relates to the field of computer vision technology. Specifically, it relates to a point cloud registration method and system based on curvature-weighted 3D-SIFT and adaptive-radius FPFH. Background Art

[0002] With the rapid development of computer vision and sensor technology, point cloud registration has become a key technology in the field of three-dimensional vision. Point cloud registration refers to the technical process of aligning point cloud data obtained from different perspectives, different times, or different sensors to the same coordinate system through spatial transformation (such as rotation and translation). Its essence is to eliminate the relative position deviation between point clouds and achieve the precise fusion of multi-source data. Currently, there are three widely used methods in traditional point cloud registration, namely Iterative Closest Point (ICP), Normal Distribution Transform (NDT), and Four-Point Congruent Set (4PCS). However, traditional point cloud registration algorithms have problems such as low registration accuracy, being prone to falling into local optimal solutions, and being sensitive to the initial pose.

[0003] Artyom Makovetskii et al. proposed a new algorithm for orthogonal registration of point clouds using regularized point-to-point and point-to-point ICP algorithms. This algorithm uses the famous Horn algorithm, combines point coordinates with normal vectors, and improves the convergence speed of the ICP algorithm. However, the selection of the regularization coefficient is sensitive, and the quality of the registration effect depends on the choice of the coefficient. Jing Xiang et al. proposed a point cloud registration method that combines the optimized Intrinsic Shape Signature (ISS) algorithm with the improved ICP algorithm. Key points are extracted by optimizing the search radius of the ISS algorithm, described by the Fast Point Feature Histogram (FPFH), and corresponding relationships are established based on the features, which helps to reduce the risk of the ICP algorithm being prone to falling into local optimality during the iteration process. However, the computational complexity is relatively high, the parameter tuning work is cumbersome, and there are still problems such as a large number of iterations when the overlap is low.

[0004] In summary, although some achievements have been made for traditional point cloud registration algorithms, how to efficiently complete point cloud registration while ensuring registration accuracy is still a problem with important research value. Summary of the Invention

[0005] Aiming at the defects in the prior art, the purpose of the present invention is to provide a point cloud registration method and system based on curvature-weighted 3D-SIFT and adaptive-radius FPFH.

[0006] According to a point cloud registration method based on curvature-weighted 3D-SIFT and adaptive-radius FPFH provided by the present invention, it includes:

[0007] Step S1: Obtain point cloud data and perform preprocessing;

[0008] Step S2: Extract key points from the point cloud based on curvature-weighted 3D-SIFT to obtain corresponding key points;

[0009] Step S3: Construct a point cloud feature descriptor and calculate the feature descriptor using an improved FPFH (Fast Point Feature Histogram) algorithm;

[0010] Step S4: Adopt the pose estimation scheme in the IGSP algorithm. First, construct an energy function, and then optimize the energy function through the KM algorithm to determine the global optimal correspondence. Based on this, calculate the transformation. Finally, through an iterative strategy, continuously repeat the process of optimal correspondence matching and transformation calculation to obtain the global optimal solution.

[0011] Preferably, the point cloud includes a target point cloud and a source point cloud;

[0012] The preprocessing includes uniformly simplifying the point cloud data while ensuring that the original features of the point cloud are not damaged, and removing the noise points in the point cloud.

[0013] Preferably, the step S2 includes:

[0014] Step S2.1: Fast Gaussian curvature estimation;

[0015] Step S2.2: Multi-scale curvature calculation. In order to obtain curvature information at different scales, calculate the Gaussian curvature of the point cloud under different scale parameters. Assume that m different scale parameters σ1, σ2, …, σ m , are selected. For each scale parameter, the corresponding Gaussian curvatures K1, K2, …, K m can be calculated. Among them, for each scale parameter, when calculating the local neighborhood covariance matrix, with point p as the center, the radius is:

[0016] r j = 3σ j (j ∈ [1, m])

[0017] where j represents the number of different scale parameters selected;

[0018] Step S2.3: After assigning corresponding weights to the curvatures of each scale, fuse the curvature information at different scales to obtain the fused curvature weight;

[0019] Step S2.4: Use the fused curvature weight to update the scale space construction. Assume the original scale space is L(x, y, z, σ), and the updated weighted scale space L'(x, y, z, σ) is expressed as:

[0020]

[0021] where W fused,irepresents the fused curvature weight of the i-th neighboring point in the point cloud, and L(x, y, z, σ) represents the scale space representation of the corresponding point under the action of the Gaussian convolution kernel;

[0022] Step S2.5: Use the Gaussian difference DoG operator to detect salient feature points. DoG detection is achieved by calculating the difference between adjacent scale space images.

[0023] Assuming there are two adjacent scale parameters σ1 and σ2 (where σ1<σ2), the corresponding weighted scale space images are L(x,y,z,σ) and L'(x,y,z,σ), respectively. DoGD(x,y,z) is expressed as:

[0024] D(x,y,z)=L(x,y,z,σ2)-L'(x,y,z,σ1)

[0025] In the DoG response image, local extreme points usually correspond to significant feature points in the point cloud. Local extreme point detection is performed on the DoG response image to find the extreme points, and the extreme points are marked as feature points.

[0026] Preferably, the step S2.1 includes:

[0027] Step S2.1.1: For each point P in the point cloud, construct a covariance matrix C in the local neighborhood of the point P. Assuming that there are k neighboring points in the local neighborhood, the covariance is as follows:

[0028]

[0029] Among them, p i represents the i-th point in the local neighborhood, represents the centroid of k neighboring points;

[0030] Step S2.1.2: Perform eigenvalue decomposition on the covariance matrix C to obtain three eigenvalues λ1, λ2, and λ3;

[0031] Step S2.1.3: Use the eigenvalues of the covariance matrix to quickly estimate the curvature at point p. The curvature estimation formula at point p is as follows:

[0032]

[0033] The difference between the maximum eigenvalue λ1 and the minimum eigenvalue λ3 is used to approximate the curvature. The larger the difference in eigenvalues, the more drastic the geometric changes in the local area and the greater the curvature.

[0034] Preferably, the step S2.3 includes:

[0035] Step S2.3.1: Determine the scale fusion weights as ω1, ω2, …, ωj , the calculation formula is as follows:

[0036]

[0037] Among them, σ ref represents the average value of all scale parameters, and σ scale represents the bandwidth parameter in the Gaussian function, which is used to adjust the attenuation rate of weight distribution during multi-scale curvature fusion;

[0038] Step S2.3.2: Combine multi-scale curvature calculation and scale fusion weights to obtain the fused curvature weight W fused , the formula is as follows:

[0039]

[0040] Among them, K j is the Gaussian curvature calculated at scale σ j .

[0041] Preferably, the said step S3 includes:

[0042] Step S3.1: Calculate the local density. Let the point cloud Q = {p i | i = 1, 2,..., N}, which contains coordinate and normal information n i Adopt the circumferential density formula:

[0043]

[0044] Among them, S k represents the circumference of the circle with the shortest distance from point p i to all points as the diameter, N represents the total number of points in the point cloud, represents the local circumferential density of point p i ;

[0045] Step S3.2: Calculate the adaptive neighborhood radius r i , the formula is as follows:

[0046]

[0047]

[0048]

[0049] Among them, γ represents the hyperparameter that controls the degree of nonlinearity, α i and β i represent the parameters related to the density within the neighborhood of point p i , represents the global maximum density variance, represents point p iThe density variance within the neighborhood, β base , α base are custom parameters;

[0050] Step S3.3: Calculate the basic FPFH geometric features, divide the FPFH feature descriptor into 11 intervals to form a 33-dimensional histogram;

[0051] Step S3.4: Calculate for all points Determine the minimum value D min and D max , divide [D min , D max into 11 equal intervals, count the intervals into which the density values of each point fall to form an 11-dimensional histogram;

[0052] Step S3.5: Integrate the FPFH feature descriptor of the density feature, splice the 33-dimensional geometric histogram and the 11-dimensional density histogram to form a 44-dimensional feature descriptor.

[0053] Preferably, the said Step S3.3 includes:

[0054] Step S3.3.1: With the origin p i as the center, calculate the normal deviation between the center and the points in the neighborhood (M(p i , r i )) and denote it as SPFH*(p i );

[0055] Step S3.3.2: With other points in the neighborhood as the centers, calculate the normal deviation between each neighborhood point and its own neighborhood points and denote it as SPFH(p i );

[0056] Step S3.3.3: With weights, add the results of Step S3.3.1 and Step S3.3.2 as follows:

[0057]

[0058] where m represents the number of neighborhood points, ω ij represents the distance weight between p i and p j , the closer the distance, the greater the weight.

[0059] Preferably, the said Step S4 includes:

[0060] Step S4.1: Construct a global key point matching energy function, and the energy function includes similar data and loss costs;

[0061] Step S4.2: Optimize with the KM algorithm, generalize the global energy function to a bipartite graph minimum weight matching problem, and solve it using the relaxed KM algorithm;

[0062] Step S4.3: Transformation estimation uses singular value decomposition (SVD) to estimate the optimal transformation, and the formula is as follows:

[0063]

[0064] Step S4.4: By comparing whether the change difference between the current iteration and the previous iteration is less than a preset threshold, if so, output the current optimal transformation and execute Step S4.5; if not, update the current target point cloud and then return to execute Step S4.1;

[0065] Step S4.5: According to the root mean square error (RMSE) of the corresponding point pairs after registration, measure the final completion degree of point cloud registration. The larger the RMSE, the greater the error, and vice versa. The description formula is as follows:

[0066]

[0067] Among them, W represents the total number corresponding to the two groups of point clouds p and q.

[0068] Preferably, the said Step S4.1 includes:

[0069] Step S4.1.1: Similar data represents the number of matching key points in the source point cloud and the target point cloud. p and q represent a pair of matching key points. Calculate the Euclidean distance ED and the feature distance HD between the key points p and q. The calculation formulas are as follows:

[0070] ED(p,q) = s ed ||p - q||

[0071] HD(p,q) = W hd ·FD(f p , f q )

[0072] ED(p,q) is calculated as the proportionality factor related to the point density s ed multiplied by the Euclidean distance between p and q, and FD(f p , f q ) is the difference between the FPFH descriptors of points p and q; where W ed and W hd represent the weights of the Euclidean distance and the feature distance respectively, and the calculation formulas are as follows:

[0073]

[0074] Among them, n represents the number of calculation iterations, and controls the weight change rate from 0 to y.

[0075] Step S4.1.2: Calculate the composite distance BD(p,q) between key points p and q based on the Euclidean distance ED and the feature distance HD between the key points p and q, which is defined as the weighted sum of the feature distance and the Euclidean distance. The formula is as follows:

[0076] BD(p,q) = W hd HD(p,q) + W ed ED(p,q);

[0077] Step S4.1.3: Construct an energy function. The formula is as follows:

[0078]

[0079] where A represents the set of corresponding points of the two-frame point cloud matching, S and T respectively represent the sets of the source point cloud and the target point cloud, the loss cost represents the unmatched key points, χ represents the set of unmatched key points in the two-frame point cloud, and W O represents the weight of the loss cost;

[0080] Step S4.1.4: Obtain the global optimal correspondence {N, χ} by minimizing the sum of the energy functions * as follows:

[0081]

[0082] A point cloud registration system based on curvature-weighted 3D-SIFT and adaptive radius FPFH provided by the present invention includes:

[0083] Module M1: Obtain point cloud data and perform preprocessing;

[0084] Module M2: Extract point cloud key points based on curvature-weighted 3D-SIFT to obtain corresponding key points;

[0085] Module M3: Construct a point cloud feature descriptor and calculate the feature descriptor using an improved FPFH fast point feature histogram algorithm;

[0086] Module M4: Adopt the attitude estimation scheme in the IGSP algorithm. First, construct an energy function, then optimize the energy function through the KM algorithm to determine the global optimal correspondence, and calculate the transformation based on this; finally, through an iterative strategy, continuously repeat the process of optimal correspondence matching and transformation calculation to obtain the global optimal solution.

[0087] Compared with the prior art, the present invention has the following beneficial effects:

[0088] 1. In terms of key point extraction, the present invention adopts a curvature-weighted 3D-SIFT method, which has stronger robustness to noise and local geometric changes, and avoids the problem that traditional SIFT is sensitive to tiny structures at a single scale. In repetitive structures or symmetric scenes, key points can be more stably distinguished.

[0089] 2. In terms of establishing feature descriptors, the adaptive radius FPFH dynamically adapts to the non-uniform distribution of point clouds through density-driven neighborhood adjustment and fusion with density-geometric features (44-dimensional histogram). The neighborhood radius is expanded in sparse areas to capture more context information, and reduced in dense areas to avoid redundant calculations, thereby enhancing the feature distinctiveness.

[0090] 3. The present invention combines density features, significantly enhancing the description ability for complex scenes (such as vegetation and irregular terrain) and reducing false matches.

[0091] 4. The improved algorithm of the present invention is significantly superior to the original IGSP in terms of registration accuracy (error reduced by 33%) and time efficiency (total time consumption reduced by 23.5%), being more accurate and rapid. BRIEF DESCRIPTION OF THE DRAWINGS

[0092] Other features, objectives, and advantages of the present invention will become more apparent by reading the following detailed description of non-limiting embodiments with reference to the accompanying drawings:

[0093] Figure 1 is a schematic diagram of the overall process of the present invention;

[0094] Figure 2 is a schematic diagram of key point extraction using the curvature-weighted 3D-SIFT proposed by the present invention;

[0095] Figure 3 is a schematic diagram of key point extraction of traditional 3D-SIFT;

[0096] Figure 4 is a registration result diagram based on the KITTI dataset using the present algorithm. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0097] The present invention will be described in detail below with reference to specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but do not limit the present invention in any form. It should be noted that those of ordinary skill in the art can make several changes and improvements without departing from the concept of the present invention. These all fall within the protection scope of the present invention.

[0098] The present invention is based on fusing curvature multi-scale for 3D-SIFT key point detection and density-driven construction of FPFH feature descriptors. First, the point cloud is voxel downsampled, and conditional filtering is used to remove the point cloud noise. Secondly, key points are extracted by combining weighted curvature information. The improved FPFH fast point feature histogram algorithm is used to calculate the feature descriptor. Then, an energy function is constructed and the KM algorithm is used to optimize the energy function to determine the global optimal correspondence. Finally, on this basis, the transformation is calculated through an iterative strategy, continuously repeating the process of optimal correspondence matching and transformation calculation to obtain the global optimal solution.

[0099] Embodiment 1

[0100] A point cloud registration method based on curvature weighted 3D-SIFT and adaptive radius FPFH provided by the present invention is improved on the basis of the IGSP (paper: Iterative Global Similarity Points: A robust coarse-to-fine integration solution for pairwise 3D point cloud registration) algorithm by enhancing the robustness of key points (3D-SIFT + curvature weighted) and optimizing the feature expression ability (FPFH), as Figure 1 shown, and includes the following steps:

[0101] Step S1: Obtain point cloud data and perform preprocessing. The point cloud includes a target point cloud and a source point cloud. The preprocessing includes uniformly simplifying the point cloud data while ensuring that the original features of the point cloud are not damaged, and removing the noise points in the point cloud. The step S1 includes point cloud downsampling and conditional filtering. The point cloud downsampling includes dividing the point cloud space into uniform voxels (three-dimensional cubes), and using the centroid point in each voxel grid to replace the remaining points in the voxel grid. The conditional filtering includes using the Conditional Removal function in the PCL open source library to complete the purpose of processing the point cloud data by setting different function parameters. For example, if the retention conditions in the X-axis and Y-axis directions of a single-frame point cloud are set to [40, 40], the point cloud data beyond the distance range of 40m in the X-axis and Y-axis will be removed.

[0102] Step S2: Extract point cloud key points based on curvature weighted 3D-SIFT to obtain corresponding key points. Specifically, it combines Gaussian curvature estimation, multi-scale fusion, weighted scale space construction, and DoG detection, and finally determines the main direction through gradient histogram. The step S2 includes:

[0103] Step S2.1: Fast Gaussian curvature estimation. The step S2.1 includes:

[0104] Step S2.1.1: For each point P in the point cloud, construct the covariance matrix C in its local neighborhood. Assuming there are k neighboring points in the local neighborhood, the covariance is as follows:

[0105]

[0106] The neighborhood is determined by the fixed radius method. i represents the i-th point in the local neighborhood, represents the centroid of k neighboring points.

[0107] Step S2.1.2: Perform eigenvalue decomposition on the covariance matrix C to obtain three eigenvalues λ1, λ2 and λ3, whose size relationship is λ1≥λ2≥λ3 and corresponding eigenvectors. The three eigenvalues reflect the distribution characteristics of points in the local neighborhood and are used to estimate the curvature.

[0108] Step S2.1.3: Use the eigenvalues of the covariance matrix to quickly estimate the curvature at point p, and use the ratio of the eigenvalues to approximate the curvature. For the curvature estimation at point p, the formula is as follows:

[0109]

[0110] The difference between the maximum eigenvalue λ1 and the minimum eigenvalue λ3 is used to approximate the curvature. The larger the difference in eigenvalues, the more dramatic the geometric changes in the local area and the greater the curvature.

[0111] Step S2.2: Multi-scale curvature calculation, in order to obtain curvature information at multiple scales, the Gaussian curvature of the point cloud is calculated at different scale parameters. Assume that m different scale parameters σ1, σ2,…,σ m ,For each scale parameter, the corresponding Gaussian curvature K1, K2, ..., K m For each scale parameter, in the local neighborhood covariance matrix, with point p as the center, the radius is:

[0112] r j =3σ j (j∈[1,m])

[0113] Here, j represents the number of different scale parameters selected.

[0114] Step S2.3: Determine the scale fusion weight. That is, after assigning a corresponding weight to the curvature of each scale, fuse the curvature information at different scales. Use a Gaussian function to assign the weight. As the scale increases, the weight should gradually decrease to avoid over-smoothing of local feature points by large-scale neighborhoods. Step S2.3 includes:

[0115] Step S2.3.1: Determine the scale fusion weights as ω1, ω2, …, ω j , and the calculation formula is as follows:

[0116]

[0117] where σ ref represents the average value of all scale parameters, and σ scale represents the bandwidth parameter in the Gaussian function, which is used to adjust the attenuation rate of weight distribution during multi-scale curvature fusion.

[0118] Step S2.3.2: Combine the multi-scale curvature calculation and the scale fusion weights to obtain the fused curvature weight W fused , and the formula is as follows:

[0119]

[0120] where K j is the Gaussian curvature calculated at scale σ j .

[0121] Step S2.4: Construct the weighted scale space. Use the fused curvature weight W fused to update the scale space construction. Assume the original scale space is L(x, y, z, σ), and the updated weighted scale space L'(x, y, z, σ) can be expressed as:

[0122]

[0123] where W fused,i represents the fused curvature weight of the i-th neighboring point in the point cloud, and L(x, y, z, σ) represents the scale space representation of the corresponding point under the action of the Gaussian convolution kernel.

[0124] Step S2.5: DoG detection. After constructing the weighted scale space, use the Difference of Gaussian (DoG) operator to detect significant feature points. DoG detection is achieved by calculating the difference between adjacent scale space images. Assume there are two adjacent scale parameters σ1 and σ2 (where σ1 < σ2), then the corresponding weighted scale space images are L(x, y, z, σ) and L'(x, y, z, σ) respectively. DoGD(x, y, z) can be expressed as:

[0125] D(x, y, z) = L(x, y, z, σ2) - L'(x, y, z, σ1)

[0126] In the DoG response image, local extreme points (i.e., points that are larger or smaller than the pixel values around them) usually correspond to significant feature points in the point cloud. To find these extreme points, local extreme detection is performed on the DoG response image. Specifically, for each pixel (x, y, z), its pixel value is compared with the pixel values in its surrounding neighborhood (including pixels in the same scale layer and adjacent scale layers). If it is a local extreme point, it is marked as a feature point.

[0127] Step S3: Construct a point cloud feature descriptor and calculate the feature descriptor. Calculate the improved Fast Point Feature Histogram (FPFH) based on the spatial differences between the extracted feature points and their neighboring feature points to accurately describe the spatial geometric properties within the point neighborhood. This step uses the improved FPFH fast point feature histogram algorithm to calculate the feature descriptor. The step S3 includes:

[0128] Step S3.1: Calculate the local density. Let the point cloud Q = {p i | i = 1, 2,..., N}, which contains coordinate and normal information n i Adopt the circular density formula:

[0129]

[0130] where S k represents the circumference of a circle with the shortest distance from point p i to all points as the diameter, N represents the total number of points in the point cloud, represents point p i local circular density.

[0131] Step S3.2: Calculate the adaptive neighborhood radius r i , and the formula is as follows:

[0132]

[0133]

[0134]

[0135] where γ represents a hyperparameter that controls the degree of nonlinearity (optimized by cross-validation, default γ = 0.1), α i and β i are calculated as shown in the formula and are parameters related to the density within the neighborhood of point p i , represents the global maximum density variance, represents the density variance within the neighborhood of point p i (pre-defined initial neighborhood radius r init is used for calculation), β base , α base are custom parameters.

[0136] Step S3.3: Calculate the basic FPFH geometric features, divide the FPFH feature descriptor into 11 intervals, and form a 33-dimensional histogram. The step S3.3 includes:

[0137] Step S3.3.1: With the origin p i as the center, calculate the normal deviation between it and the points in the neighborhood (M(p i , r i )) and denote it as SPFH*(p i ).

[0138] Step S3.3.2: With other points in the neighborhood as the center, calculate the normal deviation between each neighborhood point and its own neighborhood points and denote it as SPFH(p i ).

[0139] Step S3.3.3: With weights, add the results of step S3.3.1 and step S3.3.2 as follows:

[0140]

[0141] where m represents the number of neighborhood points, ω ij represents the distance weight between p i and p j , and the closer the distance, the greater the weight.

[0142] Step S3.4: Calculate for all points Determine the minimum values D min and D max , divide [D min , D max into 11 equal intervals, count the intervals into which the density values of each point fall, and form an 11-dimensional histogram.

[0143] Step S3.5: Fuse the FPFH feature descriptor of the density feature, splice the 33-dimensional geometric histogram and the 11-dimensional density histogram to form a 44-dimensional feature descriptor.

[0144] Step S4: Adopt the pose estimation scheme in the IGSP algorithm. First, construct an energy function, then optimize the energy function through the KM algorithm to determine the global optimal correspondence, and calculate the transformation on this basis; finally, through the iterative strategy, continuously repeat the process of optimal correspondence matching and transformation calculation to obtain the global optimal solution. The step S4 includes:

[0145] Step S4.1: Construct a global key point matching energy function, and the energy function includes similar data and loss costs. The step S4.1 includes:

[0146] Step S4.1.1: The number of matching key points in the source point cloud and the target point cloud is represented by similar data. p and q represent a pair of matching key points. Calculate the Euclidean distance ED and the feature distance HD between the key points p and q. The calculation formulas are as follows:

[0147] ED(p,q) = s ed ||p - q||

[0148] HD(p,q) = W hd ·FD(f p ,f q )

[0149] ED(p,q) is calculated as the Euclidean distance between p and q multiplied by a scale factor related to the point density s ed FD(f p ,f q ) is the difference between the FPFH descriptors of points p and q. Where W ed and W hd represent the weights of the Euclidean distance and the feature distance respectively. The calculation formulas are as follows:

[0150]

[0151] Where n represents the number of calculation iterations, and the control weight change rate ranges from 0 to y.

[0152] Step S4.1.2: According to the Euclidean distance ED and the feature distance HD between the key points p and q, calculate the composite distance BD(p,q) between the key points p and q, which is defined as the weighted sum of the feature distance and the Euclidean distance. The formula is as follows:

[0153] BD(p,q) = W hd HD(p,q)+W ed ED(p,q)

[0154] Step S4.1.3: Construct an energy function. The formula is as follows:

[0155]

[0156] Where A represents the set of corresponding points in the two-frame point cloud matching, S and T represent the sets of the source point cloud and the target point cloud respectively, the loss cost represents the non-matching key points, χ represents the set of non-matching key points in the two-frame point cloud, and W O represents the weight of the loss cost.

[0157] Step S4.1.4: By minimizing the sum of the energy functions, obtain the global optimal correspondence {N, χ} * as follows:

[0158]

[0159] Step S4.2: Optimize the KM algorithm, generalize the global energy function to the minimum weight matching problem of a bipartite graph, and solve it using the relaxed KM algorithm.

[0160] Step S4.3: Estimate the optimal transformation using singular value decomposition (SVD). The formula is as follows:

[0161]

[0162] Step S4.4: By comparing whether the change difference (translation and rotation) between the current iteration and the previous iteration is less than a preset threshold. If so, output the current optimal transformation; if not, update the current target point cloud and then return to execute Step S4.1. Specifically, in practice, the rotation error is obtained by calculating the radian value of the rotation matrix difference, and the translation error is obtained by calculating the Euclidean norm of the translation vector difference. Thresholds are preset for both translation and rotation. Once the transformation difference between two iterations meets the conditions, the iterative process stops.

[0163] Step S4.5: According to the root mean square error (RMSE) of the corresponding point pairs after registration, measure the final completion degree of point cloud registration. The larger the RMSE, the greater the error; conversely, the smaller the error. The description formula is as follows:

[0164]

[0165] Among them, W represents the total number of corresponding point cloud pairs of groups p and q.

[0166] Verify the effectiveness of the key point extraction algorithm of this paper on the Bunny dataset, where Figure 2 is the feature point algorithm extracted by the algorithm of this paper, such as Figure 3 the feature points extracted by the traditional 3D-SIFT algorithm as shown. It can be seen that in Figure 2 , the feature points are in areas with large curvature changes. In contrast, in areas with relatively small curvature changes, the feature points are less and more evenly distributed, indicating that the algorithm of this paper can extract key points more effectively. Conduct an overall registration experiment on the KIITI dataset and use a point cloud registration algorithm based on improved curvature-weighted 3D-SIFT and adaptive radius FPFH. The effect is as Figure 4 shown. The improved algorithm is significantly superior to the original IGSP in terms of registration accuracy (error reduced by 33%) and time efficiency (total time consumption reduced by 23.5%). Experiments show that in complex outdoor scenes, the present invention can achieve a good balance between registration efficiency and registration accuracy.

[0167] Embodiment 2

[0168] The present invention also provides a point cloud registration system based on curvature-weighted 3D-SIFT and adaptive radius FPFH. The point cloud registration system based on curvature-weighted 3D-SIFT and adaptive radius FPFH can be implemented by executing the process steps of the point cloud registration method based on curvature-weighted 3D-SIFT and adaptive radius FPFH, that is, those skilled in the art can understand the point cloud registration method based on curvature-weighted 3D-SIFT and adaptive radius FPFH as a preferred implementation of the point cloud registration system based on curvature-weighted 3D-SIFT and adaptive radius FPFH.

[0169] According to the present invention, a point cloud registration system based on curvature-weighted 3D-SIFT and adaptive radius FPFH is provided, comprising:

[0170] Module M1: Acquire and preprocess point cloud data. The point cloud includes a target point cloud and a source point cloud. Preprocessing involves uniformly simplifying the point cloud data and removing noise points while preserving the original features of the point cloud.

[0171] Module M2: Extract key points from point clouds based on curvature-weighted 3D-SIFT to obtain corresponding key points. Module M2 includes:

[0172] Module M2.1: Fast Gaussian curvature estimation. Module M2.1 includes: Module M2.1.1: For each point P in the point cloud, construct a covariance matrix C in the local neighborhood of the point P. Assuming that there are k neighboring points in the local neighborhood, the covariance is as follows:

[0173]

[0174] Among them, p i represents the i-th point in the local neighborhood, Represents the centroid of k neighboring points. Module M2.1.2: Perform eigenvalue decomposition on the covariance matrix C to obtain three eigenvalues λ1, λ2, and λ3. Module M2.1.3: Use the eigenvalues of the covariance matrix to quickly estimate the curvature at point p. For the curvature estimation at point p, the formula is as follows:

[0175]

[0176] The difference between the maximum eigenvalue λ1 and the minimum eigenvalue λ3 is used to approximate the curvature. The larger the difference in eigenvalues, the more drastic the geometric changes in the local area and the greater the curvature.

[0177] Module M2.2: Multi-scale curvature calculation. In order to obtain curvature information at multiple scales, the Gaussian curvature of the point cloud is calculated at different scale parameters. Assume that m different scale parameters σ1, σ2,…,σm , For each scale parameter, the corresponding Gaussian curvatures K1, K2, …, K can be calculated m , where for each scale parameter, when calculating the local neighborhood covariance matrix, centered at point p, the radius is:

[0178] r j = 3σ j (j ∈ [1, m])

[0179] where j represents the number of different scale parameters selected.

[0180] Module M2.3: After assigning corresponding weights to the curvatures at each scale, fuse the curvature information at different scales to obtain the fused curvature weights. The module M2.3 includes: Module M2.3.1: Determine the scale fusion weights as ω1, ω2, …, ω j , and the calculation formula is as follows:

[0181]

[0182] where σ ref represents the average value of all scale parameters, and σ scale represents the bandwidth parameter in the Gaussian function, which is used to adjust the attenuation speed of weight assignment during multi-scale curvature fusion. Module M2.3.2: Combine the multi-scale curvature calculation and the scale fusion weights to obtain the fused curvature weight W fused , and the formula is as follows:

[0183]

[0184] where K j is the Gaussian curvature calculated at scale σ j .

[0185] Module M2.4: Use the fused curvature weights to update the scale space construction. Assuming the original scale space is L(x, y, z, σ), the updated weighted scale space L'(x, y, z, σ) is expressed as:

[0186]

[0187] where W fused,i represents the fused curvature weight of the i-th neighboring point in the point cloud, and L(x, y, z, σ) represents the scale space representation of the corresponding point under the action of the Gaussian convolution kernel.

[0188] Module M2.5: Detect significant feature points using the Difference of Gaussians (DoG) operator. DoG detection is achieved by calculating the difference between adjacent scale-space images. Suppose there are two adjacent scale parameters σ1 and σ2 (where σ1 < σ2), then the corresponding weighted scale-space images are L(x, y, z, σ) and L'(x, y, z, σ) respectively. DoGD(x, y, z) is expressed as:

[0189] D(x, y, z) = L(x, y, z, σ2) - L'(x, y, z, σ1)

[0190] In the DoG response image, local extreme points usually correspond to significant feature points in the point cloud. Perform local extreme detection on the DoG response image to find the extreme points, and mark the extreme points as feature points.

[0191] Module M3: Construct a point cloud feature descriptor and calculate the feature descriptor using an improved Fast Point Feature Histogram (FPFH) algorithm. Module M3 includes: Module M3.1: Calculate the local density. Let the point cloud Q = {p i | i = 1, 2,..., N}, which contains coordinate and normal information n i Adopt the circular density formula:

[0192]

[0193] where S k represents the circumference of a circle with the shortest distance from point p i to all points as the diameter, N represents the total number of points in the point cloud, represents point p i local circular density. Module M3.2: Calculate the adaptive neighborhood radius r i , and the formula is as follows:

[0194]

[0195]

[0196]

[0197] where γ represents a hyperparameter that controls the degree of non-linearity, α i and β i represent parameters related to the density within the neighborhood of point p i , represents the global maximum density variance, represents point p i neighborhood density variance, β base , α baseis a custom parameter. Module M3.3: Calculate the basic FPFH geometric features, divide the FPFH feature descriptor into 11 intervals, and form a 33-dimensional histogram. The module M3.3 includes: Module M3.3.1: With the origin p i as the center, calculate the normal deviation between the center and the points in the neighborhood (M(p i , r i ), denoted as SPFH*(p i ). Module M3.3.2: With other points in the neighborhood as the center, calculate the normal deviation between each neighborhood point and its own neighborhood points, denoted as SPFH(p i ). Module M3.3.3: With the assistance of weights, add the results of Module M3.3.1 and Module M3.3.2, as shown in the following formula:

[0198]

[0199] where m represents the number of neighborhood points, and ω ij represents the distance weight between p i and p j , the closer the distance, the greater the weight. Module M3.4: Calculate the of all points, determine the minimum values D min and D max , divide [D min , D max into 11 equal intervals, count the intervals into which the density values of each point fall, and form an 11-dimensional histogram. Module M3.5: Integrate the FPFH feature descriptor with density features, splice the 33-dimensional geometric histogram and the 11-dimensional density histogram to form a 44-dimensional feature descriptor.

[0200] Module M4: Adopt the attitude estimation scheme in the IGSP algorithm. First, construct an energy function, and then optimize the energy function through the KM algorithm to determine the global optimal correspondence, and calculate the transformation on this basis. Finally, through an iterative strategy, continuously repeat the process of optimal correspondence matching and transformation calculation to obtain the global optimal solution. The module M4 includes:

[0201] Module M4.1: Construct a global key point matching energy function, and the energy function includes similar data and loss costs. The module M4.1 includes: Module M4.1.1: The similar data represents the number of matching key points in the source point cloud and the target point cloud. p and q represent a pair of matching key points. Calculate the Euclidean distance ED and feature distance HD between the key points p and q. The calculation formulas are as follows:

[0202] ED(p, q) = s ed ||p - q||

[0203] HD(p, q) = W hd ·FD(fp , f q )

[0204] ED(p, q) is calculated as the Euclidean distance between p and q multiplied by a scale factor related to the point density s ed , and FD(f p , f q ) is the difference between the FPFH descriptors of points p and q. Where W ed and W hd represent the weights of the Euclidean distance and the feature distance respectively, and the calculation formulas are as follows:

[0205]

[0206] Where n represents the number of iterations of the calculation, controlling the weight change rate from 0 to y. Module M4.1.2: Calculate the composite distance BD(p, q) between the key points p and q according to the Euclidean distance ED and the feature distance HD of the key points p and q, which is defined as the weighted sum of the feature distance and the Euclidean distance, and the formula is as follows:

[0207] BD(p, q) = W hd HD(p, q) + W ed ED(p, q).

[0208] Module M4.1.3: Construct an energy function, and the formula is as follows:

[0209]

[0210] Where A represents the set of corresponding points of the two-frame point cloud matching, S and T represent the sets of the source point cloud and the target point cloud respectively, the loss cost represents the unmatched key points, χ represents the set of unmatched key points in the two-frame point cloud, and W O represents the weight of the loss cost. Module M4.1.4: Obtain the global optimal correspondence {N, χ} by minimizing the sum of the energy functions * as follows:

[0211]

[0212] Module M4.2: KM algorithm optimization, generalize the global energy function to the minimum weight matching problem of a bipartite graph, and use the relaxed KM algorithm to solve it.

[0213] Module M4.3: Transformation estimation uses singular value decomposition SVD to estimate the optimal transformation, and the formula is as follows:

[0214]

[0215] Module M4.4: By comparing whether the change difference between the current iteration and the previous iteration is less than a preset threshold, if so, output the current optimal transformation to trigger Module M4.5. If not, update the current target point cloud and then return to trigger Module M4.1.

[0216] Module M4.5: According to the root mean square error (RMSE) of the corresponding point pairs after registration, measure the final completion degree of point cloud registration. The larger the RMSE, the greater the error, and vice versa. The description formula is as follows:

[0217]

[0218] Among them, W represents the total number of corresponding point cloud pairs of groups p and q.

[0219] Those skilled in the art know that in addition to implementing the system and its various devices, modules, and units provided by the present invention in the form of pure computer-readable program code, the method steps can be logically programmed to enable the system and its various devices, modules, and units provided by the present invention to be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers, etc., to achieve the same functions. Therefore, the system and its various devices, modules, and units provided by the present invention can be considered as a kind of hardware component, and the devices, modules, and units included therein for implementing various functions can also be regarded as the structures within the hardware component; it can also be considered that the devices, modules, and units for implementing various functions are both software modules for implementing the method and structures within the hardware component.

[0220] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. Without conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A point cloud registration method based on curvature-weighted 3D-SIFT and adaptive radius FPFH, characterized in that, include: Step S1: Acquire point cloud data and perform preprocessing; Step S2: Extract key points of point cloud based on curvature-weighted 3D-SIFT to obtain corresponding key points; Step S3: constructing a point cloud feature descriptor and calculating the feature descriptor using the improved FPFH fast point feature histogram algorithm; Step S4: Using the posture estimation scheme in the IGSP algorithm, first construct an energy function, then optimize the energy function through the KM algorithm to determine the global optimal correspondence, and calculate the transformation based on this; finally, through the iterative strategy, continuously repeat the process of optimal correspondence matching and transformation calculation to obtain the global optimal solution.

2. The point cloud registration method based on curvature weighted 3D-SIFT and adaptive radius FPFH according to claim 1, wherein, The point cloud includes a target point cloud and a source point cloud; The preprocessing includes uniformly simplifying the point cloud data and removing noise points in the point cloud while ensuring that the original features of the point cloud are not destroyed.

3. The point cloud registration method based on curvature-weighted 3D-SIFT and adaptive radius FPFH according to claim 1, wherein step S2 comprises: Step S2.1: Fast Gaussian curvature estimation; Step S2.2: Multi-scale curvature calculation. To obtain the curvature information at multiple scales, the Gaussian curvature of the point cloud is calculated under different scale parameters. Suppose m different scale parameters σ1, σ2, …, σ m are selected. For each scale parameter, the corresponding Gaussian curvatures K1, K2, …, K m can be calculated. Among them, for each scale parameter, when calculating the local neighborhood covariance matrix, with point p as the center, the radius is: r j = 3σ j (j ∈ [1, m]) Where j represents the number of different scale parameters selected; Step S2.3: After assigning corresponding weights to the curvature at each scale, the curvature information at different scales is fused to obtain the fused curvature weights; Step S2.4: Use the fused curvature weights to update the scale space. Assuming the original scale space is L(x, y, z, σ), the updated weighted scale space L'(x, y, z, σ) is expressed as: Among them, W fused,i represents the fused curvature weight of the i-th neighboring point in the point cloud, and L(x, y, z, σ) represents the scale space representation of the corresponding point under the action of the Gaussian convolution kernel; Step S2.5: Use the Gaussian difference DoG operator to detect salient feature points. DoG detection is achieved by calculating the difference between adjacent scale space images. Assuming there are two adjacent scale parameters σ1 and σ2 (where σ1<σ2), the corresponding weighted scale space images are L(x,y,z,σ) and L'(x,y,z,σ), respectively. DoGD(x,y,z) is expressed as: D(x,y,z)=L(x,y,z,σ2)-L'(x,y,z,σ1) In the DoG response image, local extreme points usually correspond to significant feature points in the point cloud. Local extreme point detection is performed on the DoG response image to find the extreme points, and the extreme points are marked as feature points.

4. The point cloud registration method based on curvature-weighted 3D-SIFT and adaptive radius FPFH according to claim 3, wherein step S2.1 comprises: Step S2.1.1: For each point P in the point cloud, construct a covariance matrix C in the local neighborhood of the point P. Assuming that there are k neighboring points in the local neighborhood, the covariance is as follows: Among them, p i represents the i-th point in the local neighborhood, and represents the centroid of k neighboring points; Step S2.1.2: Perform eigenvalue decomposition on the covariance matrix C to obtain three eigenvalues λ1, λ2, and λ3; Step S2.1.3: Use the eigenvalues of the covariance matrix to quickly estimate the curvature at point p. The curvature estimation formula at point p is as follows: The difference between the maximum eigenvalue λ1 and the minimum eigenvalue λ3 is used to approximate the curvature. The larger the difference in eigenvalues, the more drastic the geometric changes in the local area and the greater the curvature.

5. The point cloud registration method based on curvature-weighted 3D-SIFT and adaptive radius FPFH according to claim 3, wherein the step S2.3 includes: Step S2.3.1: Determine the scale fusion weights as ω1, ω2, …, ω j , and the calculation formula is as follows: Among them, σ ref represents the average value of all scale parameters, and σ scale represents the bandwidth parameter in the Gaussian function, which is used to adjust the attenuation rate of weight distribution during multi-scale curvature fusion; Step S2.3.2: Combine the multi-scale curvature calculation and the scale fusion weight to obtain the fused curvature weight W fused , and the formula is as follows: Among them, K j is the Gaussian curvature calculated at scale σ j .

6. The point cloud registration method based on curvature-weighted 3D-SIFT and adaptive radius FPFH according to claim 1, wherein the step S3 includes: Step S3.1: Calculate the local density. Let the point cloud Q = {p i | i = 1, 2,..., N}, which contains coordinate and normal information n i Adopt the circular density formula: Among them, S k represents the circumference of a circle with the shortest distance from point p i to all points as the diameter, N represents the total number of points in the point cloud, represents point p i local circumferential density; Step S3.2: Calculate the adaptive neighborhood radius r i , and the formula is as follows: Among them, γ represents the hyperparameter that controls the degree of nonlinearity, α i and β i represent the parameters related to the density within the neighborhood of point p i , represents the global maximum density variance, represents the density variance within the neighborhood of point p i , and β base , α base are user-defined parameters; Step S3.3: Calculate the basic FPFH geometric features, divide the FPFH feature descriptor into 11 intervals to form a 33-dimensional histogram; Step S3.4: Calculate for all points Determine the minimum value D min and D max , divide [D min , D max into 11 equal intervals, count the intervals into which the density values of each point fall, and form an 11-dimensional histogram; Step S3.5: Fuse the FPFH feature descriptor of the density feature, splice the 33-dimensional geometric histogram and the 11-dimensional density histogram to form a 44-dimensional feature descriptor.

7. The point cloud registration method based on curvature-weighted 3D-SIFT and adaptive radius FPFH according to claim 1, wherein the step S3.3 includes: Step S3.3.1: Taking the origin p i as the center, calculate the normal deviation between the center and the points in the neighborhood (M(p i , r i )) and denote it as SPFH*(p i ); Step S3.3.2: Taking other points in the neighborhood as the center, calculate the normal deviation between each neighborhood point and its own neighborhood points, denoted as SPFH(p i ); Step S3.3.3: With the aid of weights, add the results of step S3.3.1 and step S3.3.2, as shown in the following formula: where m represents the number of neighborhood points, ω ij represents the i distance weight between p j and p, the closer the distance, the greater the weight.

8. The point cloud registration method based on curvature-weighted 3D-SIFT and adaptive radius FPFH according to claim 1, wherein the step S4 includes: Step S4.1: Construct a global key point matching energy function, and the energy function includes similar data and loss cost; Step S4.2: Optimize the KM algorithm, generalize the global energy function to the bipartite graph minimum weight matching problem, and use the relaxed KM algorithm to solve it; Step S4.3: Use singular value decomposition SVD to estimate the optimal transformation for transformation estimation, and the formula is as follows: Step S4.4: By comparing whether the change difference between the current iteration and the previous iteration is less than a preset threshold, if so, output the current optimal transformation and execute step S4.5; if not, update the current target point cloud and then return to execute step S4.1; Step S4.5: According to the root mean square error (RMSE) of the corresponding point pairs after registration, measure the final completion degree of point cloud registration. The larger the RMSE root mean square, the greater the error, and vice versa, the smaller the error. The description formula is as follows: Wherein, W represents the total number of corresponding point cloud pairs of p and q groups.

9. The point cloud registration method based on curvature-weighted 3D-SIFT and adaptive radius FPFH according to claim 8, wherein the step S4.1 includes: Step S4.1.1: The similar data represents the number of matching key points in the source point cloud and the target point cloud. p and q represent a pair of matching key points. Calculate the Euclidean distance ED and the feature distance HD between the key points p and q. The calculation formula is as follows: ED(p,q) = s ed ||p - q|| HD(p,q) = W hd ·FD(f p , f q ) ED(p,q) is calculated as the Euclidean distance between p and q multiplied by a scale factor related to the point density s ed FD(f p , f q ) is the difference between the FPFH descriptors of points p and q; where W ed and W hd represent the weights of the Euclidean distance and the feature distance respectively, and the calculation formulas are as follows: Wherein, n represents the number of calculation iterations, and the weight change rate is controlled from 0 to y. Step S4.1.2: According to the Euclidean distance ED and the feature distance HD between the key points p and q, calculate the composite distance BD(p,q) between the key points p and q, which is defined as the weighted sum of the feature distance and the Euclidean distance. The formula is as follows: BD(p,q) = W hd HD(p,q) + W ed ED(p,q); Step S4.1.3: Construct an energy function, and the formula is as follows: Among them, A represents the set of corresponding points of two-frame point cloud matching, S and T respectively represent the sets of source point cloud and target point cloud, the loss cost represents the unmatched key points, χ represents the set of unmatched key points in two-frame point clouds, and W O represents the weight of the loss cost; Step S4.1.4: Obtain the global optimal correspondence {N, χ} by minimizing the energy functions sum and * as shown in the following equation:

10. A point cloud registration system based on curvature-weighted 3D-SIFT and adaptive radius FPFH, characterized in that, Including: Module M1: Obtain point cloud data and perform preprocessing; Module M2: Extract point cloud key points based on curvature-weighted 3D-SIFT to obtain corresponding key points; Module M3: Construct a point cloud feature descriptor and calculate the feature descriptor using an improved FPFH (Fast Point Feature Histogram) algorithm. Module M4: Adopt the pose estimation scheme in the IGSP (Iterative Global Similarity Projection) algorithm. First, construct an energy function, then optimize the energy function through the KM (Kuhn-Munkres) algorithm to determine the global optimal correspondence, and calculate the transformation based on this. Finally, through an iterative strategy, continuously repeat the process of optimal correspondence matching and transformation calculation to obtain the global optimal solution.

Images