Point cloud registration method based on neighborhood normal vector and curvature
Through feature extraction based on neighborhood normal vectors and curvature and FPFH feature description, combined with bidirectional k-dimensional tree to improve the ICP algorithm, the problems of low point cloud registration efficiency and poor accuracy in the prior art are solved, and the noise and large angles are achieved Efficient and accurate point cloud registration in the case.
Patent Information
- Application Number
- CN202411097504.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-12
- Publication Date
- 2024-11-22
- Estimated Expiration
- Not applicable · inactive patent
AI Technical Summary
The existing three-dimensional point cloud registration algorithm has problems such as low efficiency, poor accuracy and insufficient robustness in noise interference, incomplete data and large corners, especially in the extraction, description and matching stages of feature point, and it is difficult to resist noise interference and The impact of data changes.
Using a point cloud registration method based on neighborhood normal vector and curvature, the initial feature points are extracted by constructing the number of neighboring points constraints and the internal product mean of normal vectors, and the final feature point set is formed by combining curvature feature parameters, and FPFH feature description and bidirectional minimum The European-style distance ratio is matched, the mismatched point pairs are eliminated, and finally a two-way k-dimensional tree is constructed to improve the ICP algorithm for precise registration.
It improves the efficiency, accuracy and robustness of point cloud registration, can accurately match feature points in the case of noise interference and incomplete data, improves the iterative computing efficiency of the ICP algorithm, and achieves high-precision point cloud registration.
Smart Images

Figure CN119006543A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of three-dimensional point cloud registration, and in particular to a point cloud registration method based on neighborhood normal vectors and curvature. Background Art
[0002] With the rapid development of 3D laser scanning technology, measurement methods and techniques are becoming increasingly advanced. The spatial coordinate information (x, y, z) obtained by measuring the surface of the object to be measured is usually called 3D point cloud data. Since point cloud data in practical applications tends to be massive, accompanied by noise interference, incomplete data, uneven distribution of point clouds, etc. Therefore, in order to ensure the accuracy and robustness of the model, it is necessary to preprocess the large-scale point cloud data before modeling. Point cloud preprocessing mainly includes denoising, filtering, registration and segmentation, among which point cloud registration is the core process, and its registration accuracy will directly affect the reconstruction effect of the subsequent solid 3D model.
[0003] The traditional ICP algorithm calculates the Euclidean distance between two point clouds to find the nearest point, establish a corresponding relationship, and solve the ideal transformation parameters of the objective function. However, the algorithm has problems such as low registration efficiency, poor accuracy, and high requirements for the initial pose of the point cloud. Therefore, relevant scholars at home and abroad have proposed a large number of improvement methods and summarized the four main steps of the ICP algorithm: 1) feature point extraction; 2) feature description; 3) feature matching and elimination of mismatched point pairs; 4) registration. Although each improved algorithm has made innovations in one or more of the above steps, some problems are still inevitable. For example, in the feature point extraction stage, most feature points can be effectively extracted based on the geometric features of the point cloud, but relying solely on a single feature cannot accurately extract feature points with similarity areas, and is easily disturbed by noise, thereby extracting a large number of non-feature points; in the feature description stage, most improved descriptors can improve the description ability of local features, but are also unable to resist the influence of noise interference and changes in data resolution; in the feature matching and elimination of mismatched point pairs stage, most feature matching point pairs can be obtained through the similarity between descriptors, but factors such as limited overlapping areas of data and noise interference will cause these matching sets to still be mixed with a large number of mismatched point pairs; in the registration stage, most improved algorithms find it difficult to strike a balance between high efficiency and high precision. Summary of the invention
[0004] In view of the shortcomings of the prior art, the present invention provides a point cloud registration method based on neighborhood normal vectors and curvature, which can effectively improve the efficiency, accuracy and robustness of point cloud registration in the presence of noise interference, incomplete data and large rotation angles.
[0005] The present invention discloses a point cloud registration method based on neighborhood normal vectors and curvature, comprising the following steps:
[0006] Step 1: Preprocess the source point cloud and target point cloud data;
[0007] Step 2: According to the density between each point, the constraint condition of the number of neighboring points is constructed for the preprocessed source point cloud and target point cloud; the initial feature points are extracted from the preprocessed point cloud according to the mean inner product of the normal vectors between the sampling point and the neighboring points; the initial feature points are extracted again through the neighborhood average curvature to form feature points; the final feature point set is formed according to the constructed curvature feature parameters;
[0008] Step 3: Perform FPFH feature description on the final feature point set, perform initial feature matching based on the ratio of the bidirectional minimum and second minimum Euclidean distances between feature vectors, and use the Euclidean distance constraint between point pairs to eliminate mismatched point pairs;
[0009] Step 4: Use the correct matching point pairs as the unit quaternion to calculate the initial values of the initial transformation parameters, perform initial registration on the source point cloud and the target point cloud, and construct a bidirectional k-dimensional tree to improve the ICP algorithm to complete accurate registration.
[0010] Furthermore, the preprocessing of the source point cloud and the target point cloud data specifically includes:
[0011] A bounding box with a length, width and height of l×w×h is established for the original point cloud P. The bounding box is divided into M cubic voxel grids by combining the point cloud density to estimate the voxel side length L. The center of gravity of all points in each voxel grid is calculated and used as the filtering point, thereby reducing the complexity of the point cloud while retaining most of the feature information of the point cloud.
[0012] Furthermore, step 2.1: extracting initial feature points from the preprocessed point cloud based on the mean inner product of the normal vectors between the sampling point and the neighboring points specifically includes:
[0013] The curvature of the neighborhood surface is defined according to the density of neighboring points in neighborhoods of different radii;
[0014] Take sampling point p i The neighboring points and sampling points p in the search space with different radii are the center of the circle. i The point with the closest Euclidean distance;
[0015] For the preprocessed source point cloud P = {p i |p i ∈R 3 ,i=1,2,3,…,n} any point p i (x i ,y i ,z i ) and its A neighboring points in the neighborhood are fitted by the least squares local plane to obtain the plane equation F, which is as follows:
[0016]
[0017] Among them, F(n F ,d) is the plane equation, n F is the normal vector of plane F, d is the distance from plane F to the origin, p i is the sampling point;
[0018] For any point p in the preprocessed source point cloud P i (x i ,y i ,z i ) and its A neighboring points in the neighborhood construct the covariance matrix C:
[0019]
[0020] in, p i is the sampling point, is the sampling point p i The centroid of its A neighboring points;
[0021] Solving the covariance matrix C for the eigenvalues remember Minimum eigenvalue The corresponding eigenvector For p i The normal vector of point p i The mean of the inner product of the normal vectors with its neighboring points represents the angle characteristic f of the normal vector;
[0022]
[0023] Where f∈[0,1], n i For point p i The normal vector, n ij Represents the neighboring point p ij The normal vector of point p i ,satisfy:
[0024] f(p i )=max[f(p i1 ),f(p i2 ),f(p i3 ),...,f(p ik )]
[0025] Among them, f(p i ) is the sampling point p i The characteristic degree of point p i As feature points,
[0026] f(p i1 ),f(p i2 ),f(pi3 ),...,f(p ik ) is point p i The feature degree of the k neighboring points is used to extract the source point cloud P and the target point cloud Q as the initial feature point set P t and Q t ;
[0027] Step 2.2: The method of performing secondary extraction of initial feature points through neighborhood average curvature to form feature points also includes:
[0028] For the initial feature point set P t For any point p, the surface is fitted by the least squares method to obtain the quadratic surface equation r(u,v) of the neighborhood at point p, and the normal vector n at point p is obtained. P , derive the first basic quantity E, F, G and the second basic quantity L, M, N, and thus calculate the Gaussian curvature K and the mean curvature H of the neighborhood at point p on the surface;
[0029]
[0030] Among them, r u 、r v The first-order partial derivative of the quadratic surface, r uu 、r uv 、r vv is the second-order partial derivative of the quadratic surface, E = r u ·r u ,F=r u ·r v ,G=r v ·r v , L = n P ·r uu ,M=n P ·r uv ,N=n P ·r vv ;
[0031] Calculate the principal curvatures k1 and k2 at point p of the surface using the Gaussian curvature K and the mean curvature H;
[0032]
[0033] The mean curvature H of the neighborhood at point p is equal to the point curvature H i For comparison, if H i <H, remove the point; otherwise, if H i >H, retain points with curvature greater than the neighborhood average as feature point set P ω and Q ω .
[0034] Step 2.3: The method of constructing curvature feature parameters to form a final feature point set also includes:
[0035] In order to remove the estimated neighborhood curvature resulting in the feature point set P ω and Q ω Non-feature points with similar curvature, for P ω , Q ω The point with the largest curvature change in the neighborhood is extracted as the feature point, point p i The curvature characteristic parameter is:
[0036]
[0037] Among them, in the formula, k1(p i )、k2(p i ) represent the point p i Maximum principal curvature, minimum principal curvature;
[0038] Set point p i The condition for whether it is a feature point is:
[0039]
[0040] in, For point p i The curvature characteristic parameter of the adjacent point meets one of the above conditions, p i is the point with the largest curvature change in the neighborhood, and is extracted as the feature point set P γ , Q γ .
[0041] Furthermore, the method of performing FPFH feature description on the final feature point set, performing initial feature matching according to the ratio of the bidirectional minimum and second minimum Euclidean distances between feature vectors, and using the Euclidean distance constraint between point pairs to eliminate mismatched point pairs also includes:
[0042] Step 3.1: FPFH feature description;
[0043] Re-determine each feature point p i The SPFH feature values of k neighboring points in its neighborhood are added with the distance weight to obtain the FPFH feature, denoted as FPFH(p i ):
[0044]
[0045] Among them, w k Represents the distance weight, which is the feature point p i The Euclidean distance between its k neighbors in the neighborhood;
[0046] Step 3.2: Correspondence estimation;
[0047] Calculate the feature point set Pγ The FPFH vector of each point in and the feature point set Q γ The Euclidean distance between the FPFH vectors of all points in ;
[0048] P γ Each point in Find Q γ Middle distance The nearest feature point and the next closest feature point And calculate the ratio r d , the calculation method is:
[0049]
[0050] Set the threshold α∈(0,1), if r d (p i ,q j )<α, it is believed and match;
[0051] Step 3.3: Eliminate mismatched point pairs;
[0052] Select the initial matching point pair set C γ Any two pairs of matching points in and Set the Euclidean distance constraint between point pairs as:
[0053]
[0054] Among them, ε=0.02, which satisfies the above formula, The matching point pairs that meet the distance constraint are
[0055] Calculate C γ Zhongyu The number of matching point pairs that meet the above conditions is k γ , and follow k γ The value will be C γ The point pairs in the array are arranged from large to small, and the threshold β is set. The number of correctly matched point pairs n m Should meet: m =β·N m , β∈(0,1), N m is the number of initial matching point pairs.
[0056] Furthermore, the method uses the correct matching point pair as the initial value of the unit quaternion to calculate the initial transformation parameter, performs initial registration on the source point cloud and the target point cloud, and constructs a bidirectional k-dimensional tree to improve the ICP algorithm to complete the precise registration, and also includes:
[0057] Step 4.1: Initial registration;
[0058] The expression of quaternion is:
[0059]
[0060] Among them, q0, q1, q2, q3 are real numbers, i, j, k are mutually orthogonal imaginary unit vectors, q0 is the real part of the quaternion, q1, q2, q3 are the imaginary parts of the quaternion, and the corresponding rotation matrix is:
[0061]
[0062] In the matching point set C m The optimal solution of the rigid body change is calculated based on , and its objective function satisfies:
[0063]
[0064] Where R is an orthogonal matrix, then R T When R=1, When it is the maximum value, the objective function f(R) is the minimum. Represent it as a quaternion and convert it into a matrix form:
[0065]
[0066] Among them, N4 is a 4-order matrix. When q T When N4q is at its maximum value, f(R) is at its maximum;
[0067] The eigenvalues obtained by solving the matrix N4 are λ1, λ2, λ3, and λ4. Note that λ1<λ2<λ3<λ4, then the eigenvector corresponding to the maximum eigenvalue λ4 is the unit quaternion Matrix form; solve for the unit quaternion After the four parameters are substituted into the quaternion expression, and the rotation matrix R is solved, the translation parameter T = [ΔXΔYΔZ] T It can be expressed as:
[0068]
[0069] Among them, [X i Y i Z i ] T is the coordinate of the center of gravity in the source point cloud coordinate system, [xy i z i ] T is the coordinate of the center of gravity in the target point cloud coordinate system;
[0070] Step 4.2: Accurate registration;
[0071] Based on the initial registration, a bidirectional k-dimensional tree is constructed to search for the closest point pair (p i ,q i ), calculate the transformation parameters R, T, and calculate the sum of squared distances between corresponding point pairs after the transformation:
[0072]
[0073] Among them, ||d k+1 -d k || represents the distance between two adjacent iterations;
[0074] Set the threshold δ, when ||d k+1 -d k When ||<δ, the iteration ends and the optimal rigid body transformation parameters R and T are obtained; otherwise, the iteration calculation ||d is repeated. k+1 -d k ||, until ||d is satisfied k+1 -d k ||<δ or reaches the maximum number of iterations ω, select ||d k+1 -d k ||The smallest transformation matrices R and T are the optimal transformation parameters.
[0075] The beneficial effects of the present invention are:
[0076] 1) Feature point extraction: The number of neighboring points is constrained by the density between the sampling point and its neighboring points within different neighborhood radii, and the mean of the inner product of the normal vector angle is used as the feature degree, and the points with larger feature degrees are selected as initial feature points; then the neighborhood curvature is calculated and compared with the point curvature, and the points with larger surface curvature are selected as feature points; finally, the points with the largest curvature change are selected as feature points through the curvature feature parameters to optimize the feature points.
[0077] 2) Feature description and feature matching: Perform FPFH feature description on the extracted feature point set, perform initial feature matching based on the ratio of the bidirectional minimum and second minimum Euclidean distances between FPFH feature vectors, and calculate the Euclidean distance constraints between the initial matching point pairs to eliminate mismatched point pairs with similar features, thereby improving the matching accuracy.
[0078] 3) Registration: Using the correct matching point pair as the initial value, the unit quaternion coordinate transformation model is used to calculate the initial transformation parameters R0 and T0, providing a good initial value for subsequent precise registration. On this basis, a bidirectional k-dimensional tree is constructed to search for corresponding point pairs, improve the iterative calculation efficiency of the ICP algorithm, and complete precise registration. BRIEF DESCRIPTION OF THE DRAWINGS
[0079] Figure 1 It is a schematic diagram of the registration process of the present invention.
[0080] Figure 2For point p i Schematic diagram of the field.
[0081] Figure 3 Schematic diagram of normal vector distribution in different regions
[0082] Figure 4 is the feature point p i Schematic diagram of the FPFH influence range of its neighboring points
[0083] Figure 5 Schematic diagram of the initial position of the vehicle-mounted laser point cloud data at different times
[0084] Figure 6 Schematic diagram of the registration results of the vehicle-mounted laser point cloud data at different times according to the present invention
[0085] Figure 7 Schematic diagram of the registration results of different algorithms for vehicle-mounted laser point cloud data at different times
[0086] Figure 8 Schematic diagram of the initial position of ground and vehicle-mounted laser point cloud data
[0087] Fig. 9 Schematic diagram of the registration result of ground and vehicle-mounted laser point cloud data in the present invention
[0088] Fig.10 Schematic diagram of the registration results of ground and vehicle-mounted laser point cloud data using different algorithms DETAILED DESCRIPTION
[0089] The specific embodiments of the present invention are further described below in conjunction with the accompanying drawings:
[0090] In order to improve the efficiency, accuracy and robustness of point cloud registration under the conditions of noise interference, incomplete data and large angle, this paper proposes a point cloud registration method based on neighborhood normal vector and curvature. The registration flow chart of this method is as follows Figure 1 As shown, the whole process is divided into the following four main steps:
[0091] Step 1: Preprocess the source point cloud and target point cloud data;
[0092] In this step, a bounding box with a length, width and height of l×w×h is established for the source point cloud P. The bounding box is divided into M cubic voxel grids by combining the point cloud density to estimate the voxel side length L. The center of gravity of all points in each voxel grid is calculated and used as the filtering point, thereby reducing the complexity of the point cloud while retaining most of the feature information of the point cloud.
[0093] Step 2: According to the density between each point, the number of neighboring points is constrained for the preprocessed source point cloud and target point cloud;
[0094] Extract initial feature points from the preprocessed point cloud based on the mean inner product of the normal vectors between the sampling point and the neighboring points;
[0095] The initial feature points are extracted twice through the neighborhood average curvature to form feature points;
[0096] The final feature point set is formed based on the constructed curvature feature parameters.
[0097] It should be noted that
[0098] According to the mean inner product of the normal vectors between the sampling point and the neighboring points, the initial feature points are extracted from the preprocessed point cloud, which specifically includes the following steps:
[0099] (1) The curvature of the neighborhood surface is defined according to the density of neighboring points in the neighborhood with different radii. If the sampling point and the surrounding neighboring points are dense, the surface undulation is large, which is the characteristic area; on the contrary, if the neighboring points are sparsely distributed, the surface is flat, which is the non-characteristic area.
[0100] (2) Taking sampling point p i The neighboring points and sampling points p in the search space with different radii are the center of the circle. i The point with the closest Euclidean distance;
[0101] The number of neighboring points within the search radius r is k i , the number of neighboring points within the search radius R is k j , and R>r,k j >k i Assume that the required number of neighboring points A is a specific value, preferably, the specific value is 10. If k i <N<k j , select R as the radius neighborhood search p i The neighboring points, i.e. k j =A, such as Figure 2 (a); if A≤k i <k j , then p i In the neighborhood of radius r, k nearest neighbors are used to search for neighboring points, that is, k i =A, such as Figure 2 (b) as shown.
[0102] (3) The preprocessed source point cloud P = {p i |p i ∈R 3 ,i=1,2,3,…,n} any point p i (x i ,y i ,z i ) and its A neighboring points in the neighborhood are fitted by the least squares local plane to obtain the plane equation F, which is as follows:
[0103]
[0104] Among them, F(n F ,d) is the plane equation, n F is the normal vector of plane F, d is the distance from plane F to the origin, p i is the sampling point.
[0105] (4) The preprocessed source point cloud P = {p i |p i ∈R 3 ,i=1,2,3,…,n} any point p i (x i ,y i ,z i ) and its A neighboring points in the neighborhood construct the covariance matrix C:
[0106]
[0107] in, p i is the sampling point, is the sampling point p i The centroid of its A neighbors.
[0108] (5) Solve the covariance matrix C to obtain the eigenvalue remember Minimum eigenvalue The corresponding eigenvector For p i The normal vector of .
[0109] (6) According to the distribution characteristics of the point cloud normal vector, define the sampling point p i The angle between the normal vectors of its neighboring points is used to describe the curvature of the neighborhood surface, and the feature area of the point cloud is extracted based on the angle between the normal vectors to calculate the point p i The mean of the inner products of the normal vectors with its neighboring points represents the angle characteristic f of the normal vector.
[0110]
[0111] Among them, f∈[0,1], n i For point p i The normal vector, n ij Represents the neighboring point p ij The normal vector of point p i , if it satisfies:
[0112] f(p i )=max[f(p i1 ),f(p i2 ),f(pi3 ),...,f(p ik )]
[0113] Among them, f(p i ) is the sampling point p i The characteristic degree.
[0114] Then point p i is a feature point, where f(p i1 ),f(p i2 ),f(p i3 ),...,f(p ik ) is point p i The characteristic degree of the k neighboring points. Figure 3 (a), p i The angle between the normal vector and its neighboring point is small, then p i The surface curvature is small, and this area is a non-feature area. i is a non-feature point; Figure 3 (b), p i The angle between the normal vector and its neighboring point is large, then p i The surface curvature is larger, and this area is a characteristic area. i As feature points. Thus, the source point cloud P = {p i |p i ∈R 3 ,i=1,2,...n} and target point cloud Q={q j |q j ∈R 3 ,j=1,2,...,m} is extracted as the initial feature point set P t ={p t1 ,p t2 ,p t3 ,...,p tn′} and Q t = {q t1 ,q t2 ,q t3 ,...,q tm′}, where m′ is the number of feature points extracted from the source point cloud after preprocessing, n′ is the number of feature points extracted from the target point cloud after preprocessing, and m′<m, n′<n.
[0115] Step 2.2: The initial feature points are extracted again through the neighborhood average curvature to form a feature point set.
[0116] Specifically, the following steps are included:
[0117] (1) For the initial feature point set P t For any point p, the surface is fitted by the least squares method to obtain the quadratic surface equation r(u,v) of the neighborhood at point p:
[0118]
[0119] (2) Obtain the normal vector n at point p through the quadratic surface r(u,v) p , and derive the first basic quantity E, F, G and the second basic quantity L, M, N, so as to calculate the Gaussian curvature K and the mean curvature H of the neighborhood at point p of the surface;
[0120]
[0121] Where Q represents the coefficient matrix. u 、r v The first-order partial derivative of the quadratic surface, r uu 、r uv 、r vv is the second-order partial derivative of the quadratic surface, E = r u ·r u ,F=r u ·r v ,G=r v ·r v , L = n p ·r uu ,M=n p ·r uv ,N=n p ·r vv
[0122] (3) Calculate the principal curvatures k1 and k2 at point p of the surface from the Gaussian curvature K and the mean curvature H;
[0123]
[0124] (4) Compare the average curvature H of the neighborhood at point p calculated by the above method with the point curvature. If H i <H, the curvature of the surface at point p is small, and the point distribution in this area is flat, then the points with curvature less than the average curvature of the neighborhood are removed as non-feature areas; on the contrary, if H i >H, the surface curvature at point p is large, and the point distribution in this area is steep, so the points with curvature greater than the average curvature of the neighborhood are retained as the feature point set P ω ={p ω1 ,p ω2 ,p ω3 ,...,p ωn″} and Q ω = {q ω1 ,q ω2 ,q ω3 ,...,q ωm″}, where m″ is the number of feature points extracted from the initial feature point set in the source point cloud, n″ is the number of feature points extracted from the initial feature point set in the target point cloud, and m″<m′, n″<n′.
[0125] Step 2.3: Based on the constructed curvature feature parameters, the final feature point set is formed.
[0126] The main purpose of this step is to eliminate the influence of estimated curvature, remove redundant data, and obtain the final feature point set. The specific steps include the following:
[0127] (1) In order to remove the feature point set P caused by the estimated neighborhood curvature ω and Q ω For non-feature points with similar curvature, construct the curvature characteristic parameter pair P ω , Q ω The point with the largest curvature change in the neighborhood is extracted as the feature point, point p i The curvature characteristic parameter is:
[0128]
[0129] Among them, in the formula, k1(p i )、k2(p i ) represents point p i Maximum principal curvature, minimum principal curvature.
[0130] (2) Set point p i The condition for whether it is a feature point is:
[0131]
[0132] in, Represents point p i The curvature characteristic parameters of the neighboring points. i is the maximum value of the curvature characteristic parameter of its neighboring points, then the neighborhood surface is convex, p i Represents a convex point in the neighborhood; if point p i is the minimum value of the curvature characteristic parameter of its neighboring points, then the neighborhood surface is concave, p i represents a concave point in the neighborhood. If one of the above conditions is met, then p i is the point with the largest curvature change in the neighborhood, and is extracted as the feature point set P γ ={p γ1 ,p γ2 ,p γ3 ,...,p γn″′}, Q γ = {q γ1 ,q γ2 ,q γ3 ,...,qγm″′}, and m″′<m″, n″′<n″′.
[0133] Step 3: Perform FPFH feature description on the final feature point set, perform initial feature matching based on the ratio of the bidirectional minimum and second minimum Euclidean distances between feature vectors, and use the Euclidean distance constraint between point pairs to eliminate mismatched point pairs.
[0134] In this step, FPFH feature description, FPFH refers to parameterizing the difference degree of normal vector feature description between neighboring points by calculating the spatial difference between the feature point and its neighboring points, and forming a 33-dimensional feature histogram, thereby obtaining the feature point p i The geometric properties of a point and its neighbors are related to each other.
[0135] The ratio of the bidirectional minimum and second minimum Euclidean distances. Two FPFH feature vector sets search each other bidirectionally for the FPFH feature vectors that are closest to them and the second closest to them. If the distance ratio between the closest and the second closest FPFH feature vectors is less than the specified parameter value, then the two points are designated as a pair of correct matching points.
[0136] The Euclidean distance constraint between point pairs is based on the fact that the ratio of the distances between any two pairs of correctly matched point pairs should be equal. The Euclidean distance between any two pairs of matched point pairs is conditionally constrained, and a threshold is set to eliminate incorrectly matched point pairs.
[0137] Step 3.1: FPFH calculates the impact range, such as Figure 4 As shown, the following steps are included:
[0138] (1) Calculate the feature point p i The SPFH features of k neighboring points in its neighborhood are denoted as SPFH(p i );
[0139] (2) Re-determine each feature point p i With its k neighboring points in the neighborhood, and adding the distance weight, the FPFH feature is obtained, denoted as FPFH(p i ):
[0140]
[0141] Among them, w k Represents the distance weight, which is the feature point p i The Euclidean distance between the k neighbors in its neighborhood.
[0142] Step 3.2: Correspondence estimation, by extracting the feature point set P γ , Q γPerform FPFH feature description and generate corresponding FPFH feature vectors, and use the bidirectional minimum and sub-minimum Euclidean distance ratio method for initial feature matching, which includes the following steps:
[0143] (1) Calculate the feature point set P γ The FPFH vector of each point in and the feature point set Q γ The Euclidean distance between the FPFH vectors of all points in ;
[0144] (2) Positive order search: for P γ Each point in Find Q γ Middle distance The nearest feature point and the next closest feature point And calculate arrive and The ratio of the Euclidean distance d :
[0145]
[0146] Set an appropriate threshold α∈(0,1), if r d (p i ,q j )<α, it is believed and Match; otherwise, indicate the point In Q γ No matching feature points were found.
[0147] (3) Reverse search: for Q γ Each point in Find P γ Middle distance The nearest feature point and the next closest feature point And calculate the ratio, if r d (p i ,q j )<α, it is believed and Match; otherwise, indicate the point In P γ No matching feature points were found.
[0148] (4) Only when P γ Points in With Q γ Points in Match, and Q γ Points in Also with P γ Points in When matching, the matching pair is retained as the initial matching point pair set C γ .
[0149] Step 3.3: Eliminate mismatched point pairs. In the initial matching point pair set C γ In the process of filtering, there are still matching point pairs with similar features. Especially in scenes with complex features, due to noise interference, data loss, and collection environment limitations, it is easier to cause mismatching between point pairs. Therefore, in order to eliminate matching point pairs with similar features, the present invention uses the Euclidean distance between point pairs to eliminate mismatching points, which includes the following steps:
[0150] (1) If they are correct matching point pairs, then any two matching point pairs and The distance between them satisfies:
[0151]
[0152] (2) Select the initial matching point pair set C γ Any two pairs of matching points in and Set the Euclidean distance constraint between point pairs as:
[0153]
[0154] Among them, ε=0.02, which satisfies the above formula, The matching point pairs that meet the distance constraint are
[0155] (3) Calculate C γ Zhongyu The number of matching point pairs that meet the above conditions is k γ , and follow k γ The value will be C γ The point pairs in the array are arranged from large to small, and the threshold β is set. The number of correctly matched point pairs n m Should meet: m =β·N m , β∈(0,1), N m is the number of initial matching point pairs. Eliminate mismatched point pairs and update the initial matching point pair set C γ , and the correct matching point pair set is
[0156] Step 4: Use the correct matching point pair as the unit quaternion to calculate the initial value of the initial transformation parameter, perform initial registration on the source point cloud and the target point cloud, and construct a bidirectional k-dimensional tree to improve the ICP algorithm to complete the precise registration; the specific steps include:
[0157] Step 4.1: Correctly match the point pair C mThe initial values of the initial transformation parameters are calculated as unit quaternions, and the source point cloud and the target point cloud are initially aligned, which includes the following steps:
[0158] (1) The expression of quaternion is:
[0159]
[0160] Among them, q0, q1, q2, q3 are real numbers, i, j, k are mutually orthogonal imaginary unit vectors, q0 is the real part of the quaternion, q1, q2, q3 are the imaginary parts of the quaternion, and the corresponding rotation matrix is:
[0161]
[0162] (2) In the matching point pair set C m The optimal solution of the rigid body change is calculated based on , and its objective function satisfies:
[0163]
[0164] Where R is an orthogonal matrix, then R T R=1, expand it to get:
[0165]
[0166] Among them, when When it is the maximum value, the objective function f(R) is the minimum. Expressed as quaternion:
[0167]
[0168] And convert it into matrix form:
[0169]
[0170] Among them, N4 is a 4-order matrix. When q T When N4q is at its maximum value, f(R) is at its maximum value. The eigenvalues obtained by solving the matrix N4 are λ1, λ2, λ3, and λ4. Note that λ1<λ2<λ3<λ4, and the eigenvector corresponding to the maximum eigenvalue λ4 is the unit quaternion. Matrix form. Solve for the unit quaternion After the four parameters are substituted into the quaternion expression, and the rotation matrix R is solved, the translation parameter T = [ΔXΔYΔZ] T It can be expressed as:
[0171]
[0172] Among them, [X i Y i Zi ] T is the coordinate of the center of gravity in the source point cloud coordinate system, [xy i z i ] T is the coordinate of the center of gravity in the target point cloud coordinate system.
[0173] Step 4.2: Based on the initial registration, a bidirectional k-dimensional tree is constructed to improve the ICP algorithm to improve the efficiency of the nearest point search. The specific steps are as follows:
[0174] (1) Construct a k-dimensional tree for the source point cloud P and the target point cloud Q respectively;
[0175] (2) Search for q in P i The nearest point p i ;
[0176] (3) If we search for p in Q i The closest point is q i , then (p i ,q i ) the closest point pair that forms the corresponding relationship;
[0177] (4) If we search for p in Q i The closest point is not q i , then find the next point p i+1 The closest point in P;
[0178] (5) Traverse all points in the point cloud Q.
[0179] (6) Calculate the transformation parameters R and T, and calculate the sum of the squares of the distances between the corresponding point pairs after the transformation:
[0180]
[0181] Among them, ||d k+1 -d k || represents the distance between two adjacent iterations. Set the threshold δ, when ||d k+1 -d k When ||<δ, the iteration ends and the optimal rigid body transformation parameters R and T are obtained; otherwise, the iteration calculation ||d is repeated. k+1 -d k ||, until ||d is satisfied k+1 -d k ||<δ or reaches the maximum number of iterations ω, select ||d k+1 -d k ||The smallest transformation matrices R and T are the optimal transformation parameters.
[0182] The bidirectional k-dimensional tree can determine the one-to-one correspondence of the nearest points. Although the search time is twice that of the k-dimensional tree, the one-to-one correspondence of the nearest point pairs greatly reduces the time of iterative calculation. Therefore, the search method of constructing a bidirectional k-dimensional tree is more suitable for point cloud data with large data volumes.
[0183] In order to prove the effectiveness of the point cloud registration method based on neighborhood normal vectors and curvature proposed in this embodiment, vehicle-mounted laser (MLS) point cloud data and ground laser (TLS) point cloud data from different periods are used as objects for verification, see Experiment 1 and Experiment 2. Due to the large number of collected data and the interference of the external environment, the original point cloud data is mixed with a large number of noise points, which affects the efficiency of the subsequent registration. Therefore, it is necessary to perform preliminary preprocessing on the original point cloud before registration. The preliminary preprocessing work mainly includes point cloud denoising and point cloud filtering, that is, the drift points, outliers, and redundant points in the original point cloud data are removed by combining the existing mature point cloud processing tools with visual interaction methods, and the ground points are removed by the point cloud filtering method based on the hierarchical grid. In addition, in order to quantify the registration results, the root mean square error of the Euclidean distance between the same-name point pairs in the overlapping area after registration is used as the evaluation index, θ RMSE Defined as:
[0184]
[0185] Among them, B represents the number of point pairs with the same name in the overlapping area.
[0186] Experiment 1: Vehicle-mounted laser point cloud registration at different times.
[0187] The initial position of the MLS point cloud is as follows Figure 5 As shown, the maximum deviation of the initial position of the point cloud is about 6.3m, the red one is the source point cloud, and the blue one is the target point cloud. The specific implementation method is as follows: First, the two-phase vehicle-mounted point cloud data is voxel downsampled with a sampling interval of 2.5cm; secondly, the feature points are extracted through the neighborhood feature parameters, with a neighborhood radius of 3mr and a constraint of 20 neighboring points; the number of source point cloud feature points is 1644, and the number of target point cloud feature points is 1740; then, the threshold α of the ratio of the bidirectional minimum to the sub-minimum Euclidean distance between FPFH feature vectors is 0.8, the threshold β of the number of correct matching point pairs is 0.3, and the number of correct matching point pairs is 460 pairs; finally, the correct matching point pairs are used as the initial values of the initial transformation parameters calculated by the unit quaternion for initial alignment, and a bidirectional k-dimensional tree is constructed to improve the ICP algorithm to achieve accurate alignment of the two-phase vehicle-mounted point cloud data. The alignment effect is shown as follows: Figure 6 shown.
[0188] from Figure 6It can be seen that the two-phase vehicle point cloud can still complete accurate registration under the condition of noise interference and data loss. In order to evaluate the performance of the algorithm of the present invention, the registration results of the algorithm of the present invention and the ICP algorithm, the ICP algorithm with k-dimensional tree (TICP), and the ICP algorithm that uses normal vector (NV) and curvature (NC) to extract feature points for registration are compared and analyzed. The results are shown in Figure 2. Figure 7 As shown in Table 1. Figure 7 It can be found that the ICP algorithm, NV-ICP algorithm and NC-ICP algorithm are all unable to accurately align the two-phase MLS point cloud data, while the present invention is superior to other algorithms in terms of alignment accuracy and efficiency, verifying its effectiveness in the presence of noise interference and data missing.
[0189] Table 1 Comparison of registration results of different algorithms for vehicle-mounted laser point cloud data at different times
[0190]
[0191] Experiment 2: Ground and vehicle-mounted laser point cloud registration.
[0192] TLS point cloud data and MLS point cloud data Figure 8 As shown, the red one is the TLS point cloud data as the source point cloud, and the blue one is the MLS point cloud data as the target point cloud. The specific implementation method is as follows: First, the TLS point cloud data and the MLS point cloud data are voxel downsampled, and the sampling interval is 2.5cm; secondly, the feature points are extracted through the neighborhood feature parameters, and the neighborhood radius is 5mr, the number of neighboring points is constrained to 20, the number of source point cloud feature points is 1726, and the number of target point cloud feature points is 2143; then, the threshold α of the ratio of the bidirectional minimum and sub-minimum Euclidean distance between FPFH feature vectors is 0.7, and the threshold β of the number of correct matching point pairs is 0.4, and the number of correct matching point pairs is 345 pairs; finally, the correct matching point pairs are used as the initial value of the unit element for initial alignment, and a bidirectional k-dimensional tree is constructed to improve the ICP algorithm to achieve accurate alignment of the two sets of point cloud data. The alignment effect is shown as follows. Fig. 9 shown.
[0193] from Fig. 9 It can be seen that the two sets of point cloud data can still be accurately registered in the case of low local overlap, large rotation angle and translation value. In order to evaluate the performance of the algorithm of the present invention, the registration results of the algorithm of the present invention and the comparison algorithm of Experiment 1 are also compared and analyzed. The results are shown in the figure. Fig.10 As shown in Table 2. Fig.10It can be found that the ICP algorithm, NV-ICP algorithm and NC-ICP algorithm are all unable to accurately align TLS point cloud data with MLS point cloud data, while the present invention is superior to other algorithms in terms of alignment accuracy and efficiency, verifying its robustness in cases with low local overlap rate, large north declination angle and translation values.
[0194] Table 2 Comparison of registration results of ground and vehicle laser point cloud data by different algorithms
[0195]
Claims
1. A point cloud registration method based on neighborhood normal vector and curvature, characterized in that: The following steps are involved: Step 1: Preprocess the source point cloud and target point cloud data; Step 2: According to the density between each point, the number of neighboring points is constrained for the preprocessed source point cloud and target point cloud; According to the mean inner product of the normal vectors between the sampling point and the neighboring points, the initial feature points are extracted from the preprocessed point cloud; the initial feature points are extracted again through the neighborhood average curvature to form feature points; the final feature point set is formed based on the constructed curvature feature parameters; Step 3: Perform FPFH feature description on the final feature point set, perform initial feature matching based on the ratio of the bidirectional minimum and second minimum Euclidean distances between feature vectors, and use the Euclidean distance constraint between point pairs to eliminate mismatched point pairs; Step 4: Use the correct matching point pairs as the unit quaternion to calculate the initial values of the initial transformation parameters, perform initial registration on the source point cloud and the target point cloud, and construct a bidirectional k-dimensional tree to improve the ICP algorithm to complete accurate registration.
2. The point cloud registration method based on neighborhood normal vector and curvature according to claim 1, characterized in that: The preprocessing of the source point cloud and the target point cloud data specifically includes: A bounding box with a length, width and height of l×w×h is established for the original point cloud P. The bounding box is divided into M cubic voxel grids by combining the point cloud density to estimate the voxel side length L. The center of gravity of all points in each voxel grid is calculated and used as the filtering point, thereby reducing the complexity of the point cloud while retaining most of the feature information of the point cloud.
3. The point cloud registration method based on neighborhood normal vector and curvature according to claim 1, characterized in that: The extracting of initial feature points from the preprocessed point cloud based on the mean inner product of the normal vectors between the sampling point and the neighboring points specifically includes: The curvature of the neighborhood surface is defined according to the density of neighboring points in neighborhoods of different radii; Take sampling point p i The neighboring points and sampling points p in the search space with different radii are the center of the circle. i The point with the closest Euclidean distance; For the preprocessed source point cloud P = {p i |p i ∈R 3 ,i=1,2,3,…,n} any point p i (x i ,y i ,z i ) and its A neighboring points in the neighborhood are fitted by the least squares local plane to obtain the plane equation F, which is as follows: For any point p in the preprocessed source point cloud P i (x i ,y i ,z i ) and its A neighboring points in the neighborhood construct the covariance matrix C: in, p i is the sampling point, is the sampling point p i The centroid of its A neighboring points; Solving the covariance matrix C to get the eigenvalues remember Minimum eigenvalue The corresponding eigenvector For p i The normal vector of point p i The mean of the inner product of the normal vectors with its neighboring points represents the angle characteristic f of the normal vector; Where f∈[0,1], n i For point p i The normal vector, n ij Represents the neighboring point p ij The normal vector of point p i ,satisfy: f(p i )=max[f(p i1 ),f(p i2 ),f(p i3 ),...,f(p ik )] Among them, f(p i ) is the sampling point p i The characteristic degree of point p i As feature points, f(p i1 ),f(p i2 ),f(p i3 ),...,f(p ik ) is point p i The feature degree of the k neighboring points is used to extract the source point cloud P and the target point cloud Q as the initial feature point set P t and Q t .
4. The point cloud registration method based on neighborhood normal vector and curvature according to claim 3, characterized in that: The method further comprises: performing secondary extraction of initial feature points through neighborhood average curvature to form feature points, For the initial feature point set P t For any point p, the surface is fitted by the least squares method to obtain the quadratic surface equation r(u,v) of the neighborhood at point p, and the normal vector n at point p is obtained. P , derive the first basic quantity E, F, G and the second basic quantity L, M, N, and thus calculate the Gaussian curvature K and the mean curvature H of the neighborhood at point p on the surface; Among them, r u 、r v The first-order partial derivative of the quadratic surface, r uu 、r uv 、r vv is the second-order partial derivative of the quadratic surface, E = r u ·r u ,F=r u ·r v ,G=r v ·r v , L = n P ·r uu ,M=n P ·r uv ,N=n P ·r vv ; Calculate the principal curvatures k1 and k2 at point p of the surface using the Gaussian curvature K and the mean curvature H; The mean curvature H of the neighborhood at point p is equal to the point curvature H i For comparison, if H i <H, remove the point; otherwise, if H i >H, retain points with curvature greater than the neighborhood average as feature point set P ω and Q ω .
5. The point cloud registration method based on neighborhood normal vector and curvature according to claim 4, characterized in that: The method of constructing the curvature feature parameters to form a final feature point set also includes: In order to remove the estimated neighborhood curvature resulting in the feature point set P ω and Q ω Non-feature points with similar curvature, for P ω , Q ω The point with the largest curvature change in the neighborhood is extracted as the feature point, point p i The curvature characteristic parameter is: Among them, k1(p i )、k2(p i ) represent the point p i Maximum principal curvature, minimum principal curvature; Set point p i The condition for whether it is a feature point is: in, For point p i Curvature characteristic parameters of neighboring points.
6. The point cloud registration method based on neighborhood normal vector and curvature according to claim 1, characterized in that: The method of performing FPFH feature description on the final feature point set, performing initial feature matching according to the ratio of the bidirectional minimum and second minimum Euclidean distances between feature vectors, and using the Euclidean distance constraint between point pairs to eliminate mismatched point pairs, also includes: Step 3.1: FPFH feature description; Step 3.2: Correspondence estimation; Step 3.3: Eliminate mismatched point pairs.
7. The point cloud registration method based on neighborhood normal vector and curvature according to claim 6, characterized in that: The FPFH feature description also includes: Re-determine each feature point p i The SPFH feature values of k neighboring points in its neighborhood are added with the distance weight to obtain the FPFH feature, which is recorded as the formula FPFH(p i ): Among them, w k Represents the distance weight, which is the feature point p i The Euclidean distance between the k neighbors in its neighborhood.
8. The point cloud registration method based on neighborhood normal vector and curvature according to claim 6, characterized in that: The correspondence estimation further includes: Calculate the feature point set P γ The FPFH vector of each point in the feature point set Q γ The Euclidean distance between the FPFH vectors of all points in ; P γ Each point in Find Q γ Middle distance The nearest feature point and the next closest feature point And calculate the ratio r d , the calculation method is: Set the threshold α∈(0,1), if r d (p i ,q j )<α, it is believed and match.
9. The point cloud registration method based on neighborhood normal vector and curvature according to claim 6, characterized in that: The step of eliminating mismatched point pairs also includes: Select the initial matching point pair set C γ Any two pairs of matching points in and Set the Euclidean distance constraint between point pairs as: Among them, ε=0.02, which satisfies the above formula, The matching point pairs that meet the distance constraint are Calculate C γ Zhongyu The number of matching point pairs that meet the above conditions is k γ , and follow k γ The value will be C γ The point pairs in the array are arranged from large to small, and the threshold β is set. The number of correctly matched point pairs n m Should meet: m =β·N m , β∈(0,1), N m is the number of initial matching point pairs.
10. The point cloud registration method based on neighborhood normal vector and curvature according to claim 1, characterized in that: The method uses the correct matching point pair as the initial value of the unit quaternion to calculate the initial transformation parameter, performs initial registration on the source point cloud and the target point cloud, and constructs a bidirectional k-dimensional tree to improve the ICP algorithm to complete the precise registration, and also includes: Step 4.1: Initial registration; Step 4.2: Accurate registration; The initial registration also includes that the expression of the quaternion is: Among them, q0, q1, q2, q3 are real numbers, i, j, k are mutually orthogonal imaginary unit vectors, q0 is the real part of the quaternion, q1, q2, q3 are the imaginary parts of the quaternion, and the corresponding rotation matrix is: In the matching point set C m The optimal solution of the rigid body change is calculated based on , and its objective function satisfies: Where R is an orthogonal matrix, then R T When R=1, When it is the maximum value, the objective function f(R) is the minimum. Represent it as a quaternion and convert it into a matrix form: Among them, N4 is a 4-order matrix. When q T When N4q is at its maximum value, f(R) is maximum; The eigenvalues obtained by solving the matrix N4 are λ1, λ2, λ3, and λ4. Note that λ1<λ2<λ3<λ4, then the eigenvector corresponding to the maximum eigenvalue λ4 is the unit quaternion Matrix form; solve for the unit quaternion After the four parameters are substituted into the quaternion expression, and the rotation matrix R is solved, the translation parameter T = [ΔXΔYΔZ] T It can be expressed as: Among them, [X i Y i Z i ] T is the coordinate of the center of gravity in the source point cloud coordinate system, [xy i z i ] T is the coordinate of the center of gravity in the target point cloud coordinate system; The precise registration also includes constructing a bidirectional k-dimensional tree to search for the nearest point pair (p i ,q i ), calculate the transformation parameters R, T, and calculate the sum of squared distances between corresponding point pairs after the transformation: Among them, ||d k+1 -d k || represents the distance between two adjacent iterations; Set the threshold δ, when ||d k+1 -d k When ||<δ, the iteration ends and the optimal rigid body transformation parameters R and T are obtained; otherwise, the iteration calculation ||d is repeated. k+1 -d k ||, until ||d is satisfied k+1 -d k ||<δ or reaches the maximum number of iterations ω, select ||d k+1 -d k ||The smallest transformation matrices R and T are the optimal transformation parameters.
Citation Information
Patent Citations
Point cloud registration method based on key point and neighborhood point pair feature description, electronic equipment and storage medium
CN116416287A
Cited By
AR-based intelligent integrated production line special circuit control method and system
CN119229064A
Strip splicing robust processing method and system for cluster multiple beams
CN119477685A
Point cloud registration method, system, equipment and medium
CN119991759A
Efficient point cloud positioning initialization method
CN120355887A
ICP (Inductively Coupled Plasma) matching method and system based on concave-convex characteristics of mine cave
CN120375020A
