A point cloud registration method of dynamically updating a matching pair

Abstract

The application relates to the technical field of general image data processing or generation, and particularly relates to a point cloud registration method for dynamically updating matching pairs. n The spatial distance s n between p n and q n ; S400, the feature distance f n between p n and q n is obtained by traversing C; S500, an initial value d0 of the spatial distance is obtained according to S, and an initial value theta0 of the feature distance is obtained according to F; S600, in the process of the kth iteration, if the corresponding spatial distance of C k,m is less than or equal to d k th and the corresponding feature distance of C k,m is less than or equal to theta k th , C k,m is added to A k ; S700, if the target function value corresponding to the kth iteration is less than or equal to a target function threshold value, the iteration is stopped. The application improves the efficiency and accuracy of registration.

Description

Technical Field

[0001] This invention relates to the field of general image data processing or generation technology, and in particular to a point cloud registration method for dynamically updating matching pairs. Background Technology

[0002] Point cloud registration algorithms based on Iterative Closest Point (ICP) are simple and effective, making them the most widely used registration algorithms. Existing technologies have also improved upon these by proposing other ICP-like algorithms, such as Point-Plane ICP, Generalized ICP, and Normal ICP. However, the classic ICP, Point-Plane ICP, and Generalized ICP algorithms all use a fixed distance threshold and can only remove erroneous matches at a fixed scale during the iteration process, and are very sensitive to the initial position. The Normal ICP algorithm, by introducing normal vector information, adds matching constraints and achieves good results. However, since the normal vector information between neighboring matching pairs is not necessarily the same, it is still significantly affected by the initial position.

[0003] Incorrect matching pairs slow down the convergence speed and reduce the accuracy of registration algorithms. In the ICP framework, registration is an iterative approximation process. Using a small fixed threshold may affect the search results of correct matches at non-ideal initial positions, while using a large fixed threshold will be ineffective in removing incorrect matches at local positions. Furthermore, at non-ideal initial positions, closer matching pairs are not necessarily correct. How to improve the convergence speed and accuracy of registration algorithms is an urgent technical problem to be solved. Summary of the Invention

[0004] The purpose of this invention is to provide a point cloud registration method that dynamically updates matching pairs, thereby improving the convergence speed and accuracy of the registration algorithm.

[0005] According to the present invention, a method for dynamically updating point cloud registration of matching pairs is provided, comprising the following steps:

[0006] S100, acquire the target point cloud Q and the source point cloud P.

[0007] S200, obtain a matching pair set C based on the target point cloud Q and the source point cloud P, C = {(p1,q1),(p2,q2),...,(p...}} N ,q N )},(p n ,q n p represents the nth matching pair in C, where n ranges from 1 to N, and N is the number of matching pairs in C; n Let q be a point in P in the nth matching pair in C.n Let Q be the point in the nth matching pair in C.

[0008] S300, iterate through C and get p n With q n spatial distance s n and s n Append to S, and initialize S to null.

[0009] S400, iterate through C and get p n With q n Feature distance f n and f n Add to F, and initialize F to null.

[0010] S500, obtain the initial value of spatial distance d0 based on S and the initial value of feature distance θ0 based on F; d0 is the average value of spatial distance in S or obtained based on the average value of spatial distance in S; θ0 is the average value of feature distance in F or obtained based on the average value of feature distance in F.

[0011] S600, during the k-th iteration, traverse the set of matching pairs C corresponding to the k-th iteration. k If C k,m The corresponding spatial distance is less than or equal to d k th And C k,m The corresponding feature distance is less than or equal to θ k th Then C k,m Add to A k A k d is initialized to null; k th Let d be the spatial distance threshold corresponding to the k-th iteration. k th =d0×(1+(m) k d -m k-1 d ) / m k-1 d ), θ k th Let θ be the spatial distance threshold corresponding to the k-th iteration. k th =θ0×(1+(m) k θ -m k-1 θ ) / m k-1 θ ), m k d Let m be the average spatial distance of the matching pairs corresponding to the k-th iteration.k-1 d m is the average spatial distance of the matching pairs corresponding to the (k-1)th iteration. k θ m is the average feature distance of the matching pairs corresponding to the k-th iteration. k-1 d m is the average feature distance of the matching pairs corresponding to the (k-1)th iteration. 0 d Let m be the average spatial distance in S. 0 θ C is the average of the feature distances in F. k,m C k The m-th matching pair in the array, where m ranges from 1 to M, and M is C. k The number of matching pairs.

[0012] S700, according to A k Obtain the transformation matrix (R) corresponding to the k-th iteration. k ,t k If according to the transformation matrix (R) k ,t k ) and A k If the objective function value obtained in the k-th iteration is less than or equal to the objective function threshold, then the iteration stops; R k Let t be the rotation matrix corresponding to the k-th iteration. k Let be the translation matrix corresponding to the k-th iteration.

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

[0014] This invention introduces spatial distance and feature distance information between matching pairs in the matching pair search step of the registration iteration of ICP-type algorithms. By utilizing the average change of point pair spatial distance and feature distance during the iteration process, the spatial distance threshold and feature distance threshold are dynamically updated (the threshold is relaxed or tightened according to the search results of two adjacent iterations) to eliminate incorrect matching pairs. By increasing the proportion of correct matching pairs during the iteration process, the registration efficiency and accuracy are improved. Attached Figure Description

[0015] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0016] Figure 1 This is a flowchart of a point cloud registration method for dynamically updating matching pairs provided in an embodiment of the present invention. Detailed Implementation

[0017] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0018] According to the present invention, a method for dynamically updating point cloud registration of matching pairs is provided, such as... Figure 1 As shown, it includes the following steps:

[0019] S100, acquire the target point cloud Q and the source point cloud P.

[0020] S200, obtain a matching pair set C based on the target point cloud Q and the source point cloud P, C = {(p1,q1),(p2,q2),...,(p...}} N ,q N )},(p n ,q n p represents the nth matching pair in C, where n ranges from 1 to N, and N is the number of matching pairs in C; n Let q be a point in P in the nth matching pair in C. n Let Q be the point in the nth matching pair in C.

[0021] Optionally, between S100 and S200, a step of preprocessing the target point cloud Q and the source point cloud P is further included, wherein the preprocessing includes filtering.

[0022] Those skilled in the art will understand that any method for obtaining matching pairs in the prior art falls within the protection scope of this invention.

[0023] Optionally, obtaining the matching pair set C based on the target point cloud Q and the source point cloud P includes:

[0024] S210, obtain the KD-tree data structure of the target point cloud Q.

[0025] S220, perform nearest neighbor search on each point of the source point cloud P to obtain the set of matching pairs C.

[0026] Those skilled in the art will know that the process of constructing the KD-tree data structure corresponding to the point cloud and the process of performing nearest neighbor search are existing technologies and will not be described in detail here.

[0027] S300, iterate through C and get p n With q n spatial distance s n and s nAppend to S, and initialize S to null.

[0028] Optional, p n With q n spatial distance s n For p n With q n The Euclidean distance. Those skilled in the art will understand that any existing method for obtaining spatial distance falls within the protection scope of this invention.

[0029] S400, iterate through C and get p n With q n Feature distance f n and f n Add to F, and initialize F to null.

[0030] Optionally, the feature distance can be the FPFH feature distance, the USC feature distance, or the SHOT feature distance. Those skilled in the art will understand that any method for obtaining the feature distance in the prior art falls within the protection scope of this invention.

[0031] S500, obtain the initial value of spatial distance d0 based on S and the initial value of feature distance θ0 based on F; d0 is the average value of spatial distance in S or obtained based on the average value of spatial distance in S; θ0 is the average value of feature distance in F or obtained based on the average value of feature distance in F.

[0032] As one embodiment, in S100, the source point cloud P is a point cloud that has undergone initial transformation, that is, a point cloud obtained by transforming the initial point cloud B using the initial transformation matrix obtained from coarse registration; then d0 is the average spatial distance in S, and θ0 is the average feature distance in F. It should be understood that the initial point cloud is a point cloud that needs to be registered with the target point cloud, and has not yet undergone coarse registration.

[0033] In another embodiment, in S100, the source point cloud P is a point cloud that has not undergone initial transformation, i.e., the initial point cloud B; then d0 is obtained based on the average spatial distance in S, and θ0 is obtained based on the average feature distance in F. Optionally, d0 = m d +σ 2 d θ0=m θ +σ 2 θ m d Let σ be the average spatial distance in S. d Let m be the standard deviation of the spatial distance in S. θ σ is the average of the feature distances in F. θ Let F be the standard deviation of the feature distances. The method for obtaining the standard deviation is existing technology and will not be described in detail here.

[0034] S600, during the k-th iteration, traverse the set of matching pairs C corresponding to the k-th iteration. k If C k,m The corresponding spatial distance is less than or equal to d k th And C k,m The corresponding feature distance is less than or equal to θ k th Then C k,m Add to A k A k d is initialized to null; k th Let d be the spatial distance threshold corresponding to the k-th iteration. k th =d0×(1+(m) k d -m k-1 d ) / m k-1 d ), θ k th Let θ be the spatial distance threshold corresponding to the k-th iteration. k th =θ0×(1+(m) k θ -m k-1 θ ) / m k-1 θ ), m k d Let m be the average spatial distance of the matching pairs corresponding to the k-th iteration. k-1 d m is the average spatial distance of the matching pairs corresponding to the (k-1)th iteration. k θ m is the average feature distance of the matching pairs corresponding to the k-th iteration. k-1 d m is the average feature distance of the matching pairs corresponding to the (k-1)th iteration. 0 d Let m be the average spatial distance in S. 0 θ C is the average of the feature distances in F. k,m C k The m-th matching pair in the array, where m ranges from 1 to M, and M is C. k The number of matching pairs.

[0035] According to the present invention, the set C of matching pairs corresponding to the k-th iteration is obtained. k The process is similar to obtaining C, except that the set of matching pairs C corresponding to the k-th iteration is...k It is based on the target point cloud Q and the transformation matrix (R) corresponding to the (k-1)th iteration. k-1 ,t k-1 The point cloud obtained by transforming the initial matrix B is C, which is obtained from the target point cloud Q and the source point cloud P; R k-1 Let t be the rotation matrix corresponding to the (k-1)th iteration. k-1 This is the translation matrix corresponding to the (k-1)th iteration.

[0036] S700, according to A k Obtain the transformation matrix (R) corresponding to the k-th iteration. k ,t k If according to the transformation matrix (R) k ,t k ) and A k If the objective function value obtained in the k-th iteration is less than or equal to the objective function threshold, then the iteration stops; R k Let t be the rotation matrix corresponding to the k-th iteration. k Let be the translation matrix corresponding to the k-th iteration.

[0037] According to the present invention, if based on the transformation matrix (R) k ,t k ) and A k If the objective function value obtained for the k-th iteration is greater than the objective function threshold and other preset stopping iteration conditions are not met, then the iteration continues. Optionally, the other preset stopping iteration conditions include k being greater than a preset number of iterations threshold.

[0038] Those skilled in the art will understand that any method in the prior art for obtaining the transformation matrix based on a set of matching pairs falls within the protection scope of this invention.

[0039] Optionally, the objective function value corresponding to the k-th iteration. H k For A k The number of matching pairs in q k i For A k In the i-th matching pair, the point p in the target point cloud k i For A k The points in the initial point cloud B of the i-th matching pair.

[0040] Optionally, the threshold of the objective function can be preset by the user based on experience.

[0041] This invention introduces spatial distance and feature distance information between matching pairs into the matching pair search step of the registration iteration in ICP-type algorithms. By utilizing the average change in the spatial distance and feature distance between point pairs during the iteration process, the spatial distance threshold and feature distance threshold are dynamically updated (by relaxing or tightening the threshold based on the search results of two adjacent iterations) to eliminate erroneous matching pairs. This improves registration efficiency and accuracy by increasing the proportion of correct matching pairs during the iteration process. The point cloud registration method of dynamically updating matching pairs in this invention can be integrated into existing ICP-type algorithms, making it widely applicable.

[0042] While specific embodiments of the invention have been described in detail by way of example, those skilled in the art should understand that the examples are for illustrative purposes only and not intended to limit the scope of the invention. It should also be understood that various modifications can be made to the embodiments without departing from the scope and spirit of the invention. The scope of the invention is defined by the appended claims.

Claims

1. A method of point cloud registration for dynamic updating of matching pairs, characterized in that, The method comprises the following steps: S100, obtaining a target point cloud Q and a source point cloud P; S200, obtain a matching pair set C based on the target point cloud Q and the source point cloud P, C={(p1,q1),(p2,q2),…,(p...}...} N ,q N )},(p n ,q n p represents the nth matching pair in C, where n ranges from 1 to N, and N is the number of matching pairs in C; n Let q be a point in P in the nth matching pair in C. n Let Q be the point in the nth matching pair in C; S300, traverse C, get p n the spatial distance s n of q n and add s n to S, S is initialized as Null; S400, traverse C, get p n with q n characteristic distance f n of p n and add f n to F, F is initialized to Null; S500, obtaining a spatial distance initial value d0 according to S and obtaining a feature distance initial value θ0 according to F; d0 is an average value of the spatial distances in S or is obtained according to the average value of the spatial distances in S; θ0 is an average value of the feature distances in F or is obtained according to the average value of the feature distances in F; S600, in the process of the kth iteration, traversing the matching pair set C corresponding to the kth iteration k , if C k,m corresponding spatial distance is less than or equal to d k th , and C k,m corresponding feature distance is less than or equal to θ k th , then C k,m is added to A k , and A k is initialized to Null; d k th is the spatial distance threshold value corresponding to the kth iteration, d k th = d0 × (1 + (m k d -m k-1 d ) / m k-1 d , θ k th is the feature distance threshold value corresponding to the kth iteration, θ k th = θ0 × (1 + (m k θ -m k-1 θ ) / m k-1 θ , m k d is the spatial distance average of the matching pairs corresponding to the kth iteration, m k-1 d is the spatial distance average of the matching pairs corresponding to the (k-1)th iteration, m k θ is the feature distance average of the matching pairs corresponding to the kth iteration, m k-1 d is the feature distance average of the matching pairs corresponding to the (k-1)th iteration, m 0 d is the average of the spatial distances in S, m 0 θ is the average of the feature distances in F, C k,m is the mth matching pair in C k , m ranges from 1 to M, and M is the number of matching pairs in C k . S700, according to A k Obtain the transformation matrix (R) corresponding to the k-th iteration. k ,t k If according to the transformation matrix (R) k ,t k ) and A k If the objective function value obtained in the k-th iteration is less than or equal to the objective function threshold, then the iteration stops; R k Let t be the rotation matrix corresponding to the k-th iteration. k Let be the translation matrix corresponding to the k-th iteration.

2. The method of claim 1, wherein, In S100, the source point cloud P is a point cloud that has undergone initial transformation; in S500, d0 is the average value of the spatial distances in S, and θ0 is the average value of the feature distances in F.

3. The method of claim 1, wherein, In S100, the source point cloud P is a point cloud that has not undergone initial transformation; in S500, d0 is obtained according to the average value of the spatial distances in S, and θ0 is obtained according to the average value of the feature distances in F.

4. The method of claim 3, wherein, d0=m d +𝜎 2 d , θ0=m θ +𝜎 2 θ , m d is the mean of the spatial distances in S, 𝜎 d is the standard deviation of the spatial distances in S, m θ is the mean of the feature distances in F, 𝜎 θ is the standard deviation of the feature distances in F.

5. The method of claim 1, wherein, The feature distance is an FPFH feature distance, an USC feature distance or a SHOT feature distance.

6. The method of claim 1, wherein, S700 further includes: if the target function value corresponding to the kth iteration obtained according to the transformation matrix (R k , t k ) and A k is greater than the target function threshold value, and other preset stopping iteration conditions are not met, iteration is continued.

7. The method of claim 6, wherein, The other preset stopping iteration condition comprises that k is greater than a preset number threshold.

8. The method of claim 1, wherein, Between S100 and S200, a step of pre-processing the target point cloud Q and the source point cloud P is further included, and the pre-processing comprises filtering processing.

9. The method of claim 1, wherein, In S200, obtaining a matching pair set C according to the target point cloud Q and the source point cloud P comprises: S210, obtaining a KD-tree data structure of the target point cloud Q; S220, performing nearest neighbor search on the source point cloud P point by point to obtain the matching pair set C.

Images