Method for accelerating point cloud registration based on 3DSC and improved ICP algorithm

By combining 3DSC and improved ICP algorithms, using voxel filtering and Gaussian sampling, optimizing feature descriptor extraction and nearest neighbor search, the problem of large amount of point cloud registration calculation and low accuracy in the existing technology is solved, and a more efficient point cloud is achieved Registration process.

CN118918153AInactive Publication Date: 2024-11-08滨沅国科(秦皇岛)智能科技股份有限公司

Patent Information

Application Number
CN202410956517.1
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-17
Publication Date
2024-11-08
Estimated Expiration
Not applicable · inactive patent

AI Technical Summary

Technical Problem

The existing point cloud registration algorithms have shortcomings in the problems of large computing volume, low accuracy and local optimal solutions, and it is difficult to quickly and accurately align large-scale point cloud datasets.

Method used

Using a method based on 3DSC and improved ICP algorithm, combined with voxel filtering and Gaussian sampling, reduce the amount of point cloud data, use 3DSC to extract feature descriptors, optimize the nearest neighbor search parameters, and use KD-Tree in the NICP algorithm to accelerate nearest neighbor search, improve Computational efficiency of precise registration.

Benefits of technology

It improves the accuracy and speed of point cloud registration, reduces computing time, and enhances the integrity and registration effect of point cloud structure.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118918153A_ABST
    Figure CN118918153A_ABST
Patent Text Reader

Abstract

The invention discloses a method for accelerating point cloud registration based on 3DSC and an improved ICP algorithm, and the method comprises the following steps: S1, reducing the data volume of a point cloud through the combination of voxel filtering and Gaussian sampling, guaranteeing the integrity of a point cloud structure, and improving the processing speed of the point cloud; and S2, performing feature descriptor extraction and feature descriptor matching by using a 3DSC algorithm. And S3, optimizing parameters such as neighbor search radius and point cloud density according to point cloud characteristics to obtain better matrix estimation. And S4, transmitting the coarse registration matrix estimation to an NICP algorithm, initializing an initial homogeneous transformation matrix of the NICP algorithm, and improving the registration precision. And S5, in the NICP algorithm, using the KD-Tree to accelerate the nearest neighbor search and point cloud matching process, and reducing the calculation time. And S6, matrix optimization is carried out in combination with an ICP improved algorithm, namely an NICP fine registration algorithm.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention relates to a point cloud matching algorithm, and in particular to a method for accelerating point cloud registration based on 3DSC and an improved ICP algorithm, thereby improving the point cloud matching accuracy and accelerating the processing speed of point cloud matching. Background Art

[0002] With the development and popularization of 3D sensors (such as LiDAR, structured light cameras, etc.), it has become easy to obtain large-scale point cloud datasets. These point cloud datasets can contain point clouds collected from different perspectives, at different times, or using different sensors. Before using these point cloud datasets in applications, they usually need to be aligned to the same coordinate system. The point cloud registration process is to find a rotation and translation matrix (rigid transformation or Euclidean transformation) between two point clouds to transform the source point cloud to the same coordinate system as the target point cloud.

[0003] However, in the actual acquisition process of point cloud data, it is difficult to directly obtain an appropriate rotation matrix and translation matrix due to the influence of factors such as acquisition angle, acquisition distance, object occlusion, etc. Therefore, it is necessary to study and improve the point cloud registration algorithm.

[0004] The essence of point cloud registration is to find an optimal transformation matrix so that the overlap between two sets of point clouds in different coordinate systems can be as high as possible after matrix transformation. There are generally four steps in point cloud registration. The first is data preprocessing, such as data noise reduction, data downsampling, etc.; the second is coarse registration, which means finding an approximate transformation matrix through rapid search when the relative pose of the target point cloud and the reference point cloud is completely unknown; the second is fine registration (such as ICP algorithm) to obtain a more accurate matrix based on the coarse registration transformation, and the last is result evaluation, which uses the algorithm's evaluation system to evaluate the effect and quality of the registration.

[0005] Point cloud registration requires the selection of appropriate coarse registration methods and fine registration methods, combined with some point cloud processing technologies, to achieve an accurate and fast registration method. The current coarse registration methods include coarse registration based on local feature descriptors (PFH, FPFH) and probability distribution (NDT). These methods have large computational complexity or low registration accuracy, which has a great impact on subsequent fine registration. In addition, the ICP iterative algorithm is simple, easy to implement, and has high solution accuracy. However, ICP is sensitive to the initial poses of the original point cloud and the target point cloud, and is prone to fall into the local optimal solution. It also has the disadvantages of large computational complexity and slow calculation speed. Summary of the invention

[0006] The present invention proposes a method for accelerating point cloud registration based on 3DSC and improved ICP algorithm, which is used to improve the accuracy of point cloud registration. In addition, Gaussian filtering is used to downsample the point cloud and ensure the integrity of the point cloud structure, thereby improving the speed of point cloud registration.

[0007] In order to achieve the above technical effects, the present invention is implemented through the following technical solutions:

[0008] A method for accelerating point cloud registration based on 3DSC and improved ICP algorithm includes the following steps:

[0009] S1. By combining voxel filtering and Gaussian sampling, the amount of point cloud data is reduced, while ensuring the integrity of the point cloud structure and improving the point cloud processing speed.

[0010] S2. Use 3DSC algorithm to extract and match feature descriptors.

[0011] S3. According to the characteristics of the point cloud, the parameters such as the nearest neighbor search radius and point cloud density are optimized to obtain a better matrix estimation.

[0012] S4. Pass the rough registration matrix estimate to the NICP algorithm, initialize the initial homogeneous transformation matrix of the NICP algorithm, and improve the registration accuracy.

[0013] S5. In the NICP algorithm, KD-Tree is used to accelerate the nearest neighbor search and point cloud matching process and reduce the calculation time.

[0014] S6. Combine the improved ICP algorithm, namely the NICP precise registration algorithm, to perform matrix optimization.

[0015] Furthermore, the S1 step is specifically as follows:

[0016] S11. Divide the point cloud data into a regular three-dimensional voxel grid. Each voxel grid covers a fixed spatial range of [v1 v2 v3]. v1, v2, v3 are the voxel lengths along x, y, and z, respectively.

[0017] S12. For each voxel grid, select one point as a representative point. A common selection method is to select the center point in the grid as the representative point.

[0018] S13. Based on the obtained representative point set, a set of sampling points is generated using a Gaussian sampling method. The number and density of the sampling points can be controlled according to the variance parameter of the Gaussian distribution.

[0019] S14. For each sampling point, select the original point closest to it as the final downsampling result.

[0020] The step S2 is specifically as follows:

[0021] S21. For the number of point clouds S, s is any point in it, and n is the normal vector l of point s. With s as the center of the sphere and l as the due north direction of the sphere, a local spherical space is established. According to the radius r, azimuth angle α and elevation angle θ, J+1, K+1 and L+1 segments are taken respectively. The histogram data representation of J*K*L point clouds is obtained. The radius segment equation is as follows:

[0022]

[0023] where r min and r max is the boundary of the spherical space. In addition, an initial weight needs to be assigned to each spherical space:

[0024]

[0025] Where V(j,k,l) ​​is the volume of the corresponding space, s i is the density of the corresponding space. It is estimated based on the number of points and the radius. i ) values ​​form a feature vector This vector contains the shape context features around the query point.

[0026]

[0027] Among them, k = {1, 2,…, n}, n = J×K×L.

[0028] S22. The source point cloud and the target point cloud are matched using local space descriptors. The nearest neighbor matching strategy is adopted to calculate the Euclidean distance between the source point cloud and the feature descriptor target point cloud feature descriptor and compare them. The one with the smallest distance is the feature descriptor matching object. Repeat the execution to complete the feature descriptor matching.

[0029] The step S4 is specifically as follows:

[0030] S41, first replace the NICP initial homogeneous matrix according to the coarse registration transformation, and set any point M(x m ,y m ,z m ), any point N(x n ,y n ,z n ), and M and N belong to the set of point clouds. The matrix transformation of the two point clouds is further solved by the NICP algorithm using the secondary transformation matrix parameters. The initial matrix transformation is as follows:

[0031]

[0032] Among them, t x ,ty ,t z is the translation variable along the three coordinate axes, w is the scaling factor, the default is 1, α, β, λ are the rotation angles around the x, y, and z axes respectively.

[0033] S42, passing the matrix parameters obtained by the 3DSC coarse registration to the NICP algorithm as update values ​​of the initial homogeneous transformation matrix.

[0034] S43, set s i For any point in the point cloud S, calculate V around it as the center i The covariance formula of the points is:

[0035]

[0036] Among them, μ i is the average value of the point cloud. Since the covariance matrix is ​​symmetric, SVD decomposition is performed to obtain:

[0037]

[0038] The characteristic roots are λ1, λ2, λ3, which are sorted from small to large. The smallest characteristic root is the direction of the normal vector. The curvature is σ i =λ1 / (λ1+λ2+λ3). If the point cloud is flat, the covariance matrix is ​​close to singular, and the minimum eigenvalue is recorded as ε.

[0039] NICP transforms the coordinates and normal vectors of the two sets of point clouds to p'=(p,n). The transformation rules are as follows:

[0040]

[0041] Rotate and translate the points, rotate the normal vector. The error function is the sum of the error between the distance between the points and the normal vector angle.

[0042]

[0043] Where q represents the point of the reference point cloud, and the final error function is:

[0044]

[0045] in Contains the information matrix, the plane and the normal vector The plane matrix uses the inverse of the covariance, and the normal vector matrix uses the unit matrix when it is non-singular, otherwise it is as follows,

[0046]

[0047] The LMF algorithm is used for optimization solution.

[0048] The step S5 is specifically as follows:

[0049] Point cloud has three dimensions: x, y, and z. Kd-tree can be used for multi-dimensional search. This algorithm uses radius search. First, kd-tree is constructed to spatially separate the x, y, and z axes. The middle node of the sorting order is found. The node smaller than the middle position is the left child node, and the node larger than the middle position is the right child node. The loop is executed until the number of points contained in the child node is less than the preset threshold (leafsize). The leaf node where the query point is located is searched downward, and the distance between all points in the leaf node and the query point is calculated. The points with a distance less than the query radius r are recorded; the distance between the query point and the current separation axis is calculated, and the search is backtracked upward. If r> the distance from the query point to the search plane, the subspace is searched and the points with a distance less than r are recorded; otherwise, no search is performed.

[0050] The step S6 is specifically as follows:

[0051] S61. Solve the normal vector and extract the covariance matrix of the Gaussian distribution of all points within the sphere with a radius of R around the target point. If the point cloud surface within the target point area is well defined, they can be approximated as a plane, and only one eigenvalue of the covariance eigenvalue is much smaller than the other two. Then the eigenvector corresponding to the smallest eigenvalue is considered to be the normal vector of the point cloud plane in this point area.

[0052] The mathematical formula is as follows: Let s i For any point in the point cloud S, calculate V around it as the center i The covariance formula of the points is:

[0053]

[0054] Among them, μ i is the average value of the point cloud.

[0055] S62. Find matching point pairs. The matching point pair rules are as follows:

[0056] a) At least one of the two points in a point pair does not have a well-defined normal vector.

[0057] b) Threshold of distance between matching points:

[0058] c) The difference in curvature between matching points: |logσ i -logσ j |>ε d .

[0059] d) If the angle between the normal vectors of two points exceeds the threshold, vector difference multiplication is used to determine the angle based on the projection.

[0060] S63. Find the coordinate transformation based on the matching pair. NICP transforms the coordinates and normal vectors of the two sets of point clouds to p'=(p,n). The transformation rules are as follows:

[0061]

[0062] Rotate and translate the points, rotate the normal vector. The error function is the sum of the error between the distance between the points and the normal vector angle.

[0063]

[0064] Where q represents the point of the reference point cloud, and the final error function is:

[0065]

[0066] in Contains the information matrix, the plane and the normal vector The plane matrix uses the inverse of the covariance, and the normal vector matrix uses the unit matrix when it is non-singular, otherwise it is as follows:

[0067]

[0068] The LMF algorithm is used for optimization and the precise registration is initially completed. According to the point cloud registration results, the parameters and error thresholds are adjusted for optimization. At this point, the point cloud registration example is completed. The advantages of this invention compared to the previous algorithms are:

[0069] 1. In the voxel filtering stage, after dividing the voxel grid, Gaussian sampling is used to further enhance the integrity of the point cloud structure while downsampling.

[0070] 2. In the 3DSC stage, the 3DSC algorithm is used to create feature descriptors, which has better effects and higher accuracy than PFH, FPFH, etc.

[0071] 3. In the fine registration stage, an improved version of the ICP algorithm, NICP, was used, and the normal vector evaluation criteria were added. In actual measurements, the registration effect was improved without a significant change in the calculation time.

[0072] 4. The process of precise point cloud registration uses KD-Tree nearest neighbor search to improve the processing speed of point cloud data, speed up point cloud registration, and improve computing power. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] Figure 1 is a flow chart of the method of the present invention; DETAILED DESCRIPTION

[0074] The present invention is described in detail below in conjunction with specific embodiments.

[0075] A method for accelerating point cloud registration based on 3DSC and improved ICP algorithm, the specific steps include:

[0076] S1. Determine the source point cloud and the target point cloud to be registered. In order to ensure that the subsequent 3DSC algorithm can generate different feature descriptors, it is necessary to select an appropriate voxel grid range [v x v y v z ], are the lengths along the x, y, and z axes, respectively. Based on the above division lengths, the voxel grid is divided, and Gaussian sampling is used to generate uniform points based on the voxel center points, which greatly reduces the amount of point cloud data while ensuring the integrity of the point cloud density and point cloud structure, and improving the point cloud processing speed.

[0077] The specific steps are:

[0078] S11. Divide the point cloud data into a regular three-dimensional voxel grid. Each voxel grid covers a fixed spatial range of [v1 v2 v3]. v1, v2, v3 are the voxel lengths along x, y, and z, respectively.

[0079] S12. For each voxel grid, select one point as a representative point. A common selection method is to select the center point in the grid as the representative point.

[0080] S13. Based on the obtained representative point set, a set of sampling points is generated using a Gaussian sampling method. The number and density of the sampling points can be controlled according to the variance parameter of the Gaussian distribution.

[0081] S14. For each sampling point, select the original point closest to it as the final downsampling result.

[0082] S2. According to the 3DSC algorithm, the downsampled point cloud is subjected to feature descriptor extraction. The extraction method is as follows: for the number of point clouds S, s is any point in it, and n is the normal vector l of point s. With s as the center of the sphere and l as the due north direction of the sphere, a local spherical space is established. According to the radius r, azimuth angle α and elevation angle θ, J+1, K+1 and L+1 segments are taken respectively. The histogram data representation of J*K*L point clouds is obtained. The radius segment equation is as follows:

[0083]

[0084] where r min and r max is the boundary of the spherical space. In addition, an initial weight needs to be assigned to each spherical space:

[0085]

[0086] Where V(j,k,l) ​​is the volume of the corresponding space, s i is the density of the corresponding space. It is estimated based on the number of points and the radius. i ) values ​​form a feature vector This vector contains the shape context features around the query point.

[0087]

[0088] Among them, k = {1, 2, ..., n}, n = J × K × L. So far, the feature descriptor construction is completed.

[0089] Then, feature descriptor matching is started, using the nearest neighbor matching strategy. Let the source point cloud descriptor be S = {s1, s2, s3}, the target point cloud descriptor be T = {t1, t2, t3}, and each descriptor be a vector. The Euclidean distance of the descriptor is calculated as follows:

[0090]

[0091] Calculate the Euclidean distance and find the minimum value, which is the best matching descriptor. Repeat the process to find the nearest neighbor feature descriptor of the source point cloud and the target point cloud.

[0092] S3. According to the characteristics of the point cloud, the parameters such as the nearest neighbor search radius and point cloud density are optimized to obtain a better matrix estimation.

[0093] S4. Pass the rough registration matrix estimate to the NICP algorithm, initialize the initial homogeneous transformation matrix of the NICP algorithm, and improve the registration accuracy.

[0094] Specifically:

[0095] S41, first replace the NICP initial homogeneous matrix according to the coarse registration transformation, and set any point M(x m ,y m ,z m ), any point N(x n ,y n ,z n ), and M and N belong to the set of point clouds. The matrix transformation of the two point clouds is further solved by the NICP algorithm using the secondary transformation matrix parameters. The initial matrix transformation is as follows:

[0096]

[0097] Among them, t x ,t y ,t zis the translation variable along the three coordinate axes, w is the scaling factor, the default is 1, α, β, λ are the rotation angles around the x, y, and z axes respectively.

[0098] S42, passing the matrix parameters obtained by the 3DSC coarse registration to the NICP algorithm as update values ​​of the initial homogeneous transformation matrix.

[0099] S43, set s i For any point in the point cloud S, calculate V around it as the center i The covariance formula of the points is:

[0100]

[0101] Among them, μ i is the average value of the point cloud. Since the covariance matrix is ​​symmetric, SVD decomposition is performed to obtain:

[0102]

[0103] The characteristic roots are λ1, λ2, λ3, which are sorted from small to large. The smallest characteristic root is the direction of the normal vector. The curvature is σ i =λ1 / (λ1+λ2+λ3). If the point cloud is flat, the covariance matrix is ​​close to singular, and the minimum eigenvalue is recorded as ε.

[0104] NICP transforms the coordinates and normal vectors of the two sets of point clouds to p'=(p,n). The transformation rules are as follows:

[0105]

[0106] Rotate and translate the points, rotate the normal vector. The error function is the sum of the error between the distance between the points and the normal vector angle.

[0107]

[0108] Where q represents the point of the reference point cloud, and the final error function is:

[0109]

[0110] in Contains the information matrix, the plane Ω ij s and the normal vector Ω ij n The plane matrix uses the inverse of the covariance, and the normal vector matrix uses the unit matrix when it is non-singular, otherwise it is as follows,

[0111]

[0112] The LMF algorithm is used for optimization solution.

[0113] S5. Point cloud has three dimensions: x, y, and z. Kd-tree can be used for multi-dimensional search. This algorithm uses radius search. First, construct a kd-tree, perform spatial separation on the x, y, and z axes, and find the middle node in the sorting order. The node with a smaller value than the middle position is the left child node, and the node with a larger value than the middle position is the right child node. Execute in a loop until the number of points contained in the child node is less than the preset threshold (leafsize). Search down to the leaf node where the query point is located, calculate the distance between all points in the leaf node and the query point, and record the points with a distance less than the query radius r; calculate the distance between the query point and the current separation axis, and trace back upward. If r> the distance from the query point to the search plane, search the subspace and record the points with a distance less than r; otherwise, do not search. Use kd-tree to speed up the point cloud search and reduce the calculation time.

[0114] S6. Then combine the ICP improved algorithm, namely the NICP precise registration algorithm, to perform matrix optimization. The steps of the NICP algorithm are as follows:

[0115] S61. Solve the normal vector and extract the covariance matrix of the Gaussian distribution of all points within the sphere with radius R around the target point. If the point cloud surface in the target point area is well defined, they can be approximated as a plane, and only one eigenvalue of the covariance is much smaller than the other two. Then the eigenvector corresponding to the smallest eigenvalue is considered to be the normal vector of the point cloud plane in this point area.

[0116] The mathematical formula is as follows: Let s i For any point in the point cloud S, calculate V around it as the center i The covariance formula of the points is:

[0117]

[0118] Among them, μ i is the average value of the point cloud.

[0119] S62. Find matching point pairs. The matching point pair rules are as follows:

[0120] a) At least one of the two points in a point pair does not have a well-defined normal vector.

[0121] b) Threshold of distance between matching points:

[0122] c) The difference in curvature between matching points: |logσ i -logσ j |>ε d .

[0123] d) If the angle between the normal vectors of two points exceeds the threshold, vector difference multiplication is used to determine the angle based on the projection.

[0124] S63. Find the coordinate transformation based on the matching pair. NICP transforms the coordinates and normal vectors of the two sets of point clouds to p'=(p,n). The transformation rules are as follows:

[0125]

[0126] Rotate and translate the points, rotate the normal vector. The error function is the sum of the error between the distance between the points and the normal vector angle.

[0127]

[0128] Where q represents the point of the reference point cloud, and the final error function is:

[0129]

[0130] in Contains the information matrix, the plane and the normal vector The plane matrix uses the inverse of the covariance, and the normal vector matrix uses the unit matrix when it is non-singular, otherwise it is as follows:

[0131]

[0132] The LMF algorithm is used for optimization and the precise registration is initially completed. According to the point cloud registration results, the parameters and error thresholds are adjusted for optimization. At this point, the point cloud registration example is completed.

[0133] It should be understood that those skilled in the art can make improvements or changes based on the above description, and all these improvements and changes should fall within the scope of protection of the appended claims of the present invention.

Claims

1. A method for accelerating point cloud registration based on 3DSC and improved ICP algorithm, characterized in that: The steps include: S1. By combining voxel filtering and Gaussian sampling, the amount of point cloud data is reduced, while ensuring the integrity of the point cloud structure and improving the point cloud processing speed; S2, using 3DSC algorithm to extract feature descriptors and match feature descriptors; S3. According to the characteristics of the point cloud, the parameters such as the nearest neighbor search radius and point cloud density are optimized to obtain a better matrix estimation; S4, passing the rough registration matrix estimate to the NICP algorithm, initializing the initial homogeneous transformation matrix of the NICP algorithm, and improving the registration accuracy; S5. In the NICP algorithm, KD-Tree is used to accelerate the nearest neighbor search and point cloud matching process and reduce the calculation time; S6. Combine the improved ICP algorithm, namely the NICP precise registration algorithm, to perform matrix optimization.

2. The method according to claim 1, characterized in that The S1 step is specifically as follows: S11, divide the point cloud data into a regular three-dimensional voxel grid; each voxel grid covers a fixed spatial range, the range is [v1 v2 v3]; v1, v2, v3 are the voxel lengths along x, y, and z respectively; S12. For each voxel grid, select one point as a representative point; a common selection method is to select the center point in the grid as the representative point; S13. Based on the obtained representative point set, a set of sampling points is generated using a Gaussian sampling method; the number and density of the sampling points can be controlled according to a variance parameter of the Gaussian distribution; S14. For each sampling point, select the original point closest to it as the final downsampling result.

3. The method according to claim 1, characterized in that: The step S2 is specifically as follows: S21. For the number of point clouds S, s is any point in it, n is the normal vector l of point s; take s as the center of the sphere and l as the due north direction of the sphere to establish a local spherical space; take J+1, K+1 and L+1 segments according to the radius r, azimuth angle α and elevation angle θ respectively; obtain the histogram data representation of J*K*L point clouds; the radius segmentation equation is as follows: where r min and r max is the boundary of the spherical space; in addition, an initial weight needs to be assigned to each spherical space: Where V(j,k,l) ​​is the volume of the corresponding space, s i is the density of the corresponding space; Estimated based on the number of points and radius; merge w(s i ) values ​​form a feature vector This vector contains the shape context features around the query point; Where, k = {1, 2, ..., n}, n = J × K × L; S22. The source point cloud and the target point cloud are matched using local space descriptors. The nearest neighbor matching strategy is adopted to calculate the Euclidean distance between the source point cloud and the feature descriptor target point cloud feature descriptor and compare them. The one with the smallest distance is the feature descriptor matching object. Repeat the execution to complete the feature descriptor matching.

4. The method according to claim 1, characterized in that: The step S4 is specifically as follows: S41, first replace the NICP initial homogeneous matrix according to the coarse registration transformation, and set any point M(x m ,y m ,z m ), any point N(x n ,y n ,z n ), and M and N belong to a set of point clouds. The matrix transformation of the two point clouds is further solved by the NICP algorithm using the secondary transformation matrix parameters; the initial matrix transformation is as follows: Among them, t x ,t y ,t z is the translation variable along the three coordinate axes, w is the scaling factor, the default is 1, α, β, λ are the rotation angles around the x, y, and z axes respectively; S42, passing the matrix parameters obtained by 3DSC coarse registration to the NICP algorithm as update values ​​of the initial homogeneous transformation matrix; S43, set s i For any point in the point cloud S, calculate V around it as the center i The covariance formula of the points is: Among them, μ i is the average value of the point cloud; since the covariance matrix is ​​symmetric, SVD decomposition is performed to obtain: The characteristic roots are λ1, λ2, λ3, which are sorted from small to large. The smallest characteristic root is the direction of the normal vector. The curvature is σ i =λ1 / (λ1+λ2+λ3); If the point cloud is flat, the covariance matrix is ​​close to singular, and the minimum eigenvalue is recorded as ε; NICP transforms the coordinates and normal vectors of the two sets of point clouds to p'=(p,n). The transformation rules are as follows: Rotate and translate the points, and rotate the normal vector; the error function is the sum of the errors of the distance between the points and the normal vector angle; Where q represents the point of the reference point cloud, and the final error function is: in Contains the information matrix, the plane and the normal vector The plane matrix uses the inverse of the covariance, and the normal vector matrix uses the unit matrix when it is non-singular, otherwise it is as follows, The LMF algorithm is used for optimization solution.

5. The method according to claim 1, characterized in that The step S5 is specifically as follows: Point cloud has three dimensions: x, y, and z. Kd-tree can be used for multi-dimensional search. Using radius search, first build a kd-tree, spatially separate the x, y, and z axes, find the middle node in the sorting order, and the node smaller than the middle position is the left child node, and the node larger than the middle position is the right child node; loop execution until the number of points contained in the child node is less than the preset threshold leafsize, search down to the leaf node where the query point is located, calculate the distance between all points in the leaf node and the query point, and record the points whose distance is less than the query radius r; Calculate the distance between the query point and the current separation axis, backtrack upward, if r> the distance from the query point to the search plane, search the subspace and record the points with a distance less than r; otherwise, do not search.

6. The method according to claim 1, characterized in that The step S6 is specifically as follows: S61. Solve the normal vector and extract the covariance matrix of the Gaussian distribution of all points within the sphere with a radius of R around the target point. If the point cloud surface within the target point area is well defined, they can be approximated as a plane, and only one eigenvalue of the covariance eigenvalue is much smaller than the other two. Then the eigenvector corresponding to the smallest eigenvalue is considered to be the normal vector of the point cloud plane in this point area. Assume i For any point in the point cloud S, calculate V around it as the center i The covariance formula of the points is: Among them, μ i is the average value of the point cloud; S62, find matching point pairs, the matching point pair rules are as follows: a) At least one of the two points in the point pair does not have a well-defined normal vector; b) Threshold of distance between matching points: c) The difference in curvature between matching points: |logσ i -logσ j |>ε d ; d) If the angle between the normal vectors of two points exceeds the threshold, vector difference multiplication is used to determine the angle based on the projection. S63. According to the matching pairs, find the coordinate transformation; NICP transforms the coordinates and normal vectors of the two sets of point clouds to p'=(p,n), and the transformation rules are as follows: Rotate and translate the points, and rotate the normal vector; the error function is the sum of the errors of the distance between the points and the normal vector angle; Where q represents the point of the reference point cloud, and the final error function is: in Contains the information matrix, the plane and the normal vector The plane matrix uses the inverse of the covariance, and the normal vector matrix uses the unit matrix when it is non-singular, otherwise it is as follows: The LMF algorithm is used for optimization and solution, and the precise registration is initially completed; according to the point cloud registration results, the parameters and error thresholds are adjusted for optimization.

Citation Information

Patent Citations

  • Point cloud registration method based on improved ISS-3DSC feature in combination with ICP

    CN117274339A

  • Strip mine three-dimensional map construction and cross-around parallel path planning method

    CN118274816A

Cited By

  • Neural network-based search radius adaptive ICP registration method

    CN121120719A